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;
}
int main_ada_maps(int argc, char **argv)
{
if (argc != 10) {
printf(" %s adaMaps <edge-image1.bmp> <edge-image2.bmp> "
"<dpyr12-ada> <dpyr21-ada> <dpyr12-in> <dpyr21-in> "
"<dpyr12-out> <dpyr21-out>\n",
argv[0]);
return -1;
}
char *in_image1 = argv[2];
char *in_image2 = argv[3];
char *ada_dpyr1to2 = argv[4];
char *ada_dpyr2to1 = argv[5];
char *in_dpyr1to2 = argv[6];
char *in_dpyr2to1 = argv[7];
char *out_dpyr1to2 = argv[8];
char *out_dpyr2to1 = argv[9];
img_t *img_edge1 = img_read_bmp_file(in_image1);
img_t *img_edge2 = img_read_bmp_file(in_image2);
img_dist_pyr_t *ada1to2 = img_read_distance_pyramid_file(ada_dpyr1to2);
img_dist_pyr_t *ada2to1 = img_read_distance_pyramid_file(ada_dpyr2to1);
img_dist_pyr_t *in1to2 = img_read_distance_pyramid_file(in_dpyr1to2);
img_dist_pyr_t *in2to1 = img_read_distance_pyramid_file(in_dpyr2to1);
int w = img_edge1->w;
int h = img_edge1->h;
/* First, compute a TPS warp using the ADA points */
MotionParams params;
params.num_basis_pts = NUM_FEATURE_POINTS;
params.lambda = 1.0e1;
img_dmap_t *tmp1to2a = img_dmap_new(w, h);
img_dmap_t *tmp2to1a = img_dmap_new(w, h);
std::vector<int> inliers;
inliers = EstimateTransform(&(ada1to2->dmaps[0]), &(ada2to1->dmaps[0]),
MotionThinPlateSpline, 1, 100.0, ¶ms,
tmp1to2a, tmp2to1a, true);
img_dist_pyr_t *tmp1to2a_pyr = dmap2dpyr(tmp1to2a);
img_dist_pyr_t *tmp2to1a_pyr = dmap2dpyr(tmp2to1a);
img_write_distance_pyramid_file(tmp1to2a_pyr, "tmp12.pyr");
img_write_distance_pyramid_file(tmp2to1a_pyr, "tmp21.pyr");
printf("[AfterInit] num_inliers = %d\n", inliers.size());
fflush(stdout);
/* Now find the inliers for the shape context points */
std::vector<point_t> p1, p2;
std::vector<point_match_t> matches;
VectorizeDMAP(&(in1to2->dmaps[0]), &(in2to1->dmaps[0]),
p1, p2, matches);
CountInliers(p1, p2, matches,
params.basis_pts,
params.x_affine, params.x_weights,
params.y_affine, params.y_weights,
25.0, inliers);
printf("[AfterFit] num_inliers = %d\n", inliers.size());
img_dmap_t *tmp1to2b = img_dmap_new(w, h);
img_dmap_t *tmp2to1b = img_dmap_new(w, h);
PruneDMAP(&(in1to2->dmaps[0]), &(in2to1->dmaps[0]),
p1, p2, matches, inliers, tmp1to2b, tmp2to1b);
/* Finally, relax the fit */
img_dmap_t *tmp1to2c = img_dmap_new(w, h);
img_dmap_t *tmp2to1c = img_dmap_new(w, h);
params.num_basis_pts = 512;
params.lambda = 1.0e3;
inliers = EstimateTransform(tmp1to2b, tmp2to1b,
MotionThinPlateSpline, 64, 6.0, ¶ms,
tmp1to2c, tmp2to1c, true);
printf("[AfterRelax] num_inliers = %d\n", inliers.size());
img_dist_pyr_t *out1to2 = dmap2dpyr(tmp1to2c);
img_dist_pyr_t *out2to1 = dmap2dpyr(tmp2to1c);
img_write_distance_pyramid_file(out1to2, out_dpyr1to2);
img_write_distance_pyramid_file(out2to1, out_dpyr2to1);
return 0;
}
/* Compute a transform between a given pair of images */
bool BundlerApp::ComputeTransform(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);
double M[9];
if (idx1 == idx2) {
printf("[ComputeTransform] Error: computing tranform "
"for identical images\n");
return false;
}
std::vector<KeypointMatch> &list = m_matches.GetMatchList(offset);
std::vector<int> inliers =
EstimateTransform(m_image_data[idx1].m_keys,
m_image_data[idx2].m_keys,
list, MotionHomography,
m_homography_rounds,
m_homography_threshold,
/* 15.0 */ /* 6.0 */ /* 4.0 */ M);
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 10
if (num_inliers >= MIN_INLIERS) {
m_transforms[offset].m_num_inliers = num_inliers;
m_transforms[offset].m_inlier_ratio =
((double) num_inliers) / ((double) list.size());
memcpy(m_transforms[offset].m_H, M, 9 * sizeof(double));
// m_transforms[offset]->m_scale = sqrt(M[0] * M[0] + M[1] * M[1]);
#if 1
printf("Inliers[%d,%d] = %d out of %d\n", idx1, idx2, num_inliers,
(int) m_matches.GetNumMatches(offset));
// (int) m_match_lists[offset].size());
printf("Ratio[%d,%d] = %0.3e\n",
idx1, idx2, m_transforms[offset].m_inlier_ratio);
matrix_print(3, 3, m_transforms[offset].m_H);
#endif
return true;
} else {
return false;
}
}
int main_smooth_maps(int argc, char **argv)
{
MotionParams params;
char *in_image1 = NULL, *in_image2 = NULL;
img_t *img_edge1 = NULL, *img_edge2 = NULL;
char *in_dpyr1to2, *in_dpyr2to1;
img_dist_pyr_t *dpyr1to2, *dpyr2to1;
img_dist_pyr_t *tmp1, *tmp2;
char *out_dpyr1to2, *out_dpyr2to1;
img_dist_pyr_t *dpyr1to2_out, *dpyr2to1_out;
img_dmap_t *dmap1to2_out, *dmap2to1_out;
img_dmap_t *dmap1to2_out2, *dmap2to1_out2;
img_dmap_t *dmap1to2_out3, *dmap2to1_out3;
img_dmap_t *dmap1to2_out4, *dmap2to1_out4;
char *out_params;
if (argc != 9) {
printf("Usage: %s smoothMaps <edge-image1.bmp> <edge-image2.bmp> "
"<dpyr12> <dpyr21> <dpyr12-out> <dpyr21-out> <params.txt>\n",
argv[0]);
return -1;
}
in_image1 = argv[2];
in_image2 = argv[3];
in_dpyr1to2 = argv[4];
in_dpyr2to1 = argv[5];
out_dpyr1to2 = argv[6];
out_dpyr2to1 = argv[7];
out_params = argv[8];
img_edge1 = img_read_bmp_file(in_image1);
img_edge2 = img_read_bmp_file(in_image2);
dpyr1to2 = img_read_distance_pyramid_file(in_dpyr1to2);
dpyr2to1 = img_read_distance_pyramid_file(in_dpyr2to1);
dmap1to2_out = img_dmap_new(img_edge1->w, img_edge1->h);
dmap2to1_out = img_dmap_new(img_edge2->w, img_edge2->h);
dmap1to2_out2 = img_dmap_new(img_edge1->w, img_edge1->h);
dmap2to1_out2 = img_dmap_new(img_edge2->w, img_edge2->h);
dmap1to2_out3 = img_dmap_new(img_edge1->w, img_edge1->h);
dmap2to1_out3 = img_dmap_new(img_edge2->w, img_edge2->h);
dmap1to2_out4 = img_dmap_new(img_edge1->w, img_edge1->h);
dmap2to1_out4 = img_dmap_new(img_edge2->w, img_edge2->h);
/* Round 1 */
std::vector<int> inliers;
#if 1
inliers = EstimateTransform(&(dpyr1to2->dmaps[0]),
&(dpyr2to1->dmaps[0]),
MotionAffine, 400, 20.0 /*50.0*/, ¶ms,
dmap1to2_out, dmap2to1_out, false);
matrix_print(3, 3, params.M);
printf("[round1] num_inliers = %d\n", inliers.size());
fflush(stdout);
tmp1 = dmap2dpyr(dmap1to2_out);
tmp2 = dmap2dpyr(dmap2to1_out);
img_write_distance_pyramid_file(tmp1, "tmp0_12.pyr");
img_write_distance_pyramid_file(tmp2, "tmp0_21.pyr");
#else
dmap1to2_out = &(dpyr1to2->dmaps[0]);
dmap2to1_out = &(dpyr2to1->dmaps[0]);
#endif
/* ------------- */
#if 0
#if 1
params.num_basis_pts = 64;
params.lambda = 5.0e3;
inliers = EstimateTransform(dmap1to2_out, dmap2to1_out,
MotionThinPlateSpline, 256, 25.0, ¶ms,
dmap1to2_out2, dmap2to1_out2, false);
printf("[round2] num_inliers = %d\n", inliers.size());
fflush(stdout);
tmp1 = dmap2dpyr(dmap1to2_out2);
tmp2 = dmap2dpyr(dmap2to1_out2);
img_write_distance_pyramid_file(tmp1, "tmp1_12.pyr");
img_write_distance_pyramid_file(tmp2, "tmp1_21.pyr");
#else
dmap1to2_out2 = dmap1to2_out;
dmap2to1_out2 = dmap2to1_out;
#endif
#if 1
params.num_basis_pts = 512; // 256; // 512;
params.lambda = 1.0e1;
inliers = EstimateTransform(dmap1to2_out2, dmap2to1_out2,
MotionThinPlateSpline, 64, 5.0, ¶ms,
dmap1to2_out3, dmap2to1_out3, true);
printf("[round3] num_inliers = %d\n", inliers.size());
fflush(stdout);
#endif
#if 1
params.num_basis_pts = 2048;
params.lambda = 5.0e1;
inliers = EstimateTransform(dmap1to2_out3, dmap2to1_out3,
MotionThinPlateSpline, 4, 1.0, ¶ms,
dmap1to2_out4, dmap2to1_out4, true);
printf("[round4] num_inliers = %d\n", inliers.size());
fflush(stdout);
#endif
#endif
/* -------------- */
#if 1
/* Round 2 */
#if 1
params.num_basis_pts = 64;
params.lambda = 5.0e3;
inliers = EstimateTransform(dmap1to2_out, dmap2to1_out,
MotionThinPlateSpline, 256, 15.0, ¶ms,
dmap1to2_out2, dmap2to1_out2, false);
printf("[round2] num_inliers = %d\n", inliers.size());
fflush(stdout);
#endif
#if 1
params.num_basis_pts = 512;
params.lambda = 1.0e3;
inliers = EstimateTransform(dmap1to2_out2, dmap2to1_out2,
MotionThinPlateSpline, 64, 3.0, ¶ms,
dmap1to2_out3, dmap2to1_out3, true);
printf("[round3] num_inliers = %d\n", inliers.size());
fflush(stdout);
#endif
#if 0
params.num_basis_pts = 2048;
params.lambda = 5.0e1;
inliers = EstimateTransform(dmap1to2_out3, dmap2to1_out3,
MotionThinPlateSpline, 4, 1.0, ¶ms,
dmap1to2_out4, dmap2to1_out4, true);
printf("[round4] num_inliers = %d\n", inliers.size());
fflush(stdout);
#else
dmap1to2_out4 = dmap1to2_out3;
dmap2to1_out4 = dmap2to1_out3;
#endif
#endif
/* ------------ */
#if 0
#if 1
params.num_basis_pts = 64;
params.lambda = 1.0e4;
inliers = EstimateTransform(dmap1to2_out, dmap2to1_out,
MotionThinPlateSpline, 256, 32.0, ¶ms,
dmap1to2_out2, dmap2to1_out2, false);
printf("[round2] num_inliers = %d\n", inliers.size());
#endif
#if 1
params.num_basis_pts = 512;
params.lambda = 1.0e3;
inliers = EstimateTransform(dmap1to2_out2, dmap2to1_out2,
MotionThinPlateSpline, 64, 8.0, ¶ms,
dmap1to2_out3, dmap2to1_out3, true);
printf("[round3] num_inliers = %d\n", inliers.size());
#endif
#if 0
params.num_basis_pts = 2048;
params.lambda = 5.0e1;
inliers = EstimateTransform(dmap1to2_out3, dmap2to1_out3,
MotionThinPlateSpline, 4, 1.0, ¶ms,
dmap1to2_out4, dmap2to1_out4, true);
printf("[round4] num_inliers = %d\n", inliers.size());
fflush(stdout);
#else
dmap1to2_out4 = dmap1to2_out3;
dmap2to1_out4 = dmap2to1_out3;
#endif
#endif
dpyr1to2_out = dmap2dpyr(dmap1to2_out4);
dpyr2to1_out = dmap2dpyr(dmap2to1_out4);
img_write_distance_pyramid_file(dpyr1to2_out, out_dpyr1to2);
img_write_distance_pyramid_file(dpyr2to1_out, out_dpyr2to1);
/* Write the parameters */
FILE *f = fopen(out_params, "w");
fprintf(f, "%d\n", (int) params.basis_pts.size());
for (int i = 0; i < (int) params.basis_pts.size(); i++) {
fprintf(f, "%0.3f %0.3f\n",
params.basis_pts[i].x, params.basis_pts[i].y);
}
/* x, y affine */
fprintf(f, "%0.3f %0.3f %0.3f\n",
params.x_affine[0], params.x_affine[1], params.x_affine[2]);
fprintf(f, "%0.3f %0.3f %0.3f\n",
params.y_affine[0], params.y_affine[1], params.y_affine[2]);
/* x, y weights */
for (int i = 0; i < (int) params.basis_pts.size(); i++) {
fprintf(f, "%0.3f %0.3f", params.x_weights[i], params.y_weights[i]);
}
fclose(f);
return 0;
}
