tracker/pt: use max distance for dynamic pose

3 files changed, 34 insertions, 6 deletions

diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp
index 374120f4..49159ac8 100644
--- a/tracker-pt/ftnoir_tracker_pt.cpp
+++ b/tracker-pt/ftnoir_tracker_pt.cpp
@@ -103,6 +103,11 @@ void Tracker_PT::run()
 
         if (new_frame && !frame_.empty())
         {
+            CamInfo cam_info;
+
+            if (!camera.get_info(cam_info))
+                continue;
+
             point_extractor.extract_points(frame_, points);
             point_count = points.size();
 
@@ -115,7 +120,13 @@ void Tracker_PT::run()
 
             if (success)
             {
-                point_tracker.track(points, PointModel(s), fx, s.dynamic_pose, s.init_phase_timeout);
+                point_tracker.track(points,
+                                    PointModel(s),
+                                    fx,
+                                    s.dynamic_pose,
+                                    s.init_phase_timeout,
+                                    cam_info.res_x,
+                                    cam_info.res_y);
                 ever_success = true;
             }
 
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);
diff --git a/tracker-pt/point_tracker.h b/tracker-pt/point_tracker.h
index fab0e9ac..00e371f5 100644
--- a/tracker-pt/point_tracker.h
+++ b/tracker-pt/point_tracker.h
@@ -80,7 +80,7 @@ public:
     // track the pose using the set of normalized point coordinates (x pos in range -0.5:0.5)
     // f : (focal length)/(sensor width)
     // dt : time since last call
-    void track(const std::vector<vec2>& projected_points, const PointModel& model, f focal_length, bool dynamic_pose, int init_phase_timeout);
+    void track(const std::vector<vec2>& projected_points, const PointModel& model, f focal_length, bool dynamic_pose, int init_phase_timeout, int w, int h);
     Affine pose() { return X_CM; }
     vec2 project(const vec3& v_M, PointTracker::f focal_length);
 
@@ -97,7 +97,7 @@ private:
     };
 
     PointOrder find_correspondences(const std::vector<vec2>& projected_points, const PointModel &model);
-    PointOrder find_correspondences_previous(const std::vector<vec2>& points, const PointModel &model, f focal_length);
+    PointOrder find_correspondences_previous(const std::vector<vec2>& points, const PointModel &model, f focal_length, int w, int h);
     int POSIT(const PointModel& point_model, const PointOrder& order, f focal_length);  // The POSIT algorithm, returns the number of iterations
 
     Affine X_CM; // trafo from model to camera