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;
}
/* 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);
}
