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;
}
