pt, ht, aruco: use calibration data. rename fov to diagonal in UI

6 files changed, 36 insertions, 15 deletions

diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
index 4433c47c..bc384f39 100644
--- a/ftnoir_tracker_aruco/aruco-trackercontrols.ui
+++ b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
@@ -9,7 +9,7 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>586</width>
+    <width>485</width>
     <height>202</height>
    </rect>
   </property>
@@ -109,7 +109,7 @@
       <item row="0" column="0">
        <widget class="QLabel" name="label">
         <property name="text">
-         <string>Horizontal FOV</string>
+         <string>Diagonal FOV</string>
         </property>
        </widget>
       </item>
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 70af379d..6d87503f 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -18,6 +18,7 @@
 #include <opencv2/videoio.hpp>
 #include "opentrack/camera-names.hpp"
 #include "opentrack/thread.hpp"
+#include "opentrack/opencv-calibration.hpp"
 
 typedef struct {
     int width;
@@ -134,6 +135,9 @@ void Tracker::run()
     double failed = 0;
     const double max_failed = 1.25;
     cv::Vec3d rvec, tvec;
+    double last_fov = -1;
+    cv::Mat intrinsics = cv::Mat::eye(3, 3, CV_32FC1);
+    cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1);
 
     while (!stop)
     {
@@ -150,15 +154,28 @@ void Tracker::run()
         const int scale = grayscale.cols > 480 ? 2 : 1;
         detector.setThresholdParams(box_sizes[box_idx], 5);
 
-        const float focal_length_w = 0.5 * grayscale.cols / tan(0.5 * s.fov * HT_PI / 180);
-        const float focal_length_h = 0.5 * grayscale.rows / tan(0.5 * s.fov * grayscale.rows / grayscale.cols * HT_PI / 180.0);
-        cv::Mat intrinsics = cv::Mat::eye(3, 3, CV_32FC1);
-        intrinsics.at<float> (0, 0) = focal_length_w;
-        intrinsics.at<float> (1, 1) = focal_length_h;
-        intrinsics.at<float> (0, 2) = grayscale.cols/2;
-        intrinsics.at<float> (1, 2) = grayscale.rows/2;
+        static constexpr double pi = 3.1415926f;
+        const int w = grayscale.cols, h = grayscale.rows;
+        const double diag = sqrt(w * w + h * h)/w, diag_fov = static_cast<int>(s.fov) * pi / 180.;
+        const double fov = 2.*atan(tan(diag_fov/2.)/sqrt(1. + diag*diag));
+        const float focal_length_w = .5 * w / tan(.5 * fov);
+        const float focal_length_h = focal_length_w;
 
-        cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1);
+        if (last_fov != s.fov)
+        {
+            last_fov = s.fov;
+            if (!get_camera_calibration(static_cast<QString>(s.camera_name), intrinsics, dist_coeffs, grayscale.cols, grayscale.rows))
+            {
+                intrinsics.at<float> (0, 0) = focal_length_w;
+                intrinsics.at<float> (1, 1) = focal_length_h;
+                intrinsics.at<float> (0, 2) = grayscale.cols/2;
+                intrinsics.at<float> (1, 2) = grayscale.rows/2;
+            }
+            else
+            {
+                qDebug() << "got calibration";
+            }
+        }
 
         std::vector< aruco::Marker > markers;
 
diff --git a/ftnoir_tracker_ht/ftnoir_tracker_ht.cpp b/ftnoir_tracker_ht/ftnoir_tracker_ht.cpp
index deb90ee5..1ec0e13f 100644
--- a/ftnoir_tracker_ht/ftnoir_tracker_ht.cpp
+++ b/ftnoir_tracker_ht/ftnoir_tracker_ht.cpp
@@ -42,6 +42,7 @@ void Tracker::load_settings(ht_config_t* config)
         break;
     }
 
+    static constexpr float pi = 3.14159265358979323846f;
     config->classification_delay = 500;
     config->field_of_view = s.fov;
     config->max_keypoints = 150;
diff --git a/ftnoir_tracker_ht/ht-trackercontrols.ui b/ftnoir_tracker_ht/ht-trackercontrols.ui
index dd5e57f3..29b80c8d 100644
--- a/ftnoir_tracker_ht/ht-trackercontrols.ui
+++ b/ftnoir_tracker_ht/ht-trackercontrols.ui
@@ -55,7 +55,7 @@
    <item row="0" column="0">
     <widget class="QLabel" name="label">
      <property name="text">
-      <string>Horizontal FOV</string>
+      <string>Diagonal FOV</string>
      </property>
     </widget>
    </item>
diff --git a/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
index 3e900e8e..928b1374 100644
--- a/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
+++ b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
@@ -114,7 +114,7 @@
              </sizepolicy>
             </property>
             <property name="text">
-             <string>Field of view</string>
+             <string>Diagonal field of view</string>
             </property>
            </widget>
           </item>
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index dc0659e5..5b9ec8b6 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -54,9 +54,12 @@ void Tracker_PT::reset_command(Command command)
 
 float Tracker_PT::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);
+    CamInfo info = camera.get_info();
+    const int w = info.res_x, h = info.res_y;
+    static constexpr double pi = 3.1415926f;
+    const double diag = sqrt(w * w + h * h)/w, diag_fov = static_cast<int>(s.fov) * pi / 180.;
+    const double fov = 2.*atan(tan(diag_fov/2.0)/sqrt(1. + diag*diag));
+    return .5 / tan(.5 * fov);
 }
 
 void Tracker_PT::run()