1 files changed, 79 insertions, 131 deletions

diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp
index 6116bec5..39e96038 100644
--- a/tracker-pt/point_tracker.cpp
+++ b/tracker-pt/point_tracker.cpp
@@ -8,14 +8,16 @@
 #include "point_tracker.h"
 #include "compat/math-imports.hpp"
 
-using namespace types;
-
 #include <vector>
 #include <algorithm>
 #include <cmath>
 
 #include <QDebug>
 
+namespace pt_impl {
+
+using namespace numeric_types;
+
 static void get_row(const mat33& m, int i, vec3& v)
 {
     v[0] = m(i,0);
@@ -48,13 +50,16 @@ 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);
@@ -63,69 +68,62 @@ void PointModel::set_model(const pt_settings& 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()
-{
-}
+PointTracker::PointTracker() = default;
 
 PointTracker::PointOrder PointTracker::find_correspondences_previous(const vec2* points,
                                                                      const PointModel& model,
                                                                      const pt_camera_info& info)
 {
-    const double fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y);
-    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(f(info.res_x*info.res_x + info.res_y*info.res_y)));
-    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;
@@ -135,119 +133,63 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const vec2*
     return p;
 }
 
-bool PointTracker::maybe_use_old_point_order(const PointOrder& order, const pt_camera_info& info)
-{
-    constexpr f std_width = 640, std_height = 480;
-
-    PointOrder scaled_order;
-
-    const f cx = std_width / info.res_x;
-    const f cy = std_height / info.res_y;
-
-    for (unsigned k = 0; k < 3; k++)
-    {
-        // note, the .y component is actually scaled by width
-        scaled_order[k][0] = std_width * cx * order[k][0];
-        scaled_order[k][1] = std_width * cy * order[k][1];
-    }
-
-    f sum = 0;
-
-    for (unsigned k = 0; k < 3; k++)
-    {
-        vec2 tmp = prev_scaled_order[k] - scaled_order[k];
-        sum += std::sqrt(tmp.dot(tmp));
-    }
-
-    // CAVEAT don't increase too much, it visibly loses precision
-    constexpr f max_dist = f(.13);
-
-    const bool validp = sum < max_dist;
-
-    prev_order_valid &= validp;
-
-    if (!prev_order_valid)
-    {
-        prev_order = order;
-        prev_scaled_order = scaled_order;
-    }
-
-#if 0
-    {
-        static Timer tt;
-        static int cnt1 = 0, cnt2 = 0;
-        if (tt.elapsed_ms() >= 1000)
-        {
-            tt.start();
-            if (cnt1 + cnt2)
-            {
-                qDebug() << "old-order" << ((cnt1 * 100) / f(cnt1 + cnt2)) << "nsamples" << (cnt1 + cnt2);
-                cnt1 = 0, cnt2 = 0;
-            }
-        }
-        if (validp)
-            cnt1++;
-        else
-            cnt2++;
-    }
-#endif
-
-    prev_order_valid = validp;
-
-    return validp;
-}
-
 void PointTracker::track(const std::vector<vec2>& points,
                          const PointModel& model,
                          const pt_camera_info& info,
-                         int init_phase_timeout)
+                         int init_phase_timeout,
+                         point_filter& filter,
+                         f deadzone_amount)
 {
-    const double fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y);
+    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 (maybe_use_old_point_order(order, info) ||
-        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
@@ -256,7 +198,7 @@ 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(X_CM.R);
+    const mat33& R_expected{X_CM_expected.R};
 
     // initial pose = last (predicted) pose
     vec3 k;
@@ -265,8 +207,7 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca
 
     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;
@@ -277,8 +218,8 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca
 
     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;
@@ -303,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);
         }
 
@@ -321,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;
@@ -340,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;
@@ -353,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;
@@ -373,17 +314,17 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca
             int ret = std::fpclassify(r(i, j));
             if (ret == FP_NAN || ret == FP_INFINITE)
             {
-                qDebug() << "posit nan -- R";
+                qDebug() << "posit nan R";
                 return -1;
             }
         }
 
-    for (unsigned i = 0; i < 3; 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 -- T";
+            qDebug() << "posit nan T";
             return -1;
         }
     }
@@ -394,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);
@@ -412,7 +359,8 @@ vec2 PointTracker::project(const vec3& v_M, f focal_length, const Affine& X_CM)
 
 void PointTracker::reset_state()
 {
-    prev_order_valid = false;
     init_phase = true;
+    X_CM_expected = {};
 }
 
+} // ns pt_impl