pt: set correct focal length from camera fovopentrack-2.3-rc6

Issue: #96

Having user-supplied camera fov, we can prevent yaw and pitch occuring
by itself when moving horizontally and vertically.

Note PointExtractor::extract_points(Mat& frame) should enable same value
for fx and fy:

			c[0] =  (mx/m - W/2)/W;
			c[1] = -(my/m - H/2)/W;

1 files changed, 3 insertions, 3 deletions

diff --git a/ftnoir_tracker_pt/point_tracker.h b/ftnoir_tracker_pt/point_tracker.h
index f9bba311..d37fb726 100644
--- a/ftnoir_tracker_pt/point_tracker.h
+++ b/ftnoir_tracker_pt/point_tracker.h
@@ -88,15 +88,15 @@ public:
     // 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>& projected_points, const PointModel& model);
+    void track(const std::vector<cv::Vec2f>& projected_points, const PointModel& model, float f);
     Affine pose() const { return X_CM; }
+    
 private:
     // the points in model order
     typedef struct { cv::Vec2f points[PointModel::N_POINTS]; } PointOrder;
-    static constexpr float focal_length = 1.0f;
 
     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
+    int POSIT(const PointModel& point_model, const PointOrder& order, float focal_length);  // The POSIT algorithm, returns the number of iterations
 
     Affine X_CM; // trafo from model to camera
 };