decruft PT more, so it doesn't crash finally

1 files changed, 13 insertions, 12 deletions

diff --git a/ftnoir_tracker_pt/point_tracker.h b/ftnoir_tracker_pt/point_tracker.h
index a1f6f041..0339f392 100644
--- a/ftnoir_tracker_pt/point_tracker.h
+++ b/ftnoir_tracker_pt/point_tracker.h
@@ -14,7 +14,7 @@
 #else
 #   include <memory>
 #endif
-#include <list>
+#include <vector>
 
 // ----------------------------------------------------------------------------
 // Affine frame trafo
@@ -60,9 +60,10 @@ public:
 	static constexpr int N_POINTS = 3;
 
 	PointModel(cv::Vec3f M01, cv::Vec3f M02);
+    PointModel();
 
-	const cv::Vec3f& get_M01() const { return M01; }
-	const cv::Vec3f& get_M02() const { return M02; }
+	inline const cv::Vec3f& get_M01() const { return M01; }
+	inline const cv::Vec3f& get_M02() const { return M02; }
 
 private:
 	cv::Vec3f M01;	// M01 in model frame
@@ -86,27 +87,27 @@ class PointTracker
 {
 public:
 	PointTracker();
-
 	// track the pose using the set of normalized point coordinates (x pos in range -0.5:0.5)
 	// f : (focal length)/(sensor width)
 	// dt : time since last call
-	void track(const std::vector<cv::Vec2f>& points, float f);
-	std::shared_ptr<PointModel> point_model;
-
+	void track(const std::vector<cv::Vec2f>& projected_points, const PointModel& model);
 	FrameTrafo get_pose() const { return X_CM; }
 	void reset();
 
 private:
-	inline cv::Vec2f project(const cv::Vec3f& v_M, float f)
+    // the points in model order
+    typedef struct { cv::Vec2f points[PointModel::N_POINTS]; } PointOrder;
+    static constexpr float focal_length = 1.0f;    
+    
+	inline cv::Vec2f project(const cv::Vec3f& v_M)
 	{
 		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]);
+		return cv::Vec2f(focal_length*v_C[0]/v_C[2], focal_length*v_C[1]/v_C[2]);
 	}
 
-    void find_correspondences(const std::vector<cv::Vec2f>& points, float f);
-    int POSIT(float f);  // The POSIT algorithm, returns the number of iterations
+    PointOrder find_correspondences(const std::vector<cv::Vec2f>& projected_points, const PointModel &model);
+    int POSIT(const PointModel& point_model, const PointOrder& order);  // The POSIT algorithm, returns the number of iterations
     
-	cv::Vec2f p[PointModel::N_POINTS];	// the points in model order
 	FrameTrafo X_CM; // trafo from model to camera
 };