tracker/pt: check for nan before writing pose

3 files changed, 28 insertions, 10 deletions

diff --git a/opentrack-compat/nan.cpp b/opentrack-compat/nan.cpp
index 6c4b1f6d..899b907a 100644
--- a/opentrack-compat/nan.cpp
+++ b/opentrack-compat/nan.cpp
@@ -1,11 +1,12 @@
 #include <cmath>
+#include "export.hpp"
 
 #if defined(__GNUC__)
-extern "C" bool __attribute__ ((noinline)) nanp(double value)
+extern "C" OPENTRACK_COMPAT_EXPORT bool __attribute__ ((noinline)) nanp(double value)
 #elif defined(_WIN32)
-extern "C" __declspec(noinline) bool nanp(double value)
+extern "C" OPENTRACK_COMPAT_EXPORT __declspec(noinline) bool nanp(double value)
 #else
-extern "C" bool nanp(double value)
+extern "C" OPENTRACK_COMPAT_EXPORT bool nanp(double value)
 #endif
 {
     using std::isnan;
diff --git a/opentrack-compat/nan.hpp b/opentrack-compat/nan.hpp
index f9e5cc1e..9926da13 100644
--- a/opentrack-compat/nan.hpp
+++ b/opentrack-compat/nan.hpp
@@ -5,7 +5,7 @@
 #if defined(__GNUC__)
 extern "C" OPENTRACK_COMPAT_EXPORT bool __attribute__ ((noinline)) nanp(double value);
 #elif defined(_WIN32)
-extern "C" __declspec(noinline) OPENTRACK_COMPAT_EXPORT bool nanp(double value);
+extern "C" OPENTRACK_COMPAT_EXPORT __declspec(noinline) bool nanp(double value);
 #else
 extern "C" OPENTRACK_COMPAT_EXPORT bool nanp(double value);
 #endif
diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp
index 4c1e177f..61c54fa4 100644
--- a/tracker-pt/point_tracker.cpp
+++ b/tracker-pt/point_tracker.cpp
@@ -6,6 +6,7 @@
  */
 
 #include "point_tracker.h"
+#include "opentrack-compat/nan.hpp"
 
 #include <vector>
 #include <algorithm>
@@ -145,8 +146,8 @@ void PointTracker::track(const std::vector<vec2>& points, const PointModel& mode
     else
         order = find_correspondences_previous(points, model, focal_length);
 
-    POSIT(model, order, focal_length);
-    init_phase = false;
+    const int iters = POSIT(model, order, focal_length);
+    init_phase = iters != -1;
     t.start();
 }
 
@@ -294,12 +295,28 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, f foc
         old_epsilon_2 = epsilon_2;
     }
 
+    const f t[3] = {
+        order[0][0] * Z0/focal_length,
+        order[0][1] * Z0/focal_length,
+        Z0
+    };
+    const mat33& r = *R_current;
+
+    for (int i = 0; i < 3; i++)
+        for (int j = 0; j < 3; j++)
+            if (nanp(r(i, j)))
+                return -1;
+
+    for (unsigned i = 0; i < 3; i++)
+        if (nanp(t[i]))
+            return -1;
+
     QMutexLocker l(&mtx);
     // apply results
-    X_CM.R = *R_current;
-    X_CM.t[0] = order[0][0] * Z0/focal_length;
-    X_CM.t[1] = order[0][1] * Z0/focal_length;
-    X_CM.t[2] = Z0;
+    X_CM.R = r;
+    X_CM.t[0] = t[0];
+    X_CM.t[1] = t[1];
+    X_CM.t[2] = t[2];
 
     //qDebug() << "iter:" << i;