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