tracker/pt: declare floating-point type size in one place

We want double precision for POSIT. It's best for the type to be set in
ope place without the need to go over everything while switching it back
and forth during tests.

Machine epsilon for float is very small as per
<https://en.wikipedia.org/wiki/Machine_epsilon>. Also see the absurdly
high epsilon of 1e-4 of POSIT that we've had. With floats, making the
epsilon lower resulted in change deltas flushing to zero. This typically
led to the translation Z value being very unstable in PT.

After the epsilon and data type size changes the Z value is stable.

1 files changed, 96 insertions, 56 deletions

diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp
index 51f10470..4c1e177f 100644
--- a/tracker-pt/point_tracker.cpp
+++ b/tracker-pt/point_tracker.cpp
@@ -13,32 +13,69 @@
 
 #include <QDebug>
 
-static void get_row(const cv::Matx33f& m, int i, cv::Vec3f& v)
+using mat33 = pt_types::mat33;
+using vec3 = pt_types::vec3;
+using f = pt_types::f;
+
+static void get_row(const mat33& m, int i, vec3& v)
 {
     v[0] = m(i,0);
     v[1] = m(i,1);
     v[2] = m(i,2);
 }
 
-static void set_row(cv::Matx33f& m, int i, const cv::Vec3f& v)
+static void set_row(mat33& m, int i, const vec3& v)
 {
     m(i,0) = v[0];
     m(i,1) = v[1];
     m(i,2) = v[2];
 }
 
-static bool d_vals_sort(const std::pair<float,int> a, const std::pair<float,int> b)
+static bool d_vals_sort(const std::pair<f,int> a, const std::pair<f,int> b)
 {
     return a.first < b.first;
 }
 
-void PointModel::get_d_order(const std::vector<cv::Vec2f>& points, int d_order[], cv::Vec2f d) const
+PointModel::PointModel(settings_pt& s)
+{
+    set_model(s);
+    // calculate u
+    u = M01.cross(M02);
+    u /= norm(u);
+
+    // calculate projection matrix on M01,M02 plane
+    f s11 = M01.dot(M01);
+    f s12 = M01.dot(M02);
+    f s22 = M02.dot(M02);
+    P = 1/(s11*s22-s12*s12) * mat22(s22, -s12, -s12,  s11);
+}
+
+void PointModel::set_model(settings_pt& s)
+{
+    switch (s.active_model_panel)
+    {
+    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));
+        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));
+        break;
+    case Custom:
+        M01 = vec3(s.m01_x, s.m01_y, s.m01_z);
+        M02 = vec3(s.m02_x, s.m02_y, s.m02_z);
+        break;
+    }
+}
+
+void PointModel::get_d_order(const std::vector<vec2>& points, int* d_order, vec2 d) const
 {
     // fit line to orthographically projected points
-    std::vector<std::pair<float,int>> d_vals;
+    std::vector<std::pair<f,int>> d_vals;
     // get sort indices with respect to d scalar product
     for (unsigned i = 0; i < PointModel::N_POINTS; ++i)
-        d_vals.push_back(std::pair<float, int>(d.dot(points[i]), i));
+        d_vals.push_back(std::pair<f, int>(d.dot(points[i]), i));
 
     std::sort(d_vals.begin(),
               d_vals.end(),
@@ -54,12 +91,12 @@ PointTracker::PointTracker() : init_phase(true)
 {
 }
 
-PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::vector<cv::Vec2f>& points, const PointModel& model, float f)
+PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::vector<vec2>& points, const PointModel& model, f focal_length)
 {
     PointTracker::PointOrder p;
-    p.points[0] = project(cv::Vec3f(0,0,0), f);
-    p.points[1] = project(model.M01, f);
-    p.points[2] = project(model.M02, f);
+    p.points[0] = project(vec3(0,0,0), focal_length);
+    p.points[1] = project(model.M01, focal_length);
+    p.points[2] = project(model.M02, focal_length);
 
     // set correspondences by minimum distance to projected model point
     bool point_taken[PointModel::N_POINTS];
@@ -68,13 +105,13 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::
 
     for (unsigned i=0; i<PointModel::N_POINTS; ++i)
     {
-        float min_sdist = 0;
+        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)
         {
-            cv::Vec2f d = p.points[i]-points[j];
-            float sdist = d.dot(d);
+            vec2 d = p.points[i]-points[j];
+            f sdist = d.dot(d);
             if (sdist < min_sdist || j==0)
             {
                 min_idx = j;
@@ -93,7 +130,7 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::
     return p;
 }
 
-void PointTracker::track(const std::vector<cv::Vec2f>& points, const PointModel& model, float f, bool dynamic_pose, int init_phase_timeout)
+void PointTracker::track(const std::vector<vec2>& points, const PointModel& model, f focal_length, bool dynamic_pose, int init_phase_timeout)
 {
     PointOrder order;
 
@@ -106,26 +143,26 @@ void PointTracker::track(const std::vector<cv::Vec2f>& points, const PointModel&
     if (!dynamic_pose || init_phase)
         order = find_correspondences(points, model);
     else
-        order = find_correspondences_previous(points, model, f);
+        order = find_correspondences_previous(points, model, focal_length);
 
-    POSIT(model, order, f);
+    POSIT(model, order, focal_length);
     init_phase = false;
     t.start();
 }
 
-PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv::Vec2f>& points, const PointModel& model)
+PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<vec2>& points, const PointModel& model)
 {
     // We do a simple freetrack-like sorting in the init phase...
     // sort points
     int point_d_order[PointModel::N_POINTS];
     int model_d_order[PointModel::N_POINTS];
-    cv::Vec2f d(model.M01[0]-model.M02[0], model.M01[1]-model.M02[1]);
+    vec2 d(model.M01[0]-model.M02[0], model.M01[1]-model.M02[1]);
     model.get_d_order(points, point_d_order, d);
     // calculate d and d_order for simple freetrack-like point correspondence
-    model.get_d_order(std::vector<cv::Vec2f> {
-                          cv::Vec2f{0,0},
-                          cv::Vec2f(model.M01[0], model.M01[1]),
-                          cv::Vec2f(model.M02[0], model.M02[1])
+    model.get_d_order(std::vector<vec2> {
+                          vec2{0,0},
+                          vec2(model.M01[0], model.M01[1]),
+                          vec2(model.M02[0], model.M02[1])
                       },
                       model_d_order,
                       d);
@@ -137,7 +174,7 @@ PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv
     return p;
 }
 
-int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float focal_length)
+int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, f focal_length)
 {
     // POSIT algorithm for coplanar points as presented in
     // [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"]
@@ -145,45 +182,45 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
 
     // The expected rotation used for resolving the ambiguity in POSIT:
     // In every iteration step the rotation closer to R_expected is taken
-    cv::Matx33f R_expected = cv::Matx33f::eye();
+    mat33 R_expected = mat33::eye();
 
     // initial pose = last (predicted) pose
-    cv::Vec3f k;
+    vec3 k;
     get_row(R_expected, 2, k);
-    float Z0 = 1000.f;
+    f Z0 = f(1000);
 
-    float old_epsilon_1 = 0;
-    float old_epsilon_2 = 0;
-    float epsilon_1 = 1;
-    float epsilon_2 = 1;
+    f old_epsilon_1 = 0;
+    f old_epsilon_2 = 0;
+    f epsilon_1 = 1;
+    f epsilon_2 = 1;
 
-    cv::Vec3f I0, J0;
-    cv::Vec2f I0_coeff, J0_coeff;
+    vec3 I0, J0;
+    vec2 I0_coeff, J0_coeff;
 
-    cv::Vec3f I_1, J_1, I_2, J_2;
-    cv::Matx33f R_1, R_2;
-    cv::Matx33f* R_current;
+    vec3 I_1, J_1, I_2, J_2;
+    mat33 R_1, R_2;
+    mat33* R_current = &R_1;
 
-    constexpr int MAX_ITER = 100;
-    const float EPS_THRESHOLD = 1e-4f;
+    static constexpr int max_iter = 100;
 
-    const cv::Vec2f* order = order_.points;
+    const vec2* order = order_.points;
 
     using std::sqrt;
     using std::atan;
     using std::cos;
     using std::sin;
+    using std::fabs;
 
     int i=1;
-    for (; i<MAX_ITER; ++i)
+    for (; i<max_iter; ++i)
     {
         epsilon_1 = k.dot(model.M01)/Z0;
         epsilon_2 = k.dot(model.M02)/Z0;
 
         // vector of scalar products <I0, M0i> and <J0, M0i>
-        cv::Vec2f I0_M0i(order[1][0]*(1 + epsilon_1) - order[0][0],
+        vec2 I0_M0i(order[1][0]*(1 + epsilon_1) - order[0][0],
                 order[2][0]*(1 + epsilon_2) - order[0][0]);
-        cv::Vec2f J0_M0i(order[1][1]*(1 + epsilon_1) - order[0][1],
+        vec2 J0_M0i(order[1][1]*(1 + epsilon_1) - order[0][1],
                 order[2][1]*(1 + epsilon_2) - order[0][1]);
 
         // construct projection of I, J onto M0i plane: I0 and J0
@@ -193,22 +230,22 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
         J0 = J0_coeff[0]*model.M01 + J0_coeff[1]*model.M02;
 
         // calculate u component of I, J
-        float II0 = I0.dot(I0);
-        float IJ0 = I0.dot(J0);
-        float JJ0 = J0.dot(J0);
-        float rho, theta;
+        f II0 = I0.dot(I0);
+        f IJ0 = I0.dot(J0);
+        f JJ0 = J0.dot(J0);
+        f rho, theta;
         // CAVEAT don't change to comparison with an epsilon -sh 20160423
         if (JJ0 == II0) {
-            rho = std::sqrt(std::abs(2*IJ0));
-            theta = -PI/4;
+            rho = sqrt(fabs(2*IJ0));
+            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) * PI;
-            theta /= 2;
+            theta += (JJ0 - II0 < 0) * pi;
+            theta *= f(.5);
         }
 
         // construct the two solutions
@@ -218,7 +255,7 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
         J_1 = J0 + rho*sin(theta)*model.u;
         J_2 = J0 - rho*sin(theta)*model.u;
 
-        float norm_const = 1/cv::norm(I_1); // all have the same norm
+        f norm_const = 1/cv::norm(I_1); // all have the same norm
 
         // create rotation matrices
         I_1 *= norm_const; J_1 *= norm_const;
@@ -237,8 +274,8 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
 
         // pick the rotation solution closer to the expected one
         // in simple metric d(A,B) = || I - A * B^T ||
-        float R_1_deviation = cv::norm(cv::Matx33f::eye() - R_expected * R_1.t());
-        float R_2_deviation = cv::norm(cv::Matx33f::eye() - R_expected * R_2.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());
 
         if (R_1_deviation < R_2_deviation)
             R_current = &R_1;
@@ -248,8 +285,11 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
         get_row(*R_current, 2, k);
 
         // check for convergence condition
-        if (std::abs(epsilon_1 - old_epsilon_1) + std::abs(epsilon_2 - old_epsilon_2) < EPS_THRESHOLD)
+        const f delta = fabs(epsilon_1 - old_epsilon_1) + fabs(epsilon_2 - old_epsilon_2);
+
+        if (!(delta > eps))
             break;
+
         old_epsilon_1 = epsilon_1;
         old_epsilon_2 = epsilon_2;
     }
@@ -266,8 +306,8 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
     return i;
 }
 
-cv::Vec2f PointTracker::project(const cv::Vec3f& v_M, float f)
+pt_types::vec2 PointTracker::project(const vec3& v_M, f focal_length)
 {
-    cv::Vec3f v_C = X_CM * v_M;
-    return cv::Vec2f(f*v_C[0]/v_C[2], f*v_C[1]/v_C[2]);
+    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]);
 }