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