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;

7 files changed, 53 insertions, 10 deletions

diff --git a/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
index 63ed7f86..109e50cb 100644
--- a/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
+++ b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
@@ -9,7 +9,7 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>453</width>
+    <width>460</width>
     <height>621</height>
    </rect>
   </property>
@@ -273,6 +273,35 @@
             </property>
            </widget>
           </item>
+          <item row="4" column="0">
+           <widget class="QLabel" name="label_4">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="text">
+             <string>Field of view</string>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="1">
+           <widget class="QSpinBox" name="fov">
+            <property name="suffix">
+             <string>°</string>
+            </property>
+            <property name="prefix">
+             <string/>
+            </property>
+            <property name="minimum">
+             <number>10</number>
+            </property>
+            <property name="maximum">
+             <number>90</number>
+            </property>
+           </widget>
+          </item>
          </layout>
         </widget>
        </item>
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
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.h b/ftnoir_tracker_pt/ftnoir_tracker_pt.h
index 2f922e17..3dd15618 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.h
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.h
@@ -57,6 +57,9 @@ private:
     };
     void set_command(Command command);
     void reset_command(Command command);
+    
+    float get_focal_length();
+    
     volatile int commands;
 
     CVCamera       camera;
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
index 2bf8ba75..8f6edc2f 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
@@ -65,6 +65,8 @@ TrackerDialog::TrackerDialog()
     tie_setting(s.t_MH_x, ui.tx_spin);
     tie_setting(s.t_MH_y, ui.ty_spin);
     tie_setting(s.t_MH_z, ui.tz_spin);
+    
+    tie_setting(s.fov, ui.fov);
 
     connect( ui.tcalib_button,SIGNAL(toggled(bool)), this,SLOT(startstop_trans_calib(bool)) );
 
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
index c95dc3e5..804de22e 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
@@ -33,6 +33,8 @@ struct settings
 
     value<int> clip_ty, clip_tz, clip_by, clip_bz;
     value<int> active_model_panel, cap_x, cap_y, cap_z;
+    
+    value<int> fov;
 
     settings() :
         b(bundle("tracker-pt")),
@@ -60,7 +62,8 @@ struct settings
         active_model_panel(b, "active-model-panel", 0),
         cap_x(b, "cap-x", 0),
         cap_y(b, "cap-y", 0),
-        cap_z(b, "cap-z", 0)
+        cap_z(b, "cap-z", 0),
+        fov(b, "camera-fov", 56)
     {}
 };
 
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"]
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
 };