pt: use previous pose on NaN result from POSIT

This will reset dynamic pose resolution if the error persists.

2 files changed, 36 insertions, 3 deletions

diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index 11fe64cc..29b32dad 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -66,6 +66,11 @@ bool Tracker_PT::get_focal_length(float& ret)
     return false;
 }
 
+static inline bool nanp(double value)
+{
+    return std::isnan(value) || std::isinf(value);
+}
+
 void Tracker_PT::run()
 {
 #ifdef PT_PERF_LOG
@@ -100,19 +105,43 @@ void Tracker_PT::run()
 
             bool success = points.size() == PointModel::N_POINTS;
 
-            ever_success |= success;
-
             float fx;
             if (!get_focal_length(fx))
                 continue;
 
+            Affine X_CM_ = pose();
+            
             if (success)
             {
                 point_tracker.track(points, PointModel(s), fx, s.dynamic_pose, s.init_phase_timeout);
             }
+            
+            Affine X_CM = pose();
+
+            {
+                int j = 0;
+                
+                for (int i = 0; i < 3; i++)
+                {
+                    if (nanp(X_CM.t(i)))
+                        goto nannan;
+                    for (; j < 3; j++)
+                        if (nanp(X_CM.R(i, j)))
+                        {
+nannan:                     success = false;
+                            X_CM = X_CM_;
+                            {
+                                QMutexLocker lock(&mutex);
+                                point_tracker.reset(X_CM_);
+                            }
+                            goto nannannan;
+                        }
+                }
+            }
+            
+nannannan:  ever_success |= success;
 
             {
-                Affine X_CM = pose();
                 Affine X_MH(cv::Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z)); // just copy pasted these lines from below
                 Affine X_GH = X_CM * X_MH;
                 cv::Vec3f p = X_GH.t; // head (center?) position in global space
diff --git a/ftnoir_tracker_pt/point_tracker.h b/ftnoir_tracker_pt/point_tracker.h
index cdcf2998..fd002948 100644
--- a/ftnoir_tracker_pt/point_tracker.h
+++ b/ftnoir_tracker_pt/point_tracker.h
@@ -118,6 +118,10 @@ public:
     void track(const std::vector<cv::Vec2f>& projected_points, const PointModel& model, float f, bool dynamic_pose, int init_phase_timeout);
     Affine pose() const { return X_CM; }
     cv::Vec2f project(const cv::Vec3f& v_M, float f);
+    void reset(const Affine& pose)
+    {
+        X_CM = pose;
+    }
 private:
     // the points in model order
     struct PointOrder