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, 8 insertions, 1 deletions

diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index 05e7b067..3abaa35e 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -48,6 +48,13 @@ void Tracker::reset_command(Command command)
 	commands &= ~command;
 }
 
+float Tracker::get_focal_length()
+{
+    static constexpr float pi = 3.1415926f;
+    const float fov = static_cast<int>(s.fov) * pi / 180.f;
+    return 0.5f / tan(0.5f * fov);
+}
+
 void Tracker::run()
 {
 #ifdef PT_PERF_LOG
@@ -85,7 +92,7 @@ void Tracker::run()
                          4);
             }
             if (points.size() == PointModel::N_POINTS)
-                point_tracker.track(points, model);
+                point_tracker.track(points, model, get_focal_length());
             video_widget->update_image(frame);
         }
 #ifdef PT_PERF_LOG