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