tracker: check for NaN values

Issue: #255

2 files changed, 96 insertions, 32 deletions

diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
index a9e60555..ad10e396 100644
--- a/opentrack/tracker.cpp
+++ b/opentrack/tracker.cpp
@@ -62,6 +62,44 @@ void Tracker::t_compensate(const rmat& rmat, const double* xyz, double* output,
     output[0] = -ret(1);
 }
 
+static inline bool nanp(double value)
+{
+    return std::isnan(value) || std::isinf(value);
+}
+
+static inline double elide_nan(double value, double def)
+{
+    if (nanp(value))
+    {
+        if (nanp(def))
+            return 0;
+        return def;
+    }
+    return value;
+}
+
+static bool is_nan(const dmat<3,3>& r, const dmat<3, 1>& t)
+{
+    for (int i = 0; i < 3; i++)
+        for (int j = 0; j < 3; j++)
+            if (nanp(r(i, j)))
+                return true;
+
+    for (int i = 0; i < 3; i++)
+        if (nanp(t(i)))
+            return true;
+
+    return false;
+}
+
+static bool is_nan(const Pose& value)
+{
+    for (int i = 0; i < 6; i++)
+        if (nanp(value(i)))
+            return true;
+    return false;
+}
+
 void Tracker::logic()
 {
     bool inverts[6] = {
@@ -72,12 +110,12 @@ void Tracker::logic()
         m(4).opts.invert,
         m(5).opts.invert,
     };
-    
+
     static constexpr double pi = 3.141592653;
     static constexpr double r2d = 180. / pi;
-    
+
     Pose value, raw;
-    
+
     for (int i = 0; i < 6; i++)
     {
         auto& axis = m(i);
@@ -89,6 +127,9 @@ void Tracker::logic()
         raw(i) = newpose[i];
     }
 
+    if (is_nan(raw))
+        raw = last_raw;
+
     const double off[] = {
         (double)-s.camera_yaw,
         (double)-s.camera_pitch,
@@ -97,12 +138,13 @@ void Tracker::logic()
     const rmat cam = rmat::euler_to_rmat(off);
     rmat r = rmat::euler_to_rmat(&value[Yaw]);
     dmat<3, 1> t(value(0), value(1), value(2));
-    
+
     r = cam * r;
-    
+
     bool can_center = false;
-    
-    if (centerp)
+    const bool nan = is_nan(r, t);
+
+    if (centerp && !nan)
     {
         for (int i = 0; i < 6; i++)
             if (fabs(newpose[i]) != 0)
@@ -111,7 +153,7 @@ void Tracker::logic()
                 break;
             }
     }
-    
+
     if (can_center)
     {
         if (libs.pFilter)
@@ -121,7 +163,7 @@ void Tracker::logic()
             t_b[i] = t(i);
         r_b = r;
     }
-    
+
     {
         double tmp[3] = { t(0) - t_b[0], t(1) - t_b[1], t(2) - t_b[2] };
         t_compensate(cam, tmp, tmp, false);
@@ -143,35 +185,57 @@ void Tracker::logic()
             value(i+3) = euler(i) * r2d;
         }
     }
-    
+
+    bool nan_ = false;
+    // we're checking NaNs after every block of numeric ops
+    if (is_nan(value))
+    {
+        nan_ = true;
+    }
+    else
     {
         Pose tmp = value;
-        
+
         if (libs.pFilter)
             libs.pFilter->filter(tmp, value);
-    }
 
-    for (int i = 0; i < 6; i++)
-        value(i) = map(value(i), m(i));
-    
-    if (s.tcomp_p)
-        t_compensate(rmat::euler_to_rmat(&value[Yaw]),
-                     value,
-                     value,
-                     s.tcomp_tz);
+        for (int i = 0; i < 6; i++)
+            value(i) = map(value(i), m(i));
 
-    for (int i = 0; i < 6; i++)
-        value(i) += m(i).opts.zero;
+        if (s.tcomp_p)
+            t_compensate(rmat::euler_to_rmat(&value[Yaw]),
+                         value,
+                         value,
+                         s.tcomp_tz);
 
-    for (int i = 0; i < 6; i++)
-        value[i] *= inverts[i] ? -1. : 1.;
+        for (int i = 0; i < 6; i++)
+            value(i) += m(i).opts.zero;
 
-    if (zero_)
         for (int i = 0; i < 6; i++)
-            value(i) = 0;
+            value[i] *= inverts[i] ? -1. : 1.;
+
+        if (zero_)
+            for (int i = 0; i < 6; i++)
+                value(i) = 0;
+
+        if (is_nan(value))
+            nan_ = true;
+    }
+
+    if (nan_)
+    {
+        value = last_mapped;
+
+        // for widget last value display
+        for (int i = 0; i < 6; i++)
+            (void) map(value(i), m(i));
+    }
 
     libs.pProtocol->pose(value);
 
+    last_mapped = value;
+    last_raw = raw;
+
     QMutexLocker foo(&mtx);
     output_pose = value;
     raw_6dof = raw;
@@ -179,7 +243,7 @@ void Tracker::logic()
 
 void Tracker::run() {
     const int sleep_ms = 3;
-    
+
 #if defined(_WIN32)
     (void) timeBeginPeriod(1);
 #endif
@@ -187,14 +251,14 @@ void Tracker::run() {
     while (!should_quit)
     {
         t.start();
-        
+
         double tmp[6] {0,0,0, 0,0,0};
         libs.pTracker->data(tmp);
-        
+
         if (enabledp)
             for (int i = 0; i < 6; i++)
-                newpose[i] = tmp[i];
-        
+                newpose[i] = elide_nan(tmp[i], newpose[i]);
+
         logic();
 
         long q = sleep_ms * 1000L - t.elapsed()/1000L;
diff --git a/opentrack/tracker.h b/opentrack/tracker.h
index c5c39797..b0e89455 100644
--- a/opentrack/tracker.h
+++ b/opentrack/tracker.h
@@ -48,7 +48,7 @@ private:
     Mappings& m;
 
     Timer t;
-    Pose output_pose, raw_6dof;
+    Pose output_pose, raw_6dof, last_mapped, last_raw;
 
     double newpose[6];
     volatile bool centerp;