Line data Source code
1 : /*
2 : * Copyright (c) 2016, Alliance for Open Media. All rights reserved
3 : *
4 : * This source code is subject to the terms of the BSD 2 Clause License and
5 : * the Alliance for Open Media Patent License 1.0. If the BSD 2 Clause License
6 : * was not distributed with this source code in the LICENSE file, you can
7 : * obtain it at www.aomedia.org/license/software. If the Alliance for Open
8 : * Media Patent License 1.0 was not distributed with this source code in the
9 : * PATENTS file, you can obtain it at www.aomedia.org/license/patent.
10 : */
11 : #include <memory.h>
12 : #include <math.h>
13 : #include <time.h>
14 : #include <stdio.h>
15 : #include <stdlib.h>
16 : #include <assert.h>
17 :
18 : #include "ransac.h"
19 : #include "mathutils.h"
20 : #include "random.h"
21 :
22 : #define MAX_MINPTS 4
23 : #define MAX_DEGENERATE_ITER 10
24 : #define MINPTS_MULTIPLIER 5
25 :
26 : #define INLIER_THRESHOLD 1.25
27 : #define MIN_TRIALS 20
28 :
29 : ////////////////////////////////////////////////////////////////////////////////
30 : // ransac
31 : typedef int (*IsDegenerateFunc)(double *p);
32 : typedef void (*NormalizeFunc)(double *p, int np, double *T);
33 : typedef void (*DenormalizeFunc)(double *params, double *T1, double *T2);
34 : typedef int (*FindTransformationFunc)(int points, double *points1,
35 : double *points2, double *params);
36 : typedef void (*ProjectPointsDoubleFunc)(double *mat, double *points,
37 : double *proj, int n, int stride_points,
38 : int stride_proj);
39 :
40 0 : static void project_points_double_translation(double *mat, double *points,
41 : double *proj, int n,
42 : int stride_points,
43 : int stride_proj) {
44 : int i;
45 0 : for (i = 0; i < n; ++i) {
46 0 : const double x = *(points++), y = *(points++);
47 0 : *(proj++) = x + mat[0];
48 0 : *(proj++) = y + mat[1];
49 0 : points += stride_points - 2;
50 0 : proj += stride_proj - 2;
51 : }
52 0 : }
53 :
54 2260 : static void project_points_double_rotzoom(double *mat, double *points,
55 : double *proj, int n,
56 : int stride_points, int stride_proj) {
57 : int i;
58 5660440 : for (i = 0; i < n; ++i) {
59 5658180 : const double x = *(points++), y = *(points++);
60 5658180 : *(proj++) = mat[2] * x + mat[3] * y + mat[0];
61 5658180 : *(proj++) = -mat[3] * x + mat[2] * y + mat[1];
62 5658180 : points += stride_points - 2;
63 5658180 : proj += stride_proj - 2;
64 : }
65 2260 : }
66 :
67 0 : static void project_points_double_affine(double *mat, double *points,
68 : double *proj, int n, int stride_points,
69 : int stride_proj) {
70 : int i;
71 0 : for (i = 0; i < n; ++i) {
72 0 : const double x = *(points++), y = *(points++);
73 0 : *(proj++) = mat[2] * x + mat[3] * y + mat[0];
74 0 : *(proj++) = mat[4] * x + mat[5] * y + mat[1];
75 0 : points += stride_points - 2;
76 0 : proj += stride_proj - 2;
77 : }
78 0 : }
79 :
80 8974 : static void normalize_homography(double *pts, int n, double *T) {
81 8974 : double *p = pts;
82 8974 : double mean[2] = { 0, 0 };
83 8974 : double msqe = 0;
84 : double scale;
85 : int i;
86 :
87 8974 : assert(n > 0);
88 9452390 : for (i = 0; i < n; ++i, p += 2) {
89 9443410 : mean[0] += p[0];
90 9443410 : mean[1] += p[1];
91 : }
92 8974 : mean[0] /= n;
93 8974 : mean[1] /= n;
94 9452390 : for (p = pts, i = 0; i < n; ++i, p += 2) {
95 9443410 : p[0] -= mean[0];
96 9443410 : p[1] -= mean[1];
97 9443410 : msqe += sqrt(p[0] * p[0] + p[1] * p[1]);
98 : }
99 8974 : msqe /= n;
100 8974 : scale = (msqe == 0 ? 1.0 : sqrt(2) / msqe);
101 8974 : T[0] = scale;
102 8974 : T[1] = 0;
103 8974 : T[2] = -scale * mean[0];
104 8974 : T[3] = 0;
105 8974 : T[4] = scale;
106 8974 : T[5] = -scale * mean[1];
107 8974 : T[6] = 0;
108 8974 : T[7] = 0;
109 8974 : T[8] = 1;
110 9452390 : for (p = pts, i = 0; i < n; ++i, p += 2) {
111 9443410 : p[0] *= scale;
112 9443410 : p[1] *= scale;
113 : }
114 8974 : }
115 :
116 4487 : static void invnormalize_mat(double *T, double *iT) {
117 4487 : double is = 1.0 / T[0];
118 4487 : double m0 = -T[2] * is;
119 4487 : double m1 = -T[5] * is;
120 4487 : iT[0] = is;
121 4487 : iT[1] = 0;
122 4487 : iT[2] = m0;
123 4487 : iT[3] = 0;
124 4487 : iT[4] = is;
125 4487 : iT[5] = m1;
126 4487 : iT[6] = 0;
127 4487 : iT[7] = 0;
128 4487 : iT[8] = 1;
129 4487 : }
130 :
131 4487 : static void denormalize_homography(double *params, double *T1, double *T2) {
132 : double iT2[9];
133 : double params2[9];
134 4487 : invnormalize_mat(T2, iT2);
135 4487 : multiply_mat(params, T1, params2, 3, 3, 3);
136 4487 : multiply_mat(iT2, params2, params, 3, 3, 3);
137 4487 : }
138 :
139 0 : static void denormalize_affine_reorder(double *params, double *T1, double *T2) {
140 : double params_denorm[MAX_PARAMDIM];
141 0 : params_denorm[0] = params[0];
142 0 : params_denorm[1] = params[1];
143 0 : params_denorm[2] = params[4];
144 0 : params_denorm[3] = params[2];
145 0 : params_denorm[4] = params[3];
146 0 : params_denorm[5] = params[5];
147 0 : params_denorm[6] = params_denorm[7] = 0;
148 0 : params_denorm[8] = 1;
149 0 : denormalize_homography(params_denorm, T1, T2);
150 0 : params[0] = params_denorm[2];
151 0 : params[1] = params_denorm[5];
152 0 : params[2] = params_denorm[0];
153 0 : params[3] = params_denorm[1];
154 0 : params[4] = params_denorm[3];
155 0 : params[5] = params_denorm[4];
156 0 : params[6] = params[7] = 0;
157 0 : }
158 :
159 4487 : static void denormalize_rotzoom_reorder(double *params, double *T1,
160 : double *T2) {
161 : double params_denorm[MAX_PARAMDIM];
162 4487 : params_denorm[0] = params[0];
163 4487 : params_denorm[1] = params[1];
164 4487 : params_denorm[2] = params[2];
165 4487 : params_denorm[3] = -params[1];
166 4487 : params_denorm[4] = params[0];
167 4487 : params_denorm[5] = params[3];
168 4487 : params_denorm[6] = params_denorm[7] = 0;
169 4487 : params_denorm[8] = 1;
170 4487 : denormalize_homography(params_denorm, T1, T2);
171 4487 : params[0] = params_denorm[2];
172 4487 : params[1] = params_denorm[5];
173 4487 : params[2] = params_denorm[0];
174 4487 : params[3] = params_denorm[1];
175 4487 : params[4] = -params[3];
176 4487 : params[5] = params[2];
177 4487 : params[6] = params[7] = 0;
178 4487 : }
179 :
180 0 : static void denormalize_translation_reorder(double *params, double *T1,
181 : double *T2) {
182 : double params_denorm[MAX_PARAMDIM];
183 0 : params_denorm[0] = 1;
184 0 : params_denorm[1] = 0;
185 0 : params_denorm[2] = params[0];
186 0 : params_denorm[3] = 0;
187 0 : params_denorm[4] = 1;
188 0 : params_denorm[5] = params[1];
189 0 : params_denorm[6] = params_denorm[7] = 0;
190 0 : params_denorm[8] = 1;
191 0 : denormalize_homography(params_denorm, T1, T2);
192 0 : params[0] = params_denorm[2];
193 0 : params[1] = params_denorm[5];
194 0 : params[2] = params[5] = 1;
195 0 : params[3] = params[4] = 0;
196 0 : params[6] = params[7] = 0;
197 0 : }
198 :
199 0 : static int find_translation(int np, double *pts1, double *pts2, double *mat) {
200 : int i;
201 : double sx, sy, dx, dy;
202 : double sumx, sumy;
203 :
204 : double T1[9], T2[9];
205 0 : normalize_homography(pts1, np, T1);
206 0 : normalize_homography(pts2, np, T2);
207 :
208 0 : sumx = 0;
209 0 : sumy = 0;
210 0 : for (i = 0; i < np; ++i) {
211 0 : dx = *(pts2++);
212 0 : dy = *(pts2++);
213 0 : sx = *(pts1++);
214 0 : sy = *(pts1++);
215 :
216 0 : sumx += dx - sx;
217 0 : sumy += dy - sy;
218 : }
219 0 : mat[0] = sumx / np;
220 0 : mat[1] = sumy / np;
221 0 : denormalize_translation_reorder(mat, T1, T2);
222 0 : return 0;
223 : }
224 :
225 4487 : static int find_rotzoom(int np, double *pts1, double *pts2, double *mat) {
226 4487 : const int np2 = np * 2;
227 4487 : double *a = (double *)malloc(sizeof(*a) * (np2 * 5 + 20));
228 4487 : double *b = a + np2 * 4;
229 4487 : double *temp = b + np2;
230 : int i;
231 : double sx, sy, dx, dy;
232 :
233 : double T1[9], T2[9];
234 4487 : normalize_homography(pts1, np, T1);
235 4487 : normalize_homography(pts2, np, T2);
236 :
237 4726190 : for (i = 0; i < np; ++i) {
238 4721710 : dx = *(pts2++);
239 4721710 : dy = *(pts2++);
240 4721710 : sx = *(pts1++);
241 4721710 : sy = *(pts1++);
242 :
243 4721710 : a[i * 2 * 4 + 0] = sx;
244 4721710 : a[i * 2 * 4 + 1] = sy;
245 4721710 : a[i * 2 * 4 + 2] = 1;
246 4721710 : a[i * 2 * 4 + 3] = 0;
247 4721710 : a[(i * 2 + 1) * 4 + 0] = sy;
248 4721710 : a[(i * 2 + 1) * 4 + 1] = -sx;
249 4721710 : a[(i * 2 + 1) * 4 + 2] = 0;
250 4721710 : a[(i * 2 + 1) * 4 + 3] = 1;
251 :
252 4721710 : b[2 * i] = dx;
253 4721710 : b[2 * i + 1] = dy;
254 : }
255 4487 : if (!least_squares(4, a, np2, 4, b, temp, mat)) {
256 0 : free(a);
257 0 : return 1;
258 : }
259 4487 : denormalize_rotzoom_reorder(mat, T1, T2);
260 4487 : free(a);
261 4487 : return 0;
262 : }
263 :
264 0 : static int find_affine(int np, double *pts1, double *pts2, double *mat) {
265 0 : assert(np > 0);
266 0 : const int np2 = np * 2;
267 0 : double *a = (double *)malloc(sizeof(*a) * (np2 * 7 + 42));
268 0 : if (a == NULL) return 1;
269 0 : double *b = a + np2 * 6;
270 0 : double *temp = b + np2;
271 : int i;
272 : double sx, sy, dx, dy;
273 :
274 : double T1[9], T2[9];
275 0 : normalize_homography(pts1, np, T1);
276 0 : normalize_homography(pts2, np, T2);
277 :
278 0 : for (i = 0; i < np; ++i) {
279 0 : dx = *(pts2++);
280 0 : dy = *(pts2++);
281 0 : sx = *(pts1++);
282 0 : sy = *(pts1++);
283 :
284 0 : a[i * 2 * 6 + 0] = sx;
285 0 : a[i * 2 * 6 + 1] = sy;
286 0 : a[i * 2 * 6 + 2] = 0;
287 0 : a[i * 2 * 6 + 3] = 0;
288 0 : a[i * 2 * 6 + 4] = 1;
289 0 : a[i * 2 * 6 + 5] = 0;
290 0 : a[(i * 2 + 1) * 6 + 0] = 0;
291 0 : a[(i * 2 + 1) * 6 + 1] = 0;
292 0 : a[(i * 2 + 1) * 6 + 2] = sx;
293 0 : a[(i * 2 + 1) * 6 + 3] = sy;
294 0 : a[(i * 2 + 1) * 6 + 4] = 0;
295 0 : a[(i * 2 + 1) * 6 + 5] = 1;
296 :
297 0 : b[2 * i] = dx;
298 0 : b[2 * i + 1] = dy;
299 : }
300 0 : if (!least_squares(6, a, np2, 6, b, temp, mat)) {
301 0 : free(a);
302 0 : return 1;
303 : }
304 0 : denormalize_affine_reorder(mat, T1, T2);
305 0 : free(a);
306 0 : return 0;
307 : }
308 :
309 2260 : static int get_rand_indices(int npoints, int minpts, int *indices,
310 : unsigned int *seed) {
311 : int i, j;
312 2260 : int ptr = lcg_rand16(seed) % npoints;
313 2260 : if (minpts > npoints) return 0;
314 2260 : indices[0] = ptr;
315 2260 : ptr = (ptr == npoints - 1 ? 0 : ptr + 1);
316 2260 : i = 1;
317 6780 : while (i < minpts) {
318 4520 : int index = lcg_rand16(seed) % npoints;
319 5605140 : while (index) {
320 5600620 : ptr = (ptr == npoints - 1 ? 0 : ptr + 1);
321 13985700 : for (j = 0; j < i; ++j) {
322 8386190 : if (indices[j] == ptr) break;
323 : }
324 5600620 : if (j == i) index--;
325 : }
326 4520 : indices[i++] = ptr;
327 : }
328 2260 : return 1;
329 : }
330 :
331 : typedef struct {
332 : int num_inliers;
333 : double variance;
334 : int *inlier_indices;
335 : } RANSAC_MOTION;
336 :
337 : // Return -1 if 'a' is a better motion, 1 if 'b' is better, 0 otherwise.
338 263492 : static int compare_motions(const void *arg_a, const void *arg_b) {
339 263492 : const RANSAC_MOTION *motion_a = (RANSAC_MOTION *)arg_a;
340 263492 : const RANSAC_MOTION *motion_b = (RANSAC_MOTION *)arg_b;
341 :
342 263492 : if (motion_a->num_inliers > motion_b->num_inliers) return -1;
343 246307 : if (motion_a->num_inliers < motion_b->num_inliers) return 1;
344 224140 : if (motion_a->variance < motion_b->variance) return -1;
345 224020 : if (motion_a->variance > motion_b->variance) return 1;
346 223889 : return 0;
347 : }
348 :
349 225331 : static int is_better_motion(const RANSAC_MOTION *motion_a,
350 : const RANSAC_MOTION *motion_b) {
351 225331 : return compare_motions(motion_a, motion_b) < 0;
352 : }
353 :
354 8974 : static void copy_points_at_indices(double *dest, const double *src,
355 : const int *indices, int num_points) {
356 9452390 : for (int i = 0; i < num_points; ++i) {
357 9443410 : const int index = indices[i];
358 9443410 : dest[i * 2] = src[index * 2];
359 9443410 : dest[i * 2 + 1] = src[index * 2 + 1];
360 : }
361 8974 : }
362 :
363 : static const double kInfiniteVariance = 1e12;
364 :
365 13673 : static void clear_motion(RANSAC_MOTION *motion, int num_points) {
366 13673 : motion->num_inliers = 0;
367 13673 : motion->variance = kInfiniteVariance;
368 13673 : memset(motion->inlier_indices, 0,
369 : sizeof(*motion->inlier_indices * num_points));
370 13673 : }
371 :
372 113 : static int ransac(const int *matched_points, int npoints,
373 : int *num_inliers_by_motion, MotionModel *params_by_motion,
374 : int num_desired_motions, int minpts,
375 : IsDegenerateFunc is_degenerate,
376 : FindTransformationFunc find_transformation,
377 : ProjectPointsDoubleFunc projectpoints) {
378 113 : int trial_count = 0;
379 113 : int i = 0;
380 113 : int ret_val = 0;
381 :
382 113 : unsigned int seed = (unsigned int)npoints;
383 :
384 113 : int indices[MAX_MINPTS] = { 0 };
385 :
386 : double *points1, *points2;
387 : double *corners1, *corners2;
388 : double *image1_coord;
389 :
390 : // Store information for the num_desired_motions best transformations found
391 : // and the worst motion among them, as well as the motion currently under
392 : // consideration.
393 113 : RANSAC_MOTION *motions, *worst_kept_motion = NULL;
394 : RANSAC_MOTION current_motion;
395 :
396 : // Store the parameters and the indices of the inlier points for the motion
397 : // currently under consideration.
398 : double params_this_motion[MAX_PARAMDIM];
399 :
400 : double *cnp1, *cnp2;
401 :
402 11413 : for (i = 0; i < num_desired_motions; ++i) {
403 11300 : num_inliers_by_motion[i] = 0;
404 : }
405 113 : if (npoints < minpts * MINPTS_MULTIPLIER || npoints == 0) {
406 0 : return 1;
407 : }
408 :
409 113 : points1 = (double *)malloc(sizeof(*points1) * npoints * 2);
410 113 : points2 = (double *)malloc(sizeof(*points2) * npoints * 2);
411 113 : corners1 = (double *)malloc(sizeof(*corners1) * npoints * 2);
412 113 : corners2 = (double *)malloc(sizeof(*corners2) * npoints * 2);
413 113 : image1_coord = (double *)malloc(sizeof(*image1_coord) * npoints * 2);
414 :
415 : motions =
416 113 : (RANSAC_MOTION *)malloc(sizeof(RANSAC_MOTION) * num_desired_motions);
417 11413 : for (i = 0; i < num_desired_motions; ++i) {
418 11300 : motions[i].inlier_indices =
419 11300 : (int *)malloc(sizeof(*motions->inlier_indices) * npoints);
420 11300 : clear_motion(motions + i, npoints);
421 : }
422 113 : current_motion.inlier_indices =
423 113 : (int *)malloc(sizeof(*current_motion.inlier_indices) * npoints);
424 113 : clear_motion(¤t_motion, npoints);
425 :
426 113 : worst_kept_motion = motions;
427 :
428 113 : if (!(points1 && points2 && corners1 && corners2 && image1_coord && motions &&
429 113 : current_motion.inlier_indices)) {
430 0 : ret_val = 1;
431 0 : goto finish_ransac;
432 : }
433 :
434 113 : cnp1 = corners1;
435 113 : cnp2 = corners2;
436 283022 : for (i = 0; i < npoints; ++i) {
437 282909 : *(cnp1++) = *(matched_points++);
438 282909 : *(cnp1++) = *(matched_points++);
439 282909 : *(cnp2++) = *(matched_points++);
440 282909 : *(cnp2++) = *(matched_points++);
441 : }
442 :
443 2373 : while (MIN_TRIALS > trial_count) {
444 2260 : double sum_distance = 0.0;
445 2260 : double sum_distance_squared = 0.0;
446 :
447 2260 : clear_motion(¤t_motion, npoints);
448 :
449 2260 : int degenerate = 1;
450 2260 : int num_degenerate_iter = 0;
451 :
452 4520 : while (degenerate) {
453 2260 : num_degenerate_iter++;
454 2260 : if (!get_rand_indices(npoints, minpts, indices, &seed)) {
455 0 : ret_val = 1;
456 0 : goto finish_ransac;
457 : }
458 :
459 2260 : copy_points_at_indices(points1, corners1, indices, minpts);
460 2260 : copy_points_at_indices(points2, corners2, indices, minpts);
461 :
462 2260 : degenerate = is_degenerate(points1);
463 2260 : if (num_degenerate_iter > MAX_DEGENERATE_ITER) {
464 0 : ret_val = 1;
465 0 : goto finish_ransac;
466 : }
467 : }
468 :
469 2260 : if (find_transformation(minpts, points1, points2, params_this_motion)) {
470 0 : trial_count++;
471 0 : continue;
472 : }
473 :
474 2260 : projectpoints(params_this_motion, corners1, image1_coord, npoints, 2, 2);
475 :
476 5660440 : for (i = 0; i < npoints; ++i) {
477 5658180 : double dx = image1_coord[i * 2] - corners2[i * 2];
478 5658180 : double dy = image1_coord[i * 2 + 1] - corners2[i * 2 + 1];
479 5658180 : double distance = sqrt(dx * dx + dy * dy);
480 :
481 5658180 : if (distance < INLIER_THRESHOLD) {
482 4714940 : current_motion.inlier_indices[current_motion.num_inliers++] = i;
483 4714940 : sum_distance += distance;
484 4714940 : sum_distance_squared += distance * distance;
485 : }
486 : }
487 :
488 2260 : if (current_motion.num_inliers >= worst_kept_motion->num_inliers &&
489 2260 : current_motion.num_inliers > 1) {
490 : double mean_distance;
491 2231 : mean_distance = sum_distance / ((double)current_motion.num_inliers);
492 2231 : current_motion.variance =
493 2231 : sum_distance_squared / ((double)current_motion.num_inliers - 1.0) -
494 2231 : mean_distance * mean_distance * ((double)current_motion.num_inliers) /
495 2231 : ((double)current_motion.num_inliers - 1.0);
496 2231 : if (is_better_motion(¤t_motion, worst_kept_motion)) {
497 : // This motion is better than the worst currently kept motion. Remember
498 : // the inlier points and variance. The parameters for each kept motion
499 : // will be recomputed later using only the inliers.
500 2231 : worst_kept_motion->num_inliers = current_motion.num_inliers;
501 2231 : worst_kept_motion->variance = current_motion.variance;
502 2231 : memcpy(worst_kept_motion->inlier_indices, current_motion.inlier_indices,
503 : sizeof(*current_motion.inlier_indices) * npoints);
504 2231 : assert(npoints > 0);
505 : // Determine the new worst kept motion and its num_inliers and variance.
506 225331 : for (i = 0; i < num_desired_motions; ++i) {
507 223100 : if (is_better_motion(worst_kept_motion, &motions[i])) {
508 6051 : worst_kept_motion = &motions[i];
509 : }
510 : }
511 : }
512 : }
513 2260 : trial_count++;
514 : }
515 :
516 : // Sort the motions, best first.
517 113 : qsort(motions, num_desired_motions, sizeof(RANSAC_MOTION), compare_motions);
518 :
519 : // Recompute the motions using only the inliers.
520 11413 : for (i = 0; i < num_desired_motions; ++i) {
521 11300 : if (motions[i].num_inliers >= minpts) {
522 2227 : copy_points_at_indices(points1, corners1, motions[i].inlier_indices,
523 2227 : motions[i].num_inliers);
524 2227 : copy_points_at_indices(points2, corners2, motions[i].inlier_indices,
525 2227 : motions[i].num_inliers);
526 :
527 2227 : find_transformation(motions[i].num_inliers, points1, points2,
528 2227 : params_by_motion[i].params);
529 :
530 2227 : params_by_motion[i].num_inliers = motions[i].num_inliers;
531 2227 : memcpy(params_by_motion[i].inliers, motions[i].inlier_indices,
532 : sizeof(*motions[i].inlier_indices) * npoints);
533 : }
534 11300 : num_inliers_by_motion[i] = motions[i].num_inliers;
535 : }
536 :
537 113 : finish_ransac:
538 113 : free(points1);
539 113 : free(points2);
540 113 : free(corners1);
541 113 : free(corners2);
542 113 : free(image1_coord);
543 113 : free(current_motion.inlier_indices);
544 11413 : for (i = 0; i < num_desired_motions; ++i) {
545 11300 : free(motions[i].inlier_indices);
546 : }
547 113 : free(motions);
548 :
549 113 : return ret_val;
550 : }
551 :
552 0 : static int ransac_double_prec(const double *matched_points, int npoints,
553 : int *num_inliers_by_motion,
554 : MotionModel *params_by_motion,
555 : int num_desired_motions, int minpts,
556 : IsDegenerateFunc is_degenerate,
557 : FindTransformationFunc find_transformation,
558 : ProjectPointsDoubleFunc projectpoints) {
559 0 : int trial_count = 0;
560 0 : int i = 0;
561 0 : int ret_val = 0;
562 :
563 0 : unsigned int seed = (unsigned int)npoints;
564 :
565 0 : int indices[MAX_MINPTS] = { 0 };
566 :
567 : double *points1, *points2;
568 : double *corners1, *corners2;
569 : double *image1_coord;
570 :
571 : // Store information for the num_desired_motions best transformations found
572 : // and the worst motion among them, as well as the motion currently under
573 : // consideration.
574 0 : RANSAC_MOTION *motions, *worst_kept_motion = NULL;
575 : RANSAC_MOTION current_motion;
576 :
577 : // Store the parameters and the indices of the inlier points for the motion
578 : // currently under consideration.
579 : double params_this_motion[MAX_PARAMDIM];
580 :
581 : double *cnp1, *cnp2;
582 :
583 0 : for (i = 0; i < num_desired_motions; ++i) {
584 0 : num_inliers_by_motion[i] = 0;
585 : }
586 0 : if (npoints < minpts * MINPTS_MULTIPLIER || npoints == 0) {
587 0 : return 1;
588 : }
589 :
590 0 : points1 = (double *)malloc(sizeof(*points1) * npoints * 2);
591 0 : points2 = (double *)malloc(sizeof(*points2) * npoints * 2);
592 0 : corners1 = (double *)malloc(sizeof(*corners1) * npoints * 2);
593 0 : corners2 = (double *)malloc(sizeof(*corners2) * npoints * 2);
594 0 : image1_coord = (double *)malloc(sizeof(*image1_coord) * npoints * 2);
595 :
596 : motions =
597 0 : (RANSAC_MOTION *)malloc(sizeof(RANSAC_MOTION) * num_desired_motions);
598 0 : for (i = 0; i < num_desired_motions; ++i) {
599 0 : motions[i].inlier_indices =
600 0 : (int *)malloc(sizeof(*motions->inlier_indices) * npoints);
601 0 : clear_motion(motions + i, npoints);
602 : }
603 0 : current_motion.inlier_indices =
604 0 : (int *)malloc(sizeof(*current_motion.inlier_indices) * npoints);
605 0 : clear_motion(¤t_motion, npoints);
606 :
607 0 : worst_kept_motion = motions;
608 :
609 0 : if (!(points1 && points2 && corners1 && corners2 && image1_coord && motions &&
610 0 : current_motion.inlier_indices)) {
611 0 : ret_val = 1;
612 0 : goto finish_ransac;
613 : }
614 :
615 0 : cnp1 = corners1;
616 0 : cnp2 = corners2;
617 0 : for (i = 0; i < npoints; ++i) {
618 0 : *(cnp1++) = *(matched_points++);
619 0 : *(cnp1++) = *(matched_points++);
620 0 : *(cnp2++) = *(matched_points++);
621 0 : *(cnp2++) = *(matched_points++);
622 : }
623 :
624 0 : while (MIN_TRIALS > trial_count) {
625 0 : double sum_distance = 0.0;
626 0 : double sum_distance_squared = 0.0;
627 :
628 0 : clear_motion(¤t_motion, npoints);
629 :
630 0 : int degenerate = 1;
631 0 : int num_degenerate_iter = 0;
632 :
633 0 : while (degenerate) {
634 0 : num_degenerate_iter++;
635 0 : if (!get_rand_indices(npoints, minpts, indices, &seed)) {
636 0 : ret_val = 1;
637 0 : goto finish_ransac;
638 : }
639 :
640 0 : copy_points_at_indices(points1, corners1, indices, minpts);
641 0 : copy_points_at_indices(points2, corners2, indices, minpts);
642 :
643 0 : degenerate = is_degenerate(points1);
644 0 : if (num_degenerate_iter > MAX_DEGENERATE_ITER) {
645 0 : ret_val = 1;
646 0 : goto finish_ransac;
647 : }
648 : }
649 :
650 0 : if (find_transformation(minpts, points1, points2, params_this_motion)) {
651 0 : trial_count++;
652 0 : continue;
653 : }
654 :
655 0 : projectpoints(params_this_motion, corners1, image1_coord, npoints, 2, 2);
656 :
657 0 : for (i = 0; i < npoints; ++i) {
658 0 : double dx = image1_coord[i * 2] - corners2[i * 2];
659 0 : double dy = image1_coord[i * 2 + 1] - corners2[i * 2 + 1];
660 0 : double distance = sqrt(dx * dx + dy * dy);
661 :
662 0 : if (distance < INLIER_THRESHOLD) {
663 0 : current_motion.inlier_indices[current_motion.num_inliers++] = i;
664 0 : sum_distance += distance;
665 0 : sum_distance_squared += distance * distance;
666 : }
667 : }
668 :
669 0 : if (current_motion.num_inliers >= worst_kept_motion->num_inliers &&
670 0 : current_motion.num_inliers > 1) {
671 : double mean_distance;
672 0 : mean_distance = sum_distance / ((double)current_motion.num_inliers);
673 0 : current_motion.variance =
674 0 : sum_distance_squared / ((double)current_motion.num_inliers - 1.0) -
675 0 : mean_distance * mean_distance * ((double)current_motion.num_inliers) /
676 0 : ((double)current_motion.num_inliers - 1.0);
677 0 : if (is_better_motion(¤t_motion, worst_kept_motion)) {
678 : // This motion is better than the worst currently kept motion. Remember
679 : // the inlier points and variance. The parameters for each kept motion
680 : // will be recomputed later using only the inliers.
681 0 : worst_kept_motion->num_inliers = current_motion.num_inliers;
682 0 : worst_kept_motion->variance = current_motion.variance;
683 0 : memcpy(worst_kept_motion->inlier_indices, current_motion.inlier_indices,
684 : sizeof(*current_motion.inlier_indices) * npoints);
685 0 : assert(npoints > 0);
686 : // Determine the new worst kept motion and its num_inliers and variance.
687 0 : for (i = 0; i < num_desired_motions; ++i) {
688 0 : if (is_better_motion(worst_kept_motion, &motions[i])) {
689 0 : worst_kept_motion = &motions[i];
690 : }
691 : }
692 : }
693 : }
694 0 : trial_count++;
695 : }
696 :
697 : // Sort the motions, best first.
698 0 : qsort(motions, num_desired_motions, sizeof(RANSAC_MOTION), compare_motions);
699 :
700 : // Recompute the motions using only the inliers.
701 0 : for (i = 0; i < num_desired_motions; ++i) {
702 0 : if (motions[i].num_inliers >= minpts) {
703 0 : copy_points_at_indices(points1, corners1, motions[i].inlier_indices,
704 0 : motions[i].num_inliers);
705 0 : copy_points_at_indices(points2, corners2, motions[i].inlier_indices,
706 0 : motions[i].num_inliers);
707 :
708 0 : find_transformation(motions[i].num_inliers, points1, points2,
709 0 : params_by_motion[i].params);
710 0 : memcpy(params_by_motion[i].inliers, motions[i].inlier_indices,
711 : sizeof(*motions[i].inlier_indices) * npoints);
712 : }
713 0 : num_inliers_by_motion[i] = motions[i].num_inliers;
714 : }
715 :
716 0 : finish_ransac:
717 0 : free(points1);
718 0 : free(points2);
719 0 : free(corners1);
720 0 : free(corners2);
721 0 : free(image1_coord);
722 0 : free(current_motion.inlier_indices);
723 0 : for (i = 0; i < num_desired_motions; ++i) {
724 0 : free(motions[i].inlier_indices);
725 : }
726 0 : free(motions);
727 :
728 0 : return ret_val;
729 : }
730 :
731 2260 : static int is_collinear3(double *p1, double *p2, double *p3) {
732 : static const double collinear_eps = 1e-3;
733 2260 : const double v =
734 2260 : (p2[0] - p1[0]) * (p3[1] - p1[1]) - (p2[1] - p1[1]) * (p3[0] - p1[0]);
735 2260 : return fabs(v) < collinear_eps;
736 : }
737 :
738 0 : static int is_degenerate_translation(double *p) {
739 0 : return (p[0] - p[2]) * (p[0] - p[2]) + (p[1] - p[3]) * (p[1] - p[3]) <= 2;
740 : }
741 :
742 2260 : static int is_degenerate_affine(double *p) {
743 2260 : return is_collinear3(p, p + 2, p + 4);
744 : }
745 :
746 0 : static int ransac_translation(int *matched_points, int npoints,
747 : int *num_inliers_by_motion,
748 : MotionModel *params_by_motion,
749 : int num_desired_motions) {
750 0 : return ransac(matched_points, npoints, num_inliers_by_motion,
751 : params_by_motion, num_desired_motions, 3,
752 : is_degenerate_translation, find_translation,
753 : project_points_double_translation);
754 : }
755 :
756 113 : static int ransac_rotzoom(int *matched_points, int npoints,
757 : int *num_inliers_by_motion,
758 : MotionModel *params_by_motion,
759 : int num_desired_motions) {
760 113 : return ransac(matched_points, npoints, num_inliers_by_motion,
761 : params_by_motion, num_desired_motions, 3, is_degenerate_affine,
762 : find_rotzoom, project_points_double_rotzoom);
763 : }
764 :
765 0 : static int ransac_affine(int *matched_points, int npoints,
766 : int *num_inliers_by_motion,
767 : MotionModel *params_by_motion,
768 : int num_desired_motions) {
769 0 : return ransac(matched_points, npoints, num_inliers_by_motion,
770 : params_by_motion, num_desired_motions, 3, is_degenerate_affine,
771 : find_affine, project_points_double_affine);
772 : }
773 :
774 113 : RansacFunc av1_get_ransac_type(TransformationType type) {
775 113 : switch (type) {
776 0 : case AFFINE: return ransac_affine;
777 113 : case ROTZOOM: return ransac_rotzoom;
778 0 : case TRANSLATION: return ransac_translation;
779 0 : default: assert(0); return NULL;
780 : }
781 : }
782 :
783 0 : static int ransac_translation_double_prec(double *matched_points, int npoints,
784 : int *num_inliers_by_motion,
785 : MotionModel *params_by_motion,
786 : int num_desired_motions) {
787 0 : return ransac_double_prec(matched_points, npoints, num_inliers_by_motion,
788 : params_by_motion, num_desired_motions, 3,
789 : is_degenerate_translation, find_translation,
790 : project_points_double_translation);
791 : }
792 :
793 0 : static int ransac_rotzoom_double_prec(double *matched_points, int npoints,
794 : int *num_inliers_by_motion,
795 : MotionModel *params_by_motion,
796 : int num_desired_motions) {
797 0 : return ransac_double_prec(matched_points, npoints, num_inliers_by_motion,
798 : params_by_motion, num_desired_motions, 3,
799 : is_degenerate_affine, find_rotzoom,
800 : project_points_double_rotzoom);
801 : }
802 :
803 0 : static int ransac_affine_double_prec(double *matched_points, int npoints,
804 : int *num_inliers_by_motion,
805 : MotionModel *params_by_motion,
806 : int num_desired_motions) {
807 0 : return ransac_double_prec(matched_points, npoints, num_inliers_by_motion,
808 : params_by_motion, num_desired_motions, 3,
809 : is_degenerate_affine, find_affine,
810 : project_points_double_affine);
811 : }
812 :
813 0 : RansacFuncDouble av1_get_ransac_double_prec_type(TransformationType type) {
814 0 : switch (type) {
815 0 : case AFFINE: return ransac_affine_double_prec;
816 0 : case ROTZOOM: return ransac_rotzoom_double_prec;
817 0 : case TRANSLATION: return ransac_translation_double_prec;
818 0 : default: assert(0); return NULL;
819 : }
820 : }
|
