1 files changed, 100 insertions, 85 deletions

diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp
index cae68bf3..39e96038 100644
--- a/tracker-pt/point_tracker.cpp
+++ b/tracker-pt/point_tracker.cpp
@@ -6,9 +6,7 @@
  */
 
 #include "point_tracker.h"
-#include "compat/nan.hpp"
-
-using namespace types;
+#include "compat/math-imports.hpp"
 
 #include <vector>
 #include <algorithm>
@@ -16,7 +14,9 @@ using namespace types;
 
 #include <QDebug>
 
-constexpr unsigned PointModel::N_POINTS;
+namespace pt_impl {
+
+using namespace numeric_types;
 
 static void get_row(const mat33& m, int i, vec3& v)
 {
@@ -32,12 +32,12 @@ static void set_row(mat33& m, int i, const vec3& v)
     m(i,2) = v[2];
 }
 
-PointModel::PointModel(settings_pt& s)
+PointModel::PointModel(const pt_settings& s)
 {
     set_model(s);
     // calculate u
     u = M01.cross(M02);
-    u /= norm(u);
+    u = cv::normalize(u);
 
     // calculate projection matrix on M01,M02 plane
     f s11 = M01.dot(M01);
@@ -46,17 +46,20 @@ PointModel::PointModel(settings_pt& s)
     P = 1/(s11*s22-s12*s12) * mat22(s22, -s12, -s12,  s11);
 }
 
-void PointModel::set_model(settings_pt& s)
+void PointModel::set_model(const pt_settings& s)
 {
     switch (s.active_model_panel)
     {
+    default:
+        eval_once(qDebug() << "pt: wrong model type selected");
+        [[fallthrough]];
     case Clip:
-        M01 = vec3(0, static_cast<f>(s.clip_ty), -static_cast<f>(s.clip_tz));
-        M02 = vec3(0, -static_cast<f>(s.clip_by), -static_cast<f>(s.clip_bz));
+        M01 = vec3(0, s.clip_ty, -s.clip_tz);
+        M02 = vec3(0, -s.clip_by, -s.clip_bz);
         break;
     case Cap:
-        M01 = vec3(-static_cast<f>(s.cap_x), -static_cast<f>(s.cap_y), -static_cast<f>(s.cap_z));
-        M02 = vec3(static_cast<f>(s.cap_x), -static_cast<f>(s.cap_y), -static_cast<f>(s.cap_z));
+        M01 = vec3(-s.cap_x, -s.cap_y, -s.cap_z);
+        M02 = vec3(s.cap_x, -s.cap_y, -s.cap_z);
         break;
     case Custom:
         M01 = vec3(s.m01_x, s.m01_y, s.m01_z);
@@ -65,69 +68,62 @@ void PointModel::set_model(settings_pt& s)
     }
 }
 
-void PointModel::get_d_order(const vec2* points, unsigned* d_order, const vec2& d) const
+void PointModel::get_d_order(const vec2* points, unsigned* d_order, const vec2& d)
 {
+    constexpr unsigned cnt = PointModel::N_POINTS;
     // fit line to orthographically projected points
     using t = std::pair<f,unsigned>;
-    t d_vals[3];
+    t d_vals[cnt];
     // get sort indices with respect to d scalar product
-    for (unsigned i = 0; i < PointModel::N_POINTS; ++i)
-        d_vals[i] = t(d.dot(points[i]), i);
+    for (unsigned i = 0; i < cnt; ++i)
+        d_vals[i] = {d.dot(points[i]), i};
 
-    std::sort(d_vals,
-              d_vals + 3u,
-              [](const t& a, const t& b) { return a.first < b.first; });
+    std::sort(std::begin(d_vals), std::end(d_vals),
+              [](t a, t b) { return a.first < b.first; });
 
-    for (unsigned i = 0; i < PointModel::N_POINTS; ++i)
+    for (unsigned i = 0; i < cnt; ++i)
         d_order[i] = d_vals[i].second;
 }
 
 
-PointTracker::PointTracker() : init_phase(true)
-{
-}
+PointTracker::PointTracker() = default;
 
 PointTracker::PointOrder PointTracker::find_correspondences_previous(const vec2* points,
                                                                      const PointModel& model,
-                                                                     const CamInfo& info)
+                                                                     const pt_camera_info& info)
 {
-    f fx; info.get_focal_length(fx);
-    PointTracker::PointOrder p;
-    p[0] = project(vec3(0,0,0), fx);
-    p[1] = project(model.M01, fx);
-    p[2] = project(model.M02, fx);
+    const f fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y);
+    PointTracker::PointOrder p {
+        project(vec3(0,0,0), fx),
+        project(model.M01, fx),
+        project(model.M02, fx)
+    };
 
-    const int diagonal = int(std::sqrt(double(info.res_x*info.res_x + info.res_y*info.res_y)));
-    static constexpr int div = 100;
-    const int max_dist = diagonal / div; // 8 pixels for 640x480
+    constexpr unsigned sz = PointModel::N_POINTS;
 
     // set correspondences by minimum distance to projected model point
-    bool point_taken[PointModel::N_POINTS];
-    for (unsigned i=0; i<PointModel::N_POINTS; ++i)
-        point_taken[i] = false;
+    bool point_taken[sz] {};
 
-    for (unsigned i=0; i<PointModel::N_POINTS; ++i)
+    for (unsigned i=0; i < sz; ++i)
     {
         f min_sdist = 0;
         unsigned min_idx = 0;
         // find closest point to projected model point i
-        for (unsigned j=0; j<PointModel::N_POINTS; ++j)
+        for (unsigned j=0; j < sz; ++j)
         {
             vec2 d = p[i]-points[j];
             f sdist = d.dot(d);
-            if (sdist < min_sdist || j==0)
+            if (sdist < min_sdist || j == 0)
             {
                 min_idx = j;
                 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])
         {
-            init_phase = true;
+            reset_state();
             return find_correspondences(points, model);
         }
         point_taken[min_idx] = true;
@@ -139,55 +135,61 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const vec2*
 
 void PointTracker::track(const std::vector<vec2>& points,
                          const PointModel& model,
-                         const CamInfo& info,
-                         int init_phase_timeout)
+                         const pt_camera_info& info,
+                         int init_phase_timeout,
+                         point_filter& filter,
+                         f deadzone_amount)
 {
-    f fx;
-    info.get_focal_length(fx);
+    const f fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y);
     PointOrder order;
 
-    if (init_phase_timeout > 0 && t.elapsed_ms() > init_phase_timeout)
+    if (init_phase || init_phase_timeout <= 0 || t.elapsed_ms() > init_phase_timeout)
     {
-        t.start();
-        init_phase = true;
-    }
-
-    if (!(init_phase_timeout > 0 && !init_phase))
+        reset_state();
         order = find_correspondences(points.data(), model);
+    }
     else
         order = find_correspondences_previous(points.data(), model, info);
 
-    if (POSIT(model, order, fx) != -1)
+    if (POSIT(model, filter(order, deadzone_amount), fx) != -1)
     {
         init_phase = false;
         t.start();
     }
+    else
+        reset_state();
 }
 
 PointTracker::PointOrder PointTracker::find_correspondences(const vec2* points, const PointModel& model)
 {
-    static const Affine a(mat33::eye(), vec3(0, 0, 1));
+    constexpr unsigned cnt = PointModel::N_POINTS;
     // We do a simple freetrack-like sorting in the init phase...
-    unsigned point_d_order[PointModel::N_POINTS];
-    unsigned model_d_order[PointModel::N_POINTS];
-    // sort points
+    unsigned point_d_order[cnt];
+    unsigned model_d_order[cnt];
+    // calculate d and d_order for simple freetrack-like point correspondence
     vec2 d(model.M01[0]-model.M02[0], model.M01[1]-model.M02[1]);
+    // sort points
     model.get_d_order(points, point_d_order, d);
-    // calculate d and d_order for simple freetrack-like point correspondence
-    vec2 pts[3] = {
-        vec2(0, 0),
-        vec2(model.M01[0], model.M01[1]),
-        vec2(model.M02[0], model.M02[1])
+    vec2 pts[cnt] {
+        { 0, 0 },
+        { model.M01[0], model.M01[1] },
+        { model.M02[0], model.M02[1] },
     };
     model.get_d_order(pts, model_d_order, d);
+
     // set correspondences
     PointOrder p;
-    for (unsigned i = 0; i < PointModel::N_POINTS; ++i)
+    for (unsigned i = 0; i < cnt; ++i)
         p[model_d_order[i]] = points[point_d_order[i]];
 
     return p;
 }
 
+#ifdef __clang__
+#   pragma clang diagnostic push
+#   pragma clang diagnostic ignored "-Wfloat-equal"
+#endif
+
 int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f focal_length)
 {
     // POSIT algorithm for coplanar points as presented in
@@ -196,17 +198,16 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca
 
     // The expected rotation used for resolving the ambiguity in POSIT:
     // In every iteration step the rotation closer to R_expected is taken
-    static const mat33 R_expected(mat33::eye());
+    const mat33& R_expected{X_CM_expected.R};
 
     // initial pose = last (predicted) pose
     vec3 k;
     get_row(R_expected, 2, k);
-    f Z0 = f(1000);
+    f Z0 = X_CM.t[2] < f(1e-4) ? f(1000) : X_CM.t[2];
 
     f old_epsilon_1 = 0;
     f old_epsilon_2 = 0;
-    f epsilon_1 = 1;
-    f epsilon_2 = 1;
+    f epsilon_1, epsilon_2;
 
     vec3 I0, J0;
     vec2 I0_coeff, J0_coeff;
@@ -215,16 +216,10 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca
     mat33 R_1, R_2;
     mat33* R_current = &R_1;
 
-    static constexpr int max_iter = 100;
-
-    using std::sqrt;
-    using std::atan;
-    using std::cos;
-    using std::sin;
-    using std::fabs;
+    constexpr int max_iter = 100;
 
-    int i=1;
-    for (; i<max_iter; ++i)
+    int i;
+    for (i = 1; i < max_iter; ++i)
     {
         epsilon_1 = k.dot(model.M01)/Z0;
         epsilon_2 = k.dot(model.M02)/Z0;
@@ -249,14 +244,14 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca
         // CAVEAT don't change to comparison with an epsilon -sh 20160423
         if (JJ0 == II0) {
             rho = sqrt(fabs(2*IJ0));
-            theta = -M_PI/4;
+            theta = -pi/4;
             if (IJ0<0) theta *= -1;
         }
         else {
             rho = sqrt(sqrt( (JJ0-II0)*(JJ0-II0) + 4*IJ0*IJ0 ));
             theta = atan( -2*IJ0 / (JJ0-II0) );
             // avoid branch misprediction
-            theta += (JJ0 - II0 < 0) * M_PI;
+            theta += (JJ0 - II0 < 0) * pi;
             theta *= f(.5);
         }
 
@@ -267,7 +262,7 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca
         J_1 = J0 + rho*sin(theta)*model.u;
         J_2 = J0 - rho*sin(theta)*model.u;
 
-        f norm_const = 1/cv::norm(I_1); // all have the same norm
+        f norm_const = (f)(1/cv::norm(I_1)); // all have the same norm
 
         // create rotation matrices
         I_1 *= norm_const; J_1 *= norm_const;
@@ -286,8 +281,8 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca
 
         // pick the rotation solution closer to the expected one
         // in simple metric d(A,B) = || I - A * B^T ||
-        f R_1_deviation = cv::norm(mat33::eye() - R_expected * R_1.t());
-        f R_2_deviation = cv::norm(mat33::eye() - R_expected * R_2.t());
+        f R_1_deviation = (f)(cv::norm(mat33::eye() - R_expected * R_1.t()));
+        f R_2_deviation = (f)(cv::norm(mat33::eye() - R_expected * R_2.t()));
 
         if (R_1_deviation < R_2_deviation)
             R_current = &R_1;
@@ -299,7 +294,7 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca
         // check for convergence condition
         const f delta = fabs(epsilon_1 - old_epsilon_1) + fabs(epsilon_2 - old_epsilon_2);
 
-        if (!(delta > constants::eps))
+        if (delta < eps)
             break;
 
         old_epsilon_1 = epsilon_1;
@@ -315,18 +310,24 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca
 
     for (int i = 0; i < 3; i++)
         for (int j = 0; j < 3; j++)
-            if (nanp(r(i, j)))
+        {
+            int ret = std::fpclassify(r(i, j));
+            if (ret == FP_NAN || ret == FP_INFINITE)
             {
-                qDebug() << "posit nan";
+                qDebug() << "posit nan R";
                 return -1;
             }
+        }
 
-    for (unsigned i = 0; i < 3; i++)
-        if (nanp(t[i]))
+    for (unsigned i = 0; i < 3; i++) // NOLINT(modernize-loop-convert)
+    {
+        int ret = std::fpclassify(t[i]);
+        if (ret == FP_NAN || ret == FP_INFINITE)
         {
-            qDebug() << "posit nan";
+            qDebug() << "posit nan T";
             return -1;
         }
+    }
 
     // apply results
     X_CM.R = r;
@@ -334,11 +335,17 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca
     X_CM.t[1] = t[1];
     X_CM.t[2] = t[2];
 
+    X_CM_expected = X_CM;
+
     //qDebug() << "iter:" << i;
 
     return i;
 }
 
+#ifdef __clang__
+#   pragma clang diagnostic pop
+#endif
+
 vec2 PointTracker::project(const vec3& v_M, f focal_length)
 {
     return project(v_M, focal_length, X_CM);
@@ -349,3 +356,11 @@ vec2 PointTracker::project(const vec3& v_M, f focal_length, const Affine& X_CM)
     vec3 v_C = X_CM * v_M;
     return vec2(focal_length*v_C[0]/v_C[2], focal_length*v_C[1]/v_C[2]);
 }
+
+void PointTracker::reset_state()
+{
+    init_phase = true;
+    X_CM_expected = {};
+}
+
+} // ns pt_impl