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