diff options
Diffstat (limited to 'ftnoir_tracker_pt/point_tracker.cpp')
| -rw-r--r-- | ftnoir_tracker_pt/point_tracker.cpp | 160 |
1 files changed, 72 insertions, 88 deletions
diff --git a/ftnoir_tracker_pt/point_tracker.cpp b/ftnoir_tracker_pt/point_tracker.cpp index 8a633c5d..e4c999ad 100644 --- a/ftnoir_tracker_pt/point_tracker.cpp +++ b/ftnoir_tracker_pt/point_tracker.cpp @@ -33,44 +33,6 @@ static void set_row(Matx33f& m, int i, const Vec3f& v) m(i,2) = v[2]; } -PointModel::PointModel() : - M01 { 0, 0, 0 }, - M02 { 0, 0, 0 } -{ -} - -PointModel::PointModel(Vec3f M01, Vec3f M02) - : M01(M01), M02(M02) -{ - // calculate u - u = M01.cross(M02); - u /= norm(u); - - // calculate projection matrix on M01,M02 plane - float s11 = M01.dot(M01); - float s12 = M01.dot(M02); - float s22 = M02.dot(M02); - P = 1.0/(s11*s22-s12*s12) * Matx22f(s22, -s12, - -s12, s11); - // calculate d and d_order for simple freetrack-like point correspondence - vector<Vec2f> points; - points.push_back(Vec2f(0,0)); - points.push_back(Vec2f(M01[0], M01[1])); - points.push_back(Vec2f(M02[0], M02[1])); - // fit line to orthographically projected points - // ERROR: yields wrong results with colinear points?! - /* - Vec4f line; - fitLine(points, line, CV_DIST_L2, 0, 0.01, 0.01); - d[0] = line[0]; d[1] = line[1]; - */ - // TODO: fix this - d = Vec2f(M01[0]-M02[0], M01[1]-M02[1]); - - // sort model points - get_d_order(points, d_order); -} - #ifdef OPENTRACK_API static bool d_vals_sort(const pair<float,int> a, const pair<float,int> b) { @@ -78,10 +40,11 @@ static bool d_vals_sort(const pair<float,int> a, const pair<float,int> b) } #endif -void PointModel::get_d_order(const std::vector<cv::Vec2f>& points, int d_order[]) const +void PointModel::get_d_order(const std::vector<cv::Vec2f>& points, int d_order[], cv::Vec2f d) const { - // get sort indices with respect to d scalar product + // fit line to orthographically projected points vector< pair<float,int> > d_vals; + // get sort indices with respect to d scalar product for (unsigned i = 0; i<points.size(); ++i) d_vals.push_back(pair<float, int>(d.dot(points[i]), i)); @@ -99,69 +62,85 @@ void PointModel::get_d_order(const std::vector<cv::Vec2f>& points, int d_order[] } -// ---------------------------------------------------------------------------- PointTracker::PointTracker() { - X_CM.t[2] = 1000; // default position: 1 m away from cam; } -void PointTracker::reset() +PointTracker::PointOrder PointTracker::find_correspondences_previous(const vector<Vec2f>& points, const PointModel& model, float f) { - // enter init phase - X_CM = FrameTrafo(); -} + PointTracker::PointOrder p; + p.points[0] = project(Vec3f(0,0,0), f); + p.points[1] = project(model.M01, f); + p.points[2] = project(model.M02, f); -void PointTracker::track(const vector<Vec2f>& projected_points, const PointModel& model) -{ - const PointOrder& order = find_correspondences(projected_points, model); - int iters = POSIT(model, order); - qDebug()<<"POSIT iterations:"<<iters; -} - -PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv::Vec2f>& projected_points, const PointModel& model) -{ - // ... otherwise we look at the distance to the projection of the expected model points - // project model points under current pose - Vec2f p_exp[3]; - p_exp[0] = project(Vec3f(0,0,0)); - p_exp[1] = project(model.get_M01()); - p_exp[2] = project(model.get_M02()); - // set correspondences by minimum distance to projected model point bool point_taken[PointModel::N_POINTS]; for (int i=0; i<PointModel::N_POINTS; ++i) - point_taken[i] = false; - - PointOrder p; - for (int i=0; i<PointModel::N_POINTS; ++i) - p.points[i] = Vec2f(0, 0); - + point_taken[i] = false; + for (int i=0; i<PointModel::N_POINTS; ++i) { - float min_sdist = 1e4; - int min_idx = 0; - // find closest point to projected model point i - for (int j=0; j<PointModel::N_POINTS; ++j) - { - Vec2f d = p_exp[i]-projected_points[j]; - float sdist = d.dot(d); - if (sdist < min_sdist) + float min_sdist = 0; + int min_idx = 0; + // find closest point to projected model point i + for (int j=0; j<PointModel::N_POINTS; ++j) { - min_idx = j; - min_sdist = sdist; + Vec2f d = p.points[i]-points[j]; + float sdist = d.dot(d); + if (sdist < min_sdist || j==0) + { + min_idx = j; + min_sdist = sdist; + } } - } - // if one point is closest to more than one model point, abort - if (point_taken[min_idx]) return p; - point_taken[min_idx] = true; - p.points[i] = projected_points[min_idx]; + // if one point is closest to more than one model point, fallback + if (point_taken[min_idx]) + { + return find_correspondences(points, model); + } + point_taken[min_idx] = true; + p.points[i] = points[min_idx]; } return p; } +void PointTracker::track(const vector<Vec2f>& points, const PointModel& model, float f, bool dynamic_pose) +{ + PointOrder order; + + if (!dynamic_pose) + order = find_correspondences(points, model); + else + order = find_correspondences_previous(points, model, f); + + POSIT(model, order, f); +} +PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv::Vec2f>& points, const PointModel& model) +{ + // We do a simple freetrack-like sorting in the init phase... + // sort points + int point_d_order[PointModel::N_POINTS]; + int model_d_order[PointModel::N_POINTS]; + cv::Vec2f d(model.M01[0]-model.M02[0], model.M01[1]-model.M02[1]); + model.get_d_order(points, point_d_order, d); + // calculate d and d_order for simple freetrack-like point correspondence + model.get_d_order(std::vector<cv::Vec2f> { + Vec2f{0,0}, + Vec2f(model.M01[0], model.M01[1]), + Vec2f(model.M02[0], model.M02[1]) + }, + model_d_order, + d); + // set correspondences + PointOrder p; + for (int i=0; i<PointModel::N_POINTS; ++i) + p.points[model_d_order[i]] = points[point_d_order[i]]; + + return p; +} -int PointTracker::POSIT(const PointModel& model, const PointOrder& order_) +int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float focal_length) { // POSIT algorithm for coplanar points as presented in // [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"] @@ -169,13 +148,12 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_) // The expected rotation used for resolving the ambiguity in POSIT: // In every iteration step the rotation closer to R_expected is taken - Matx33f R_expected; - R_expected = X_CM.R; // later we want to be close to the last (predicted) rotation + Matx33f R_expected = Matx33f::eye(); // initial pose = last (predicted) pose Vec3f k; - get_row(R_expected, 2, k); - float Z0 = std::abs(X_CM.t[2]) < 1e-3 ? 1e3 : X_CM.t[2]; + get_row(R_expected, 2, k); + float Z0 = 1000.f; float old_epsilon_1 = 0; float old_epsilon_2 = 0; @@ -287,3 +265,9 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_) // //qDebug()<<"r: "<<r[0]<<' '<<r[1]<<' '<<r[2]<<'\n'; } + +cv::Vec2f PointTracker::project(const cv::Vec3f& v_M, float f) +{ + cv::Vec3f v_C = X_CM * v_M; + return cv::Vec2f(f*v_C[0]/v_C[2], f*v_C[1]/v_C[2]); +} |