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