/* Estimate an F-matrix from a given set of point matches */ std::vector<int> EstimateFMatrix(const std::vector<Keypoint> &k1, const std::vector<Keypoint> &k2, std::vector<KeypointMatch> matches, int num_trials, double threshold, double *F, bool essential) { int num_pts = (int) matches.size(); /* num_pts should be greater than a threshold */ if (num_pts < 20) { std::vector<int> inliers; return inliers; } v3_t *k1_pts = new v3_t[num_pts]; v3_t *k2_pts = new v3_t[num_pts]; v3_t *k1_pts_in = new v3_t[num_pts]; v3_t *k2_pts_in = new v3_t[num_pts]; for (int i = 0; i < num_pts; i++) { int idx1 = matches[i].m_idx1; int idx2 = matches[i].m_idx2; if(idx1 >= k1.size()) { fprintf(stderr, "Error: %d >= %d\n", idx1, k1.size()); fprintf(stderr, "idx1/idx2: %d %d\n", idx1, idx2); fprintf(stderr, "Fix me 1\n"); continue; } if(idx2 >= k2.size()) { fprintf(stderr, "Error: %d >= %d\n", idx2, k2.size()); fprintf(stderr, "idx1/idx2: %d %d\n", idx1, idx2); fprintf(stderr, "Fix me 2\n"); continue; } assert(idx1 < (int) k1.size()); assert(idx2 < (int) k2.size()); k1_pts[i] = v3_new(k1[idx1].m_x, k1[idx1].m_y, 1.0); k2_pts[i] = v3_new(k2[idx2].m_x, k2[idx2].m_y, 1.0); } estimate_fmatrix_ransac_matches(num_pts, k2_pts, k1_pts, num_trials, threshold, 0.99, (essential ? 1 : 0), F); /* Find the inliers */ std::vector<int> inliers; for (int i = 0; i < num_pts; i++) { double dist = fmatrix_compute_residual(F, k2_pts[i], k1_pts[i]); if (dist < threshold) { inliers.push_back(i); } } /* Re-estimate using inliers */ int num_inliers = (int) inliers.size(); for (int i = 0; i < num_inliers; i++) { k1_pts_in[i] = k1_pts[inliers[i]]; // v3_new(k1[idx1]->m_x, k1[idx1]->m_y, 1.0); k2_pts_in[i] = k2_pts[inliers[i]]; // v3_new(k2[idx2]->m_x, k2[idx2]->m_y, 1.0); } // printf("[1] num_inliers = %d\n", num_inliers); #if 0 double F0[9]; double e1[3], e2[3]; estimate_fmatrix_linear(num_inliers, k2_pts_in, k1_pts_in, F0, e1, e2); inliers.clear(); for (int i = 0; i < num_pts; i++) { double dist = fmatrix_compute_residual(F0, k2_pts[i], k1_pts[i]); if (dist < threshold) { inliers.push_back(i); } } num_inliers = inliers.size(); // printf("[2] num_inliers = %d\n", num_inliers); // matrix_print(3, 3, F0); #else double F0[9]; memcpy(F0, F, sizeof(double) * 9); #endif if (!essential) { /* Refine using NLLS */ for (int i = 0; i < num_inliers; i++) { k1_pts_in[i] = k1_pts[inliers[i]]; k2_pts_in[i] = k2_pts[inliers[i]]; } refine_fmatrix_nonlinear_matches(num_inliers, k2_pts_in, k1_pts_in, F0, F); } else { memcpy(F, F0, sizeof(double) * 9); } #if 0 if (essential) { /* Compute the SVD of F */ double U[9], S[3], VT[9]; dgesvd_driver(3, 3, F, U, S, VT); double E0[9] = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0 }; double tmp[9]; matrix_product(3, 3, 3, 3, U, E0, tmp); matrix_product(3, 3, 3, 3, tmp, VT, F); } #endif inliers.clear(); for (int i = 0; i < num_pts; i++) { double dist = fmatrix_compute_residual(F, k2_pts[i], k1_pts[i]); if (dist < threshold) { inliers.push_back(i); } } num_inliers = (int) inliers.size(); delete [] k1_pts; delete [] k2_pts; delete [] k1_pts_in; delete [] k2_pts_in; return inliers; }
/* Use RANSAC to estimate a homography */ void align_homography_ransac_matches(int num_pts, v3_t *a_pts, v3_t *b_pts, int num_trials, double threshold, double success_ratio, int essential, double *H) { #define MIN_SAMPLES 4 int i, j, k, idx; v3_t l_pts_best[MIN_SAMPLES], r_pts_best[MIN_SAMPLES]; double Hbest[MIN_SAMPLES]; double *resid; double error_min; int inliers_max; double *a_matrix, *b_matrix; // double threshold = 1.0e-10; // srand(time(0)); /* Make an array of all good correspondences */ if (num_pts < MIN_SAMPLES) { printf("[align_homography_ransac] " "Could not find 8 good correspondences, " "homography estimation failed\n"); return; } a_matrix = malloc(sizeof(double) * 3 * num_pts); b_matrix = malloc(sizeof(double) * 3 * num_pts); for (i = 0; i < num_pts; i++) { a_matrix[i] = Vx(a_pts[i]); a_matrix[i+num_pts] = Vy(a_pts[i]); a_matrix[i+2*num_pts] = Vz(a_pts[i]); b_matrix[i] = Vx(b_pts[i]); b_matrix[i+num_pts] = Vy(b_pts[i]); b_matrix[i+2*num_pts] = Vz(b_pts[i]); } error_min = DBL_MAX; inliers_max = 0; resid = (double *) malloc(sizeof(double) * num_pts); /* Estimate the homography using RANSAC */ for (i = 0; i < num_trials; i++) { int idxs[MIN_SAMPLES]; v3_t l_pts[MIN_SAMPLES], r_pts[MIN_SAMPLES]; double Htmp[9]; // double error; int num_inliers = 0; int success, nan = 0; int round = 0; /* Sample 4 random correspondences */ for (j = 0; j < MIN_SAMPLES; j++) { int reselect = 0; if (round == 1000) return; idx = rand() % num_pts; /* Make sure we didn't sample this index yet */ for (k = 0; k < j; k++) { if (idx == idxs[k] || (Vx(a_pts[idx]) == Vx(a_pts[idxs[k]]) && Vy(a_pts[idx]) == Vy(a_pts[idxs[k]]) && Vz(a_pts[idx]) == Vz(a_pts[idxs[k]])) || (Vx(b_pts[idx]) == Vx(b_pts[idxs[k]]) && Vy(b_pts[idx]) == Vy(b_pts[idxs[k]]) && Vz(b_pts[idx]) == Vz(b_pts[idxs[k]]))) { reselect = 1; break; } } if (reselect) { round++; j--; continue; } idxs[j] = idx; } /* Fill in the left and right points */ for (j = 0; j < 8; j++) { l_pts[j] = b_pts[idxs[j]]; r_pts[j] = a_pts[idxs[j]]; } /* Estimate the F-matrix */ success = estimate_fmatrix_linear(8, r_pts, l_pts, essential, Ftmp, e1_tmp, e2_tmp); if (success == 0) nan = 1; for (j = 0; j < 9; j++) { if (Ftmp[j] != Ftmp[j] /* isnan(Ftmp[j]) */) { printf("[estimate_fmatrix_ransac_matches] nan encountered\n"); nan = 1; break; } } /* Check for nan entries */ if (isnan(Ftmp[0]) || isnan(Ftmp[1]) || isnan(Ftmp[2]) || isnan(Ftmp[3]) || isnan(Ftmp[4]) || isnan(Ftmp[5]) || isnan(Ftmp[6]) || isnan(Ftmp[7]) || isnan(Ftmp[8])) { printf("[estimate_fmatrix_ransac_matches] " "nan matrix encountered\n"); nan = 1; } if (nan) { // error = DBL_MAX; num_inliers = 0; } else { // printf("%0.3f\n", Ftmp[0]); /* Compute residuals */ #if 1 for (j = 0; j < num_pts; j++) { resid[j] = fmatrix_compute_residual(Ftmp, a_pts[j], b_pts[j]); if (resid[j] < threshold) num_inliers++; } #else fmatrix_compute_residuals(num_pts, Ftmp, a_matrix, b_matrix, resid); for (j = 0; j < num_pts; j++) { if (resid[j] < threshold) num_inliers++; } #endif #if 0 /* Find the median */ error = median(num_pts, resid); if (error < error_min) { error_min = error; memcpy(Fbest, Ftmp, sizeof(double) * 9); memcpy(l_pts_best, l_pts, sizeof(v3_t) * 8); memcpy(r_pts_best, r_pts, sizeof(v3_t) * 8); } #else if (num_inliers > inliers_max) { inliers_max = num_inliers; memcpy(Fbest, Ftmp, sizeof(double) * 9); memcpy(l_pts_best, l_pts, sizeof(v3_t) * 8); memcpy(r_pts_best, r_pts, sizeof(v3_t) * 8); } #endif } #if 0 if (error < threshold) break; #endif if ((double) num_inliers / num_pts > success_ratio) break; } // printf("Minimum error: %0.5e\n", error_min); // printf("Maximum inliers: %d\n", inliers_max); // matrix_print(3, 3, Fbest); free(resid); /* Copy out the F-matrix */ memcpy(F, Fbest, sizeof(double) * 9); free(a_matrix); free(b_matrix); }