std::vector<int> EstimateFMatrix(const std::vector<KeypointWithDesc> &k1, const std::vector<KeypointWithDesc> &k2, std::vector<KeypointMatch> matches, int num_trials, double threshold, double *F, bool essential) { int num_keys1 = (int) k1.size(); int num_keys2 = (int) k2.size(); std::vector<Keypoint> k1_prime, k2_prime; k1_prime.resize(num_keys1); k2_prime.resize(num_keys2); for (int i = 0; i < num_keys1; i++) { Keypoint k(k1[i].m_x, k1[i].m_y); k1_prime[i] = k; } for (int i = 0; i < num_keys2; i++) { Keypoint k(k2[i].m_x, k2[i].m_y); k2_prime[i] = k; } return EstimateFMatrix(k1_prime, k2_prime, matches, num_trials, threshold, F, essential); }
/* Compute epipolar geometry between a given pair of images */ bool BundlerApp::ComputeEpipolarGeometry(int idx1, int idx2, bool removeBadMatches) { assert(m_image_data[idx1].m_keys_loaded); assert(m_image_data[idx2].m_keys_loaded); MatchIndex offset = GetMatchIndex(idx1, idx2); MatchIndex offset_rev = GetMatchIndex(idx2, idx1); std::vector<KeypointMatch> &list = m_matches.GetMatchList(offset); double F[9]; std::vector<int> inliers = EstimateFMatrix(m_image_data[idx1].m_keys, m_image_data[idx2].m_keys, list, m_fmatrix_rounds, m_fmatrix_threshold /* 20.0 */ /* 9.0 */, F); int num_inliers = (int) inliers.size(); printf("Inliers[%d,%d] = %d out of %d\n", idx1, idx2, num_inliers, (int) list.size()); if (removeBadMatches) { /* Refine the matches */ std::vector<KeypointMatch> new_match_list; for (int i = 0; i < num_inliers; i++) { new_match_list.push_back(list[inliers[i]]); } // m_match_lists[offset].clear(); // m_match_lists[offset] = new_match_list; list.clear(); list = new_match_list; } #define MIN_INLIERS_EPIPOLAR 16 if (num_inliers >= m_min_num_feat_matches /*MIN_INLIERS_EPIPOLAR*/) { // if (m_transforms[offset] == NULL) m_transforms[offset] = TransformInfo(); m_transforms[offset_rev] = TransformInfo(); memcpy(m_transforms[offset].m_fmatrix, F, 9 * sizeof(double)); // m_transforms[offset]->m_scale = sqrt(M[0] * M[0] + M[1] * M[1]); printf("Inliers[%d,%d] = %d out of %lu\n", idx1, idx2, num_inliers, list.size()); return true; } else { return false; } }
/* Estimate an E-matrix from a given set of point matches */ std::vector<int> EstimateEMatrix(const std::vector<Keypoint> &k1, const std::vector<Keypoint> &k2, std::vector<KeypointMatch> matches, int num_trials, double threshold, double f1, double f2, double *E, double *F) { int num_keys1 = k1.size(); int num_keys2 = k2.size(); std::vector<Keypoint> k1_norm, k2_norm; k1_norm.resize(num_keys1); k2_norm.resize(num_keys2); for (int i = 0; i < num_keys1; i++) { Keypoint k; k.m_x = k1[i].m_x / f1; k.m_y = k1[i].m_y / f1; k1_norm[i] = k; } for (int i = 0; i < num_keys2; i++) { Keypoint k; k.m_x = k2[i].m_x / f2; k.m_y = k2[i].m_y / f2; k2_norm[i] = k; } double scale = 0.5 * (f1 + f2); std::vector<int> inliers = EstimateFMatrix(k1_norm, k2_norm, matches, num_trials, threshold / (scale * scale), E, true); double K1_inv[9] = { 1.0 / f1, 0.0, 0.0, 0.0, 1.0 / f1, 0.0, 0.0, 0.0, 1.0 }; double K2_inv[9] = { 1.0 / f2, 0.0, 0.0, 0.0, 1.0 / f2, 0.0, 0.0, 0.0, 1.0 }; double tmp[9]; matrix_product(3, 3, 3, 3, K1_inv, E, tmp); matrix_product(3, 3, 3, 3, K2_inv, tmp, F); return inliers; }