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