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, 4 deletions

diff --git a/ftnoir_tracker_pt/point_tracker.cpp b/ftnoir_tracker_pt/point_tracker.cpp
index 3b1a49e2..9f04af30 100644
--- a/ftnoir_tracker_pt/point_tracker.cpp
+++ b/ftnoir_tracker_pt/point_tracker.cpp
@@ -83,16 +83,15 @@ void PointModel::get_d_order(const std::vector<vec>& points, int d_order[], vec
 }
 
 
-// ----------------------------------------------------------------------------
 PointTracker::PointTracker()
 {
 	X_CM.t[2] = 1000;	// default position: 1 m away from cam;
 }
 
-void PointTracker::track(const vector<Vec2f>& points, const PointModel& model)
+void PointTracker::track(const vector<Vec2f>& points, const PointModel& model, float f)
 {
     const PointOrder& order = find_correspondences(points, model);
-    POSIT(model, order);
+    POSIT(model, order, f);
 }
 
 PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv::Vec2f>& points, const PointModel& model)
@@ -117,7 +116,7 @@ PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv
     return p;
 }
 
-int PointTracker::POSIT(const PointModel& model, const PointOrder& order_)
+int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float 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"]