pt: use 3D coordinates for distinguishing points

Issue: #98

2 files changed, 15 insertions, 12 deletions

diff --git a/ftnoir_tracker_pt/point_tracker.cpp b/ftnoir_tracker_pt/point_tracker.cpp
index afb348c6..640d4528 100644
--- a/ftnoir_tracker_pt/point_tracker.cpp
+++ b/ftnoir_tracker_pt/point_tracker.cpp
@@ -6,6 +6,7 @@
  */
 
 #include "point_tracker.h"
+#include <opencv2/imgproc/imgproc.hpp>
 
 #include <vector>
 #include <algorithm>
@@ -50,8 +51,11 @@ PointModel::PointModel(Vec3f M01, Vec3f M02)
 	float s11 = M01.dot(M01);
 	float s12 = M01.dot(M02);
 	float s22 = M02.dot(M02);
-	P = 1.0/(s11*s22-s12*s12) * Matx22f(s22, -s12, 
-		                               -s12,  s11);
+	P = 1.0/(s11*s22-s12*s12) * Matx22f(s22, -s12, -s12,  s11);
+    
+    cv::Vec6f line;
+    cv::fitLine(std::vector<cv::Point3f>{M01, M02}, line, CV_DIST_L1, 0, 1e-2, 1e-4);
+    d = cv::Vec3f(line[0], line[1], line[3]);
 }
 
 #ifdef OPENTRACK_API
@@ -61,12 +65,12 @@ static bool d_vals_sort(const pair<float,int> a, const pair<float,int> b)
 }
 #endif
 
-void PointModel::get_d_order(const std::vector<cv::Vec2f>& points, int d_order[]) const
+template<typename vec>
+void PointModel::get_d_order(const std::vector<vec>& points, int d_order[], vec d) const
 {
 	// fit line to orthographically projected points
-	Vec2f d(M01[0]-M02[0], M01[1]-M02[1]);
-	// get sort indices with respect to d scalar product
 	vector< pair<float,int> > d_vals;
+    // get sort indices with respect to d scalar product
 	for (unsigned i = 0; i<points.size(); ++i)
 		d_vals.push_back(pair<float, int>(d.dot(points[i]), i));
 
@@ -102,13 +106,9 @@ PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv
     // sort points
     int point_d_order[PointModel::N_POINTS];
     int model_d_order[PointModel::N_POINTS];
-    model.get_d_order(points, point_d_order);
+    model.get_d_order(points, point_d_order, cv::Vec2f(points[0][0]-points[1][0], points[0][1]-points[1][1]));
     // calculate d and d_order for simple freetrack-like point correspondence
-    vector<Vec2f> model_points;
-    model_points.push_back(Vec2f(0,0));
-    model_points.push_back(Vec2f(model.M01[0], model.M01[1]));
-    model_points.push_back(Vec2f(model.M02[0], model.M02[1]));
-    model.get_d_order(model_points, model_d_order);
+    model.get_d_order(std::vector<cv::Vec3f>{ Vec3f{0,0,0}, model.M01, model.M02 }, model_d_order, model.d);
     // set correspondences
     PointOrder p;
     for (int i=0; i<PointModel::N_POINTS; ++i)
diff --git a/ftnoir_tracker_pt/point_tracker.h b/ftnoir_tracker_pt/point_tracker.h
index ec852286..c6e73f71 100644
--- a/ftnoir_tracker_pt/point_tracker.h
+++ b/ftnoir_tracker_pt/point_tracker.h
@@ -72,7 +72,10 @@ private:
     cv::Vec3f u;	// unit vector perpendicular to M01,M02-plane
 
     cv::Matx22f P;
-    void get_d_order(const std::vector<cv::Vec2f>& points, int* d_order) const;
+    cv::Vec3f d;
+    
+    template<typename vec>
+    void get_d_order(const std::vector<vec>& points, int* d_order, vec d) const;
 };
 
 // ----------------------------------------------------------------------------