diff options
Diffstat (limited to 'tracker-pt/point_tracker.cpp')
| -rw-r--r-- | tracker-pt/point_tracker.cpp | 23 |
1 files changed, 20 insertions, 3 deletions
diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp index 35a78777..d5948c63 100644 --- a/tracker-pt/point_tracker.cpp +++ b/tracker-pt/point_tracker.cpp @@ -92,13 +92,21 @@ PointTracker::PointTracker() : init_phase(true) { } -PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::vector<vec2>& points, const PointModel& model, f focal_length) +PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::vector<vec2>& points, + const PointModel& model, + f focal_length, + int w, + int h) { PointTracker::PointOrder p; p.points[0] = project(vec3(0,0,0), focal_length); p.points[1] = project(model.M01, focal_length); p.points[2] = project(model.M02, focal_length); + const int diagonal = int(std::sqrt(w*w + h*h)); + constexpr int div = 50; + const int max_dist = diagonal / div; // 16 pixels for 640x480 + // set correspondences by minimum distance to projected model point bool point_taken[PointModel::N_POINTS]; for (unsigned i=0; i<PointModel::N_POINTS; ++i) @@ -119,6 +127,9 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const std:: min_sdist = sdist; } } + if (min_sdist > max_dist) + return find_correspondences(points, model); + // if one point is closest to more than one model point, fallback if (point_taken[min_idx]) { @@ -132,7 +143,13 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const std:: return p; } -void PointTracker::track(const std::vector<vec2>& points, const PointModel& model, f focal_length, bool dynamic_pose, int init_phase_timeout) +void PointTracker::track(const std::vector<vec2>& points, + const PointModel& model, + f focal_length, + bool dynamic_pose, + int init_phase_timeout, + int w, + int h) { PointOrder order; @@ -146,7 +163,7 @@ void PointTracker::track(const std::vector<vec2>& points, const PointModel& mode order = find_correspondences(points, model); else { - order = find_correspondences_previous(points, model, focal_length); + order = find_correspondences_previous(points, model, focal_length, w, h); } POSIT(model, order, focal_length); |