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 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 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 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 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 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 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 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 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 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 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 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 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 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. 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 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 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 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 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 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 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 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 736 static int is_degenerate_affine(double *p) { 737 return is_collinear3(p, p + 2, p + 4); 738 } 739 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 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 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 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 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 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
