/* Estimate a transform between two sets of keypoints */ std::vector<int> EstimateTransform(const std::vector<Keypoint> &k1, const std::vector<Keypoint> &k2, const std::vector<KeypointMatch> &matches, MotionModel mm, int nRANSAC, double RANSACthresh, double *Mout) { int min_matches = -1; switch (mm) { case MotionRigid: min_matches = 3; break; case MotionHomography: min_matches = 4; break; } int *match_idxs = new int[min_matches]; int num_matches = (int) matches.size(); int max_inliers = 0; double Mbest[9]; if (num_matches < min_matches) { std::vector<int> empty; printf("Cannot estimate rigid transform\n"); return empty; } v3_t *r_pts = new v3_t[min_matches]; v3_t *l_pts = new v3_t[min_matches]; double *weight = new double[min_matches]; for (int round = 0; round < nRANSAC; round++) { for (int i = 0; i < min_matches; i++) { bool found; int idx; do { found = true; idx = rand() % num_matches; for (int j = 0; j < i; j++) { if (match_idxs[j] == idx) { found = false; break; } } } while (!found); match_idxs[i] = idx; } /* Solve for the motion */ for (int i = 0; i < min_matches; i++) { int idx1 = matches[match_idxs[i]].m_idx1; int idx2 = matches[match_idxs[i]].m_idx2; Vx(l_pts[i]) = k1[idx1].m_x; Vy(l_pts[i]) = k1[idx1].m_y; Vz(l_pts[i]) = 1.0; Vx(r_pts[i]) = k2[idx2].m_x; Vy(r_pts[i]) = k2[idx2].m_y; Vz(r_pts[i]) = 1.0; weight[i] = 1.0; } double Mcurr[9]; switch (mm) { case MotionRigid: { double R[9], T[9], Tout[9], scale; align_horn(min_matches, r_pts, l_pts, R, T, Tout, &scale, weight); memcpy(Mcurr, Tout, 9 * sizeof(double)); break; } case MotionHomography: { align_homography(min_matches, r_pts, l_pts, Mcurr, 0); break; } } std::vector<int> inliers; int num_inliers = CountInliers(k1, k2, matches, Mcurr, RANSACthresh, inliers); if (num_inliers > max_inliers) { max_inliers = num_inliers; memcpy(Mbest, Mcurr, 9 * sizeof(double)); } } std::vector<int> inliers; CountInliers(k1, k2, matches, Mbest, RANSACthresh, inliers); memcpy(Mout, Mbest, 9 * sizeof(double)); LeastSquaresFit(k1, k2, matches, mm, inliers, Mout); // memcpy(Mout, Mbest, 9 * sizeof(double)); delete [] match_idxs; delete [] r_pts; delete [] l_pts; delete [] weight; return inliers; }
bool Tracker::ProcessFrame( std::shared_ptr<CameraInterface<double>> cam, const unsigned char* I, size_t w, size_t h, size_t pitch) { double rms = 0; imgs.Process(I, w, h, pitch ); conic_finder.Find(imgs); const std::vector<Conic, Eigen::aligned_allocator<Conic> >& conics = conic_finder.Conics(); // Generate map and point structures conics_target_map.clear(); conics_target_map.resize(conics.size(),-1); vector<Vector2d, aligned_allocator<Vector2d> > ellipses; for( size_t i=0; i < conics.size(); ++i ) { ellipses.push_back(Vector2d(conics[i].center.x(),conics[i].center.y())); } // Undistort Conics vector<Conic, Eigen::aligned_allocator<Conic> > conics_camframe; for( unsigned int i=0; i<conics.size(); ++i ) { conics_camframe.push_back(UnmapConic(conics[i],cam)); } // Find target given (approximately) undistorted conics std::shared_ptr<CameraInterface<double>> idcam(new LinearCamera<double>()); target.FindTarget( idcam, imgs, conics_camframe, conics_target_map ); conics_candidate_map_first_pass = conics_target_map; int inliers = CountInliers(conics_candidate_map_first_pass); if (inliers<params.inlier_num_required) { printf("1) inliers(%d)<params.inlier_num_required(%d)\n", inliers, params.inlier_num_required ); return false; } conics_target_map = PosePnPRansac( cam, ellipses, target.Circles3D(), conics_candidate_map_first_pass, params.robust_3pt_its, params.robust_3pt_inlier_tol, &T_hw ); rms = ReprojectionErrorRMS(cam, T_hw, target.Circles3D(), ellipses, conics_target_map); target.FindTarget( T_hw, cam, imgs, conics, conics_target_map); conics_candidate_map_second_pass = conics_target_map; inliers = CountInliers(conics_candidate_map_second_pass); if (inliers<params.inlier_num_required){ printf("2) inliers<params.inlier_num_required\n"); return false; } conics_target_map = PosePnPRansac( cam, ellipses, target.Circles3D(), conics_candidate_map_second_pass, params.robust_3pt_its, params.robust_3pt_inlier_tol, &T_hw ); rms = ReprojectionErrorRMS(cam, T_hw, target.Circles3D(), ellipses, conics_target_map); inliers = CountInliers(conics_target_map); if( isfinite((double)rms) && rms < params.max_rms && inliers>=params.inlier_num_required) { T_gw = T_hw; return true; } printf("Failed: if( isfinite((double)rms) && rms < params.max_rms && inliers>=params.inlier_num_required) {\n"); return false; }
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; }