| /* |
| * Copyright (c) 2016, Alliance for Open Media. All rights reserved. |
| * |
| * This source code is subject to the terms of the BSD 2 Clause License and |
| * the Alliance for Open Media Patent License 1.0. If the BSD 2 Clause License |
| * was not distributed with this source code in the LICENSE file, you can |
| * obtain it at www.aomedia.org/license/software. If the Alliance for Open |
| * Media Patent License 1.0 was not distributed with this source code in the |
| * PATENTS file, you can obtain it at www.aomedia.org/license/patent. |
| */ |
| |
| #include <memory.h> |
| #include <math.h> |
| #include <time.h> |
| #include <stdio.h> |
| #include <stdbool.h> |
| #include <string.h> |
| #include <assert.h> |
| |
| #include "aom_dsp/flow_estimation/ransac.h" |
| #include "aom_dsp/mathutils.h" |
| #include "aom_mem/aom_mem.h" |
| |
| // TODO(rachelbarker): Remove dependence on code in av1/encoder/ |
| #include "av1/encoder/random.h" |
| |
| #define MAX_MINPTS 4 |
| #define MINPTS_MULTIPLIER 5 |
| |
| #define INLIER_THRESHOLD 1.25 |
| #define INLIER_THRESHOLD_SQUARED (INLIER_THRESHOLD * INLIER_THRESHOLD) |
| |
| // Number of initial models to generate |
| #define NUM_TRIALS 20 |
| |
| // Number of times to refine the best model found |
| #define NUM_REFINES 5 |
| |
| // Flag to enable functions for finding TRANSLATION type models. |
| // |
| // These modes are not considered currently due to a spec bug (see comments |
| // in gm_get_motion_vector() in av1/common/mv.h). Thus we don't need to compile |
| // the corresponding search functions, but it is nice to keep the source around |
| // but disabled, for completeness. |
| #define ALLOW_TRANSLATION_MODELS 0 |
| |
| typedef struct { |
| int num_inliers; |
| double sse; // Sum of squared errors of inliers |
| int *inlier_indices; |
| } RANSAC_MOTION; |
| |
| //////////////////////////////////////////////////////////////////////////////// |
| // ransac |
| typedef bool (*FindTransformationFunc)(const Correspondence *points, |
| const int *indices, int num_indices, |
| double *params); |
| typedef void (*ScoreModelFunc)(const double *mat, const Correspondence *points, |
| int num_points, RANSAC_MOTION *model); |
| |
| // vtable-like structure which stores all of the information needed by RANSAC |
| // for a particular model type |
| typedef struct { |
| FindTransformationFunc find_transformation; |
| ScoreModelFunc score_model; |
| |
| // The minimum number of points which can be passed to find_transformation |
| // to generate a model. |
| // |
| // This should be set as small as possible. This is due to an observation |
| // from section 4 of "Optimal Ransac" by A. Hast, J. Nysjö and |
| // A. Marchetti (https://dspace5.zcu.cz/bitstream/11025/6869/1/Hast.pdf): |
| // using the minimum possible number of points in the initial model maximizes |
| // the chance that all of the selected points are inliers. |
| // |
| // That paper proposes a method which can deal with models which are |
| // contaminated by outliers, which helps in cases where the inlier fraction |
| // is low. However, for our purposes, global motion only gives significant |
| // gains when the inlier fraction is high. |
| // |
| // So we do not use the method from this paper, but we do find that |
| // minimizing the number of points used for initial model fitting helps |
| // make the best use of the limited number of models we consider. |
| int minpts; |
| } RansacModelInfo; |
| |
| #if ALLOW_TRANSLATION_MODELS |
| static void score_translation(const double *mat, const Correspondence *points, |
| int num_points, RANSAC_MOTION *model) { |
| model->num_inliers = 0; |
| model->sse = 0.0; |
| |
| for (int i = 0; i < num_points; ++i) { |
| const double x1 = points[i].x; |
| const double y1 = points[i].y; |
| const double x2 = points[i].rx; |
| const double y2 = points[i].ry; |
| |
| const double proj_x = x1 + mat[0]; |
| const double proj_y = y1 + mat[1]; |
| |
| const double dx = proj_x - x2; |
| const double dy = proj_y - y2; |
| const double sse = dx * dx + dy * dy; |
| |
| if (sse < INLIER_THRESHOLD_SQUARED) { |
| model->inlier_indices[model->num_inliers++] = i; |
| model->sse += sse; |
| } |
| } |
| } |
| #endif // ALLOW_TRANSLATION_MODELS |
| |
| static void score_affine(const double *mat, const Correspondence *points, |
| int num_points, RANSAC_MOTION *model) { |
| model->num_inliers = 0; |
| model->sse = 0.0; |
| |
| for (int i = 0; i < num_points; ++i) { |
| const double x1 = points[i].x; |
| const double y1 = points[i].y; |
| const double x2 = points[i].rx; |
| const double y2 = points[i].ry; |
| |
| const double proj_x = mat[2] * x1 + mat[3] * y1 + mat[0]; |
| const double proj_y = mat[4] * x1 + mat[5] * y1 + mat[1]; |
| |
| const double dx = proj_x - x2; |
| const double dy = proj_y - y2; |
| const double sse = dx * dx + dy * dy; |
| |
| if (sse < INLIER_THRESHOLD_SQUARED) { |
| model->inlier_indices[model->num_inliers++] = i; |
| model->sse += sse; |
| } |
| } |
| } |
| |
| #if ALLOW_TRANSLATION_MODELS |
| static bool find_translation(const Correspondence *points, const int *indices, |
| int num_indices, double *params) { |
| double sumx = 0; |
| double sumy = 0; |
| |
| for (int i = 0; i < num_indices; ++i) { |
| int index = indices[i]; |
| const double sx = points[index].x; |
| const double sy = points[index].y; |
| const double dx = points[index].rx; |
| const double dy = points[index].ry; |
| |
| sumx += dx - sx; |
| sumy += dy - sy; |
| } |
| |
| params[0] = sumx / np; |
| params[1] = sumy / np; |
| params[2] = 1; |
| params[3] = 0; |
| params[4] = 0; |
| params[5] = 1; |
| return true; |
| } |
| #endif // ALLOW_TRANSLATION_MODELS |
| |
| static bool find_rotzoom(const Correspondence *points, const int *indices, |
| int num_indices, double *params) { |
| const int n = 4; // Size of least-squares problem |
| double mat[4 * 4]; // Accumulator for A'A |
| double y[4]; // Accumulator for A'b |
| double a[4]; // Single row of A |
| double b; // Single element of b |
| |
| least_squares_init(mat, y, n); |
| for (int i = 0; i < num_indices; ++i) { |
| int index = indices[i]; |
| const double sx = points[index].x; |
| const double sy = points[index].y; |
| const double dx = points[index].rx; |
| const double dy = points[index].ry; |
| |
| a[0] = 1; |
| a[1] = 0; |
| a[2] = sx; |
| a[3] = sy; |
| b = dx; |
| least_squares_accumulate(mat, y, a, b, n); |
| |
| a[0] = 0; |
| a[1] = 1; |
| a[2] = sy; |
| a[3] = -sx; |
| b = dy; |
| least_squares_accumulate(mat, y, a, b, n); |
| } |
| |
| // Fill in params[0] .. params[3] with output model |
| if (!least_squares_solve(mat, y, params, n)) { |
| return false; |
| } |
| |
| // Fill in remaining parameters |
| params[4] = -params[3]; |
| params[5] = params[2]; |
| |
| return true; |
| } |
| |
| static bool find_affine(const Correspondence *points, const int *indices, |
| int num_indices, double *params) { |
| // Note: The least squares problem for affine models is 6-dimensional, |
| // but it splits into two independent 3-dimensional subproblems. |
| // Solving these two subproblems separately and recombining at the end |
| // results in less total computation than solving the 6-dimensional |
| // problem directly. |
| // |
| // The two subproblems correspond to all the parameters which contribute |
| // to the x output of the model, and all the parameters which contribute |
| // to the y output, respectively. |
| |
| const int n = 3; // Size of each least-squares problem |
| double mat[2][3 * 3]; // Accumulator for A'A |
| double y[2][3]; // Accumulator for A'b |
| double x[2][3]; // Output vector |
| double a[2][3]; // Single row of A |
| double b[2]; // Single element of b |
| |
| least_squares_init(mat[0], y[0], n); |
| least_squares_init(mat[1], y[1], n); |
| for (int i = 0; i < num_indices; ++i) { |
| int index = indices[i]; |
| const double sx = points[index].x; |
| const double sy = points[index].y; |
| const double dx = points[index].rx; |
| const double dy = points[index].ry; |
| |
| a[0][0] = 1; |
| a[0][1] = sx; |
| a[0][2] = sy; |
| b[0] = dx; |
| least_squares_accumulate(mat[0], y[0], a[0], b[0], n); |
| |
| a[1][0] = 1; |
| a[1][1] = sx; |
| a[1][2] = sy; |
| b[1] = dy; |
| least_squares_accumulate(mat[1], y[1], a[1], b[1], n); |
| } |
| |
| if (!least_squares_solve(mat[0], y[0], x[0], n)) { |
| return false; |
| } |
| if (!least_squares_solve(mat[1], y[1], x[1], n)) { |
| return false; |
| } |
| |
| // Rearrange least squares result to form output model |
| params[0] = x[0][0]; |
| params[1] = x[1][0]; |
| params[2] = x[0][1]; |
| params[3] = x[0][2]; |
| params[4] = x[1][1]; |
| params[5] = x[1][2]; |
| |
| return true; |
| } |
| |
| // Return -1 if 'a' is a better motion, 1 if 'b' is better, 0 otherwise. |
| static int compare_motions(const void *arg_a, const void *arg_b) { |
| const RANSAC_MOTION *motion_a = (RANSAC_MOTION *)arg_a; |
| const RANSAC_MOTION *motion_b = (RANSAC_MOTION *)arg_b; |
| |
| if (motion_a->num_inliers > motion_b->num_inliers) return -1; |
| if (motion_a->num_inliers < motion_b->num_inliers) return 1; |
| if (motion_a->sse < motion_b->sse) return -1; |
| if (motion_a->sse > motion_b->sse) return 1; |
| return 0; |
| } |
| |
| static bool is_better_motion(const RANSAC_MOTION *motion_a, |
| const RANSAC_MOTION *motion_b) { |
| return compare_motions(motion_a, motion_b) < 0; |
| } |
| |
| // Returns true on success, false on error |
| static bool ransac_internal(const Correspondence *matched_points, int npoints, |
| MotionModel *motion_models, int num_desired_motions, |
| const RansacModelInfo *model_info, |
| bool *mem_alloc_failed) { |
| assert(npoints >= 0); |
| int i = 0; |
| int minpts = model_info->minpts; |
| bool ret_val = true; |
| |
| unsigned int seed = (unsigned int)npoints; |
| |
| int indices[MAX_MINPTS] = { 0 }; |
| |
| // Store information for the num_desired_motions best transformations found |
| // and the worst motion among them, as well as the motion currently under |
| // consideration. |
| RANSAC_MOTION *motions, *worst_kept_motion = NULL; |
| RANSAC_MOTION current_motion; |
| |
| // Store the parameters and the indices of the inlier points for the motion |
| // currently under consideration. |
| double params_this_motion[MAX_PARAMDIM]; |
| |
| // Initialize output models, as a fallback in case we can't find a model |
| for (i = 0; i < num_desired_motions; i++) { |
| memcpy(motion_models[i].params, kIdentityParams, |
| MAX_PARAMDIM * sizeof(*(motion_models[i].params))); |
| motion_models[i].num_inliers = 0; |
| } |
| |
| if (npoints < minpts * MINPTS_MULTIPLIER || npoints == 0) { |
| return false; |
| } |
| |
| int min_inliers = AOMMAX((int)(MIN_INLIER_PROB * npoints), minpts); |
| |
| motions = |
| (RANSAC_MOTION *)aom_calloc(num_desired_motions, sizeof(RANSAC_MOTION)); |
| |
| // Allocate one large buffer which will be carved up to store the inlier |
| // indices for the current motion plus the num_desired_motions many |
| // output models |
| // This allows us to keep the allocation/deallocation logic simple, without |
| // having to (for example) check that `motions` is non-null before allocating |
| // the inlier arrays |
| int *inlier_buffer = (int *)aom_malloc(sizeof(*inlier_buffer) * npoints * |
| (num_desired_motions + 1)); |
| |
| if (!(motions && inlier_buffer)) { |
| ret_val = false; |
| *mem_alloc_failed = true; |
| goto finish_ransac; |
| } |
| |
| // Once all our allocations are known-good, we can fill in our structures |
| worst_kept_motion = motions; |
| |
| for (i = 0; i < num_desired_motions; ++i) { |
| motions[i].inlier_indices = inlier_buffer + i * npoints; |
| } |
| memset(¤t_motion, 0, sizeof(current_motion)); |
| current_motion.inlier_indices = inlier_buffer + num_desired_motions * npoints; |
| |
| for (int trial_count = 0; trial_count < NUM_TRIALS; trial_count++) { |
| lcg_pick(npoints, minpts, indices, &seed); |
| |
| if (!model_info->find_transformation(matched_points, indices, minpts, |
| params_this_motion)) { |
| continue; |
| } |
| |
| model_info->score_model(params_this_motion, matched_points, npoints, |
| ¤t_motion); |
| |
| if (current_motion.num_inliers < min_inliers) { |
| // Reject models with too few inliers |
| continue; |
| } |
| |
| if (is_better_motion(¤t_motion, worst_kept_motion)) { |
| // This motion is better than the worst currently kept motion. Remember |
| // the inlier points and sse. The parameters for each kept motion |
| // will be recomputed later using only the inliers. |
| worst_kept_motion->num_inliers = current_motion.num_inliers; |
| worst_kept_motion->sse = current_motion.sse; |
| |
| // Rather than copying the (potentially many) inlier indices from |
| // current_motion.inlier_indices to worst_kept_motion->inlier_indices, |
| // we can swap the underlying pointers. |
| // |
| // This is okay because the next time current_motion.inlier_indices |
| // is used will be in the next trial, where we ignore its previous |
| // contents anyway. And both arrays will be deallocated together at the |
| // end of this function, so there are no lifetime issues. |
| int *tmp = worst_kept_motion->inlier_indices; |
| worst_kept_motion->inlier_indices = current_motion.inlier_indices; |
| current_motion.inlier_indices = tmp; |
| |
| // Determine the new worst kept motion and its num_inliers and sse. |
| for (i = 0; i < num_desired_motions; ++i) { |
| if (is_better_motion(worst_kept_motion, &motions[i])) { |
| worst_kept_motion = &motions[i]; |
| } |
| } |
| } |
| } |
| |
| // Sort the motions, best first. |
| qsort(motions, num_desired_motions, sizeof(RANSAC_MOTION), compare_motions); |
| |
| // Refine each of the best N models using iterative estimation. |
| // |
| // The idea here is loosely based on the iterative method from |
| // "Locally Optimized RANSAC" by O. Chum, J. Matas and Josef Kittler: |
| // https://cmp.felk.cvut.cz/ftp/articles/matas/chum-dagm03.pdf |
| // |
| // However, we implement a simpler version than their proposal, and simply |
| // refit the model repeatedly until the number of inliers stops increasing, |
| // with a cap on the number of iterations to defend against edge cases which |
| // only improve very slowly. |
| for (i = 0; i < num_desired_motions; ++i) { |
| if (motions[i].num_inliers <= 0) { |
| // Output model has already been initialized to the identity model, |
| // so just skip setup |
| continue; |
| } |
| |
| bool bad_model = false; |
| for (int refine_count = 0; refine_count < NUM_REFINES; refine_count++) { |
| int num_inliers = motions[i].num_inliers; |
| assert(num_inliers >= min_inliers); |
| |
| if (!model_info->find_transformation(matched_points, |
| motions[i].inlier_indices, |
| num_inliers, params_this_motion)) { |
| // In the unlikely event that this model fitting fails, we don't have a |
| // good fallback. So leave this model set to the identity model |
| bad_model = true; |
| break; |
| } |
| |
| // Score the newly generated model |
| model_info->score_model(params_this_motion, matched_points, npoints, |
| ¤t_motion); |
| |
| // At this point, there are three possibilities: |
| // 1) If we found more inliers, keep refining. |
| // 2) If we found the same number of inliers but a lower SSE, we want to |
| // keep the new model, but further refinement is unlikely to gain much. |
| // So commit to this new model |
| // 3) It is possible, but very unlikely, that the new model will have |
| // fewer inliers. If it does happen, we probably just lost a few |
| // borderline inliers. So treat the same as case (2). |
| if (current_motion.num_inliers > motions[i].num_inliers) { |
| motions[i].num_inliers = current_motion.num_inliers; |
| motions[i].sse = current_motion.sse; |
| int *tmp = motions[i].inlier_indices; |
| motions[i].inlier_indices = current_motion.inlier_indices; |
| current_motion.inlier_indices = tmp; |
| } else { |
| // Refined model is no better, so stop |
| // This shouldn't be significantly worse than the previous model, |
| // so it's fine to use the parameters in params_this_motion. |
| // This saves us from having to cache the previous iteration's params. |
| break; |
| } |
| } |
| |
| if (bad_model) continue; |
| |
| // Fill in output struct |
| memcpy(motion_models[i].params, params_this_motion, |
| MAX_PARAMDIM * sizeof(*motion_models[i].params)); |
| for (int j = 0; j < motions[i].num_inliers; j++) { |
| int index = motions[i].inlier_indices[j]; |
| const Correspondence *corr = &matched_points[index]; |
| motion_models[i].inliers[2 * j + 0] = (int)rint(corr->x); |
| motion_models[i].inliers[2 * j + 1] = (int)rint(corr->y); |
| } |
| motion_models[i].num_inliers = motions[i].num_inliers; |
| } |
| |
| finish_ransac: |
| aom_free(inlier_buffer); |
| aom_free(motions); |
| |
| return ret_val; |
| } |
| |
| static const RansacModelInfo ransac_model_info[TRANS_TYPES] = { |
| // IDENTITY |
| { NULL, NULL, 0 }, |
| // TRANSLATION |
| #if ALLOW_TRANSLATION_MODELS |
| { find_translation, score_translation, 1 }, |
| #else |
| { NULL, NULL, 0 }, |
| #endif |
| // ROTZOOM |
| { find_rotzoom, score_affine, 2 }, |
| // AFFINE |
| { find_affine, score_affine, 3 }, |
| }; |
| |
| // Returns true on success, false on error |
| bool ransac(const Correspondence *matched_points, int npoints, |
| TransformationType type, MotionModel *motion_models, |
| int num_desired_motions, bool *mem_alloc_failed) { |
| #if ALLOW_TRANSLATION_MODELS |
| assert(type > IDENTITY && type < TRANS_TYPES); |
| #else |
| assert(type > TRANSLATION && type < TRANS_TYPES); |
| #endif // ALLOW_TRANSLATION_MODELS |
| |
| return ransac_internal(matched_points, npoints, motion_models, |
| num_desired_motions, &ransac_model_info[type], |
| mem_alloc_failed); |
| } |