tracker/pt: use doubles

We don't have pt_types namespace on this branch so hardcode
using double instead.

1 files changed, 55 insertions, 51 deletions

diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp
index 599ce2d3..bc5bf3cd 100644
--- a/tracker-pt/point_tracker.cpp
+++ b/tracker-pt/point_tracker.cpp
@@ -15,32 +15,32 @@
 
 const float PI = 3.14159265358979323846f;
 
-static void get_row(const cv::Matx33f& m, int i, cv::Vec3f& v)
+static void get_row(const cv::Matx33d& m, int i, cv::Vec3d& 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(cv::Matx33d& m, int i, const cv::Vec3d& 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<double,int> a, const std::pair<double,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
+void PointModel::get_d_order(const std::vector<cv::Vec2d>& points, int* d_order, const cv::Vec2d& d) const
 {
     // fit line to orthographically projected points
-    std::vector<std::pair<float,int>> d_vals;
+    std::vector<std::pair<double,int>> d_vals;
     // get sort indices with respect to d scalar product
     for (unsigned i = 0; i<points.size(); ++i)
-        d_vals.push_back(std::pair<float, int>(d.dot(points[i]), i));
+        d_vals.push_back(std::pair<double, int>(d.dot(points[i]), i));
 
     std::sort(d_vals.begin(),
               d_vals.end(),
@@ -70,13 +70,13 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::
 
     for (int i=0; i<PointModel::N_POINTS; ++i)
     {
-        float min_sdist = 0;
-        int min_idx = 0;
+        double min_sdist = 0;
+        unsigned min_idx = 0;
         // find closest point to projected model point i
         for (int j=0; j<PointModel::N_POINTS; ++j)
         {
-            cv::Vec2f d = p.points[i]-points[j];
-            float sdist = d.dot(d);
+            cv::Vec2d d = p.points[i]-points[j];
+            double sdist = d.dot(d);
             if (sdist < min_sdist || j==0)
             {
                 min_idx = j;
@@ -115,19 +115,19 @@ void PointTracker::track(const std::vector<cv::Vec2f>& points, const PointModel&
     t.start();
 }
 
-PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv::Vec2f>& points, const PointModel& model)
+PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv::Vec2d>& 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]);
+    cv::Vec2d 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<cv::Vec2d> {
+                          cv::Vec2d{0,0},
+                          cv::Vec2d(model.M01[0], model.M01[1]),
+                          cv::Vec2d(model.M02[0], model.M02[1])
                       },
                       model_d_order,
                       d);
@@ -140,6 +140,7 @@ PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv
 }
 
 int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float focal_length)
+bool PointTracker::POSIT(const PointModel& model, const PointOrder& order_, double 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"]
@@ -147,29 +148,29 @@ 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();
+    cv::Matx33d R_expected = cv::Matx33d::eye();
 
     // initial pose = last (predicted) pose
-    cv::Vec3f k;
+    cv::Vec3d k;
     get_row(R_expected, 2, k);
-    float Z0 = 1000.f;
 
-    float old_epsilon_1 = 0;
-    float old_epsilon_2 = 0;
-    float epsilon_1 = 1;
-    float epsilon_2 = 1;
+    double Z0 = 1000;
+    double old_epsilon_1 = 0;
+    double old_epsilon_2 = 0;
+    double epsilon_1 = 1;
+    double epsilon_2 = 1;
 
-    cv::Vec3f I0, J0;
-    cv::Vec2f I0_coeff, J0_coeff;
+    cv::Vec3d I0, J0;
+    cv::Vec2d I0_coeff, J0_coeff;
 
-    cv::Vec3f I_1, J_1, I_2, J_2;
-    cv::Matx33f R_1, R_2;
-    cv::Matx33f* R_current;
+    cv::Vec3d I_1, J_1, I_2, J_2;
+    cv::Matx33d R_1, R_2;
+    cv::Matx33d& R_current = R_1;
 
-    const int MAX_ITER = 100;
-    const float EPS_THRESHOLD = 1e-4;
+    const int MAX_ITER = 500;
+    static constexpr double eps = 1e-6;
 
-    const cv::Vec2f* order = order_.points;
+    const cv::Vec2d* order = order_.points;
 
     int i=1;
     for (; i<MAX_ITER; ++i)
@@ -178,9 +179,9 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
         epsilon_2 = k.dot(model.M02)/Z0;
 
         // vector of scalar products <I0, M0i> and <J0, M0i>
-        cv::Vec2f I0_M0i(order[1][0]*(1.0 + epsilon_1) - order[0][0],
+        cv::Vec2d I0_M0i(order[1][0]*(1.0 + epsilon_1) - order[0][0],
                 order[2][0]*(1.0 + epsilon_2) - order[0][0]);
-        cv::Vec2f J0_M0i(order[1][1]*(1.0 + epsilon_1) - order[0][1],
+        cv::Vec2d J0_M0i(order[1][1]*(1.0 + epsilon_1) - order[0][1],
                 order[2][1]*(1.0 + epsilon_2) - order[0][1]);
 
         // construct projection of I, J onto M0i plane: I0 and J0
@@ -190,20 +191,20 @@ 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;
+        double II0 = I0.dot(I0);
+        double IJ0 = I0.dot(J0);
+        double JJ0 = J0.dot(J0);
+        double rho, theta;
         if (JJ0 == II0) {
-            rho = std::sqrt(std::abs(2*IJ0));
-            theta = -PI/4;
+            rho = std::sqrt(std::fabs(2*IJ0));
+            theta = -M_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 += (JJ0 - II0 < 0) * M_PI;
             theta /= 2;
         }
 
@@ -214,7 +215,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.0/cv::norm(I_1); // all have the same norm
+        double norm_const = 1/cv::norm(I_1); // all have the same norm
 
         // create rotation matrices
         I_1 *= norm_const; J_1 *= norm_const;
@@ -233,26 +234,29 @@ 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());
+        double R_1_deviation = cv::norm(cv::Matx33d::eye() - R_expected * R_1.t());
+        double R_2_deviation = cv::norm(cv::Matx33d::eye() - R_expected * R_2.t());
 
         if (R_1_deviation < R_2_deviation)
-            R_current = &R_1;
+            R_current = R_1;
         else
-            R_current = &R_2;
+            R_current = R_2;
 
-        get_row(*R_current, 2, k);
+        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 double 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;
     }
 
     QMutexLocker l(&mtx);
     // apply results
-    X_CM.R = *R_current;
+    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;
@@ -262,8 +266,8 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
     return i;
 }
 
-cv::Vec2f PointTracker::project(const cv::Vec3f& v_M, float f)
+cv::Vec2d PointTracker::project(const cv::Vec3d& v_M, double f)
 {
-    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]);
+    cv::Vec3d v_C = X_CM * v_M;
+    return cv::Vec2d(f*v_C[0]/v_C[2], f*v_C[1]/v_C[2]);
 }