bool FundamentalMatrixSolver::ComputeFundamentalMatrices( std::vector<Matrix3d>& fundamental_matrices) { // Clear the output. fundamental_matrices.clear(); // Determine a fundamental matrix for each pair of images. for (const auto& pair_data : matched_image_data_) { Matrix3d fundamental_matrix; if (ComputeFundamentalMatrix(pair_data.feature_matches_, fundamental_matrix)) { fundamental_matrices.push_back(fundamental_matrix); } else { VLOG(1) << "Failed to compute funamental matrix."; } } // Return whether or not we computed any fundamental matrices. if (fundamental_matrices.empty()) { VLOG(1) << "Unable to compute a fundamental matrix for any of the input " "image pairs."; return false; } return true; }
bool BundlerApp::EstimateRelativePose(int i1, int i2, camera_params_t &camera1, camera_params_t &camera2) { MatchIndex list_idx; if (i1 < i2) list_idx = GetMatchIndex(i1, i2); else list_idx = GetMatchIndex(i2, i1); std::vector<KeypointMatch> &matches = m_matches.GetMatchList(list_idx); int num_matches = (int) matches.size(); double f1 = m_image_data[i1].m_init_focal; double f2 = m_image_data[i2].m_init_focal; double E[9], F[9]; std::vector<int> inliers; if (!m_optimize_for_fisheye) { inliers = EstimateEMatrix(m_image_data[i1].m_keys, m_image_data[i2].m_keys, matches, 4 * m_fmatrix_rounds, // 8 * m_fmatrix_rounds, m_fmatrix_threshold * m_fmatrix_threshold, f1, f2, E, F); } else { /* FIXME */ inliers = EstimateEMatrix(m_image_data[i1].m_keys, m_image_data[i2].m_keys, matches, 4 * m_fmatrix_rounds, // 8 * m_fmatrix_rounds, m_fmatrix_threshold * m_fmatrix_threshold, f1, f2, E, F); } if ((int) inliers.size() == 0) return false; int num_inliers = (int) inliers.size(); printf(" Found %d / %d inliers (%0.3f%%)\n", num_inliers, num_matches, 100.0 * num_inliers / num_matches); /* Estimate a homography with the inliers */ std::vector<KeypointMatch> match_inliers; for (int i = 0; i < num_inliers; i++) { match_inliers.push_back(matches[inliers[i]]); } int num_match_inliers = (int) match_inliers.size(); double H[9]; std::vector<int> Hinliers = EstimateTransform(m_image_data[i1].m_keys, m_image_data[i2].m_keys, match_inliers, MotionHomography, 128 /*m_homography_rounds*/, 6.0 /*m_homography_threshold*/, H); printf(" Found %d / %d homography inliers (%0.3f%%)\n", (int) Hinliers.size(), num_inliers, 100.0 * Hinliers.size() / num_inliers); bool initialized = false; if ((int) Hinliers.size() > 0) { matrix_print(3, 3, H); printf("\n"); if ((double) Hinliers.size() / num_inliers >= 0.75 /*0.85*/) { KeypointMatch &match0 = matches[Hinliers[0]]; v2_t p10 = v2_new(m_image_data[i1].m_keys[match0.m_idx1].m_x, m_image_data[i1].m_keys[match0.m_idx1].m_y); v2_t p20 = v2_new(m_image_data[i2].m_keys[match0.m_idx2].m_x, m_image_data[i2].m_keys[match0.m_idx2].m_y); double R1[9], t1[3], R2[9], t2[3]; bool success = DecomposeHomography(H, f1, f2, R1, t1, R2, t2, p10, p20); if (success) { printf("[BundleTwoFrame] Using homography " "for initialization\n"); /* Decide which solution to use */ double F1h[9], F2h[9]; ComputeFundamentalMatrix(f1, f2, R1, t1, F1h); ComputeFundamentalMatrix(f1, f2, R2, t2, F2h); double F1hT[9], F2hT[9]; matrix_transpose(3, 3, F1h, F1hT); matrix_transpose(3, 3, F2h, F2hT); int num_inliers1 = 0, num_inliers2 = 0; for (int i = 0; i < num_match_inliers; i++) { const KeypointMatch &match = match_inliers[i]; const Keypoint &k1 = m_image_data[i1].m_keys[match.m_idx1]; const Keypoint &k2 = m_image_data[i2].m_keys[match.m_idx2]; v3_t rt = v3_new(k1.m_x, k1.m_y, 1.0); v3_t lft = v3_new(k2.m_x, k2.m_y, 1.0); double r1a = fmatrix_compute_residual(F1h, lft, rt); double r1b = fmatrix_compute_residual(F1hT, rt, lft); double r2a = fmatrix_compute_residual(F2h, lft, rt); double r2b = fmatrix_compute_residual(F2hT, rt, lft); if (r1a < m_fmatrix_threshold && r1b < m_fmatrix_threshold) num_inliers1++; if (r2a < m_fmatrix_threshold && r2b < m_fmatrix_threshold) num_inliers2++; } initialized = true; double *R, *t; printf(" H1: %d inliers, H2: %d inliers\n", num_inliers1, num_inliers2); if (num_inliers1 > num_inliers2) { R = R1; t = t1; } else { R = R2; t = t2; } memcpy(camera2.R, R, sizeof(double) * 9); matrix_transpose_product(3, 3, 3, 1, R, t, camera2.t); matrix_scale(3, 1, camera2.t, -1.0, camera2.t); } } } if (!initialized) { KeypointMatch &match = matches[inliers[0]]; v2_t p1 = v2_new(m_image_data[i1].m_keys[match.m_idx1].m_x / f1, m_image_data[i1].m_keys[match.m_idx1].m_y / f1); v2_t p2 = v2_new(m_image_data[i2].m_keys[match.m_idx2].m_x / f2, m_image_data[i2].m_keys[match.m_idx2].m_y / f2); double R[9], t[3]; int success = find_extrinsics_essential(E, p1, p2, R, t); if (!success) { return false; } memcpy(camera2.R, R, sizeof(double) * 9); matrix_transpose_product(3, 3, 3, 1, R, t, camera2.t); matrix_scale(3, 1, camera2.t, -1.0, camera2.t); } return true; }