aruco: allow for perfect fisheye correction

3 files changed, 249 insertions, 228 deletions

diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
index e699eccc..7dadb58c 100644
--- a/ftnoir_tracker_aruco/aruco-trackercontrols.ui
+++ b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
@@ -10,7 +10,7 @@
     <x>0</x>
     <y>0</y>
     <width>630</width>
-    <height>315</height>
+    <height>326</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -32,169 +32,42 @@
    <property name="spacing">
     <number>7</number>
    </property>
-   <item row="0" column="3" rowspan="3" colspan="2">
-    <widget class="QGroupBox" name="groupBox">
-     <property name="title">
-      <string>Enable axes</string>
-     </property>
-     <layout class="QFormLayout" name="formLayout">
-      <property name="horizontalSpacing">
-       <number>7</number>
+   <item row="2" column="1" colspan="2">
+    <widget class="QComboBox" name="cameraName"/>
+   </item>
+   <item row="1" column="1" colspan="2">
+    <widget class="QComboBox" name="cameraFPS">
+     <item>
+      <property name="text">
+       <string notr="true">Default</string>
       </property>
-      <property name="leftMargin">
-       <number>6</number>
+     </item>
+     <item>
+      <property name="text">
+       <string>30</string>
       </property>
-      <property name="topMargin">
-       <number>0</number>
+     </item>
+     <item>
+      <property name="text">
+       <string>60</string>
       </property>
-      <property name="rightMargin">
-       <number>6</number>
+     </item>
+     <item>
+      <property name="text">
+       <string>120</string>
       </property>
-      <property name="bottomMargin">
-       <number>0</number>
+     </item>
+     <item>
+      <property name="text">
+       <string>180</string>
       </property>
-      <item row="0" column="0">
-       <widget class="QCheckBox" name="rx">
-        <property name="text">
-         <string>RX</string>
-        </property>
-       </widget>
-      </item>
-      <item row="0" column="1">
-       <widget class="QCheckBox" name="tx">
-        <property name="text">
-         <string>TX</string>
-        </property>
-       </widget>
-      </item>
-      <item row="1" column="0">
-       <widget class="QCheckBox" name="ry">
-        <property name="text">
-         <string>RY</string>
-        </property>
-       </widget>
-      </item>
-      <item row="1" column="1">
-       <widget class="QCheckBox" name="ty">
-        <property name="text">
-         <string>TY</string>
-        </property>
-       </widget>
-      </item>
-      <item row="2" column="0">
-       <widget class="QCheckBox" name="rz">
-        <property name="text">
-         <string>RZ</string>
-        </property>
-       </widget>
-      </item>
-      <item row="2" column="1">
-       <widget class="QCheckBox" name="tz">
-        <property name="text">
-         <string>TZ</string>
-        </property>
-       </widget>
-      </item>
-     </layout>
-    </widget>
-   </item>
-   <item row="9" column="1" rowspan="3">
-    <widget class="QGroupBox" name="groupBox_2">
-     <property name="title">
-      <string>Head position</string>
-     </property>
-     <property name="alignment">
-      <set>Qt::AlignCenter</set>
-     </property>
-     <layout class="QGridLayout" name="gridLayout_2">
-      <item row="0" column="0">
-       <widget class="QLabel" name="label_5">
-        <property name="text">
-         <string>TX</string>
-        </property>
-       </widget>
-      </item>
-      <item row="0" column="1">
-       <widget class="QDoubleSpinBox" name="cx">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-        <property name="minimum">
-         <double>-200.000000000000000</double>
-        </property>
-        <property name="maximum">
-         <double>200.000000000000000</double>
-        </property>
-       </widget>
-      </item>
-      <item row="1" column="0">
-       <widget class="QLabel" name="label_7">
-        <property name="text">
-         <string>TY</string>
-        </property>
-       </widget>
-      </item>
-      <item row="1" column="1">
-       <widget class="QDoubleSpinBox" name="cy">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-        <property name="minimum">
-         <double>-200.000000000000000</double>
-        </property>
-        <property name="maximum">
-         <double>200.000000000000000</double>
-        </property>
-       </widget>
-      </item>
-      <item row="2" column="0">
-       <widget class="QLabel" name="label_8">
-        <property name="text">
-         <string>TZ</string>
-        </property>
-       </widget>
-      </item>
-      <item row="2" column="1">
-       <widget class="QDoubleSpinBox" name="cz">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-        <property name="minimum">
-         <double>-200.000000000000000</double>
-        </property>
-        <property name="maximum">
-         <double>200.000000000000000</double>
-        </property>
-       </widget>
-      </item>
-     </layout>
+     </item>
     </widget>
    </item>
-   <item row="2" column="1" colspan="2">
-    <widget class="QComboBox" name="cameraName"/>
-   </item>
-   <item row="0" column="1" colspan="2">
-    <widget class="QDoubleSpinBox" name="cameraFOV">
-     <property name="locale">
-      <locale language="English" country="UnitedStates"/>
-     </property>
-     <property name="minimum">
-      <double>35.000000000000000</double>
-     </property>
-     <property name="maximum">
-      <double>180.000000000000000</double>
-     </property>
-     <property name="value">
-      <double>52.000000000000000</double>
+   <item row="1" column="0">
+    <widget class="QLabel" name="label_2">
+     <property name="text">
+      <string>Frames per second</string>
      </property>
     </widget>
    </item>
@@ -205,14 +78,7 @@
      </property>
     </widget>
    </item>
-   <item row="1" column="0">
-    <widget class="QLabel" name="label_2">
-     <property name="text">
-      <string>Frames per second</string>
-     </property>
-    </widget>
-   </item>
-   <item row="6" column="2" rowspan="5" colspan="3">
+   <item row="6" column="2" rowspan="6" colspan="3">
     <widget class="QLabel" name="label_6">
      <property name="sizePolicy">
       <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
@@ -243,33 +109,41 @@
      </property>
     </widget>
    </item>
-   <item row="1" column="1" colspan="2">
-    <widget class="QComboBox" name="cameraFPS">
-     <item>
-      <property name="text">
-       <string notr="true">Default</string>
-      </property>
-     </item>
-     <item>
-      <property name="text">
-       <string>30</string>
-      </property>
-     </item>
-     <item>
-      <property name="text">
-       <string>60</string>
-      </property>
-     </item>
-     <item>
-      <property name="text">
-       <string>120</string>
-      </property>
-     </item>
-     <item>
-      <property name="text">
-       <string>180</string>
-      </property>
-     </item>
+   <item row="0" column="1" colspan="2">
+    <widget class="QDoubleSpinBox" name="cameraFOV">
+     <property name="locale">
+      <locale language="English" country="UnitedStates"/>
+     </property>
+     <property name="minimum">
+      <double>35.000000000000000</double>
+     </property>
+     <property name="maximum">
+      <double>180.000000000000000</double>
+     </property>
+     <property name="value">
+      <double>52.000000000000000</double>
+     </property>
+    </widget>
+   </item>
+   <item row="2" column="0">
+    <widget class="QLabel" name="label_3">
+     <property name="text">
+      <string>Camera name</string>
+     </property>
+    </widget>
+   </item>
+   <item row="7" column="0">
+    <widget class="QLabel" name="label_10">
+     <property name="text">
+      <string>Red channel only</string>
+     </property>
+    </widget>
+   </item>
+   <item row="0" column="0">
+    <widget class="QLabel" name="label">
+     <property name="text">
+      <string>Horizontal FOV</string>
+     </property>
     </widget>
    </item>
    <item row="6" column="1">
@@ -296,28 +170,7 @@
      </item>
     </widget>
    </item>
-   <item row="2" column="0">
-    <widget class="QLabel" name="label_3">
-     <property name="text">
-      <string>Camera name</string>
-     </property>
-    </widget>
-   </item>
-   <item row="7" column="0">
-    <widget class="QLabel" name="label_10">
-     <property name="text">
-      <string>Red channel only</string>
-     </property>
-    </widget>
-   </item>
-   <item row="0" column="0">
-    <widget class="QLabel" name="label">
-     <property name="text">
-      <string>Horizontal FOV</string>
-     </property>
-    </widget>
-   </item>
-   <item row="11" column="4">
+   <item row="12" column="4">
     <widget class="QDialogButtonBox" name="buttonBox">
      <property name="standardButtons">
       <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
@@ -331,6 +184,16 @@
      </property>
     </widget>
    </item>
+   <item row="8" column="1">
+    <widget class="QDoubleSpinBox" name="marker_pitch">
+     <property name="minimum">
+      <double>-180.000000000000000</double>
+     </property>
+     <property name="maximum">
+      <double>180.000000000000000</double>
+     </property>
+    </widget>
+   </item>
    <item row="8" column="0">
     <widget class="QLabel" name="label_9">
      <property name="text">
@@ -338,13 +201,164 @@
      </property>
     </widget>
    </item>
-   <item row="8" column="1">
-    <widget class="QDoubleSpinBox" name="marker_pitch">
-     <property name="minimum">
-      <double>-180.000000000000000</double>
+   <item row="10" column="1" rowspan="3">
+    <widget class="QGroupBox" name="groupBox_2">
+     <property name="title">
+      <string>Head position</string>
      </property>
-     <property name="maximum">
-      <double>180.000000000000000</double>
+     <property name="alignment">
+      <set>Qt::AlignCenter</set>
+     </property>
+     <layout class="QGridLayout" name="gridLayout_2">
+      <item row="2" column="0">
+       <widget class="QLabel" name="label_7">
+        <property name="text">
+         <string>TY</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="0">
+       <widget class="QLabel" name="label_5">
+        <property name="text">
+         <string>TX</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="1">
+       <widget class="QDoubleSpinBox" name="cx">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
+          <horstretch>0</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
+        <property name="minimum">
+         <double>-200.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>200.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="3" column="0">
+       <widget class="QLabel" name="label_8">
+        <property name="text">
+         <string>TZ</string>
+        </property>
+       </widget>
+      </item>
+      <item row="3" column="1">
+       <widget class="QDoubleSpinBox" name="cz">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
+          <horstretch>0</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
+        <property name="minimum">
+         <double>-200.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>200.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="1">
+       <widget class="QDoubleSpinBox" name="cy">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
+          <horstretch>0</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
+        <property name="minimum">
+         <double>-200.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>200.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item row="0" column="3" rowspan="3" colspan="2">
+    <widget class="QGroupBox" name="groupBox">
+     <property name="title">
+      <string>Enable axes</string>
+     </property>
+     <layout class="QFormLayout" name="formLayout">
+      <property name="horizontalSpacing">
+       <number>7</number>
+      </property>
+      <property name="leftMargin">
+       <number>6</number>
+      </property>
+      <property name="topMargin">
+       <number>0</number>
+      </property>
+      <property name="rightMargin">
+       <number>6</number>
+      </property>
+      <property name="bottomMargin">
+       <number>0</number>
+      </property>
+      <item row="0" column="0">
+       <widget class="QCheckBox" name="rx">
+        <property name="text">
+         <string>RX</string>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="1">
+       <widget class="QCheckBox" name="tx">
+        <property name="text">
+         <string>TX</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="0">
+       <widget class="QCheckBox" name="ry">
+        <property name="text">
+         <string>RY</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="1">
+       <widget class="QCheckBox" name="ty">
+        <property name="text">
+         <string>TY</string>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="0">
+       <widget class="QCheckBox" name="rz">
+        <property name="text">
+         <string>RZ</string>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="1">
+       <widget class="QCheckBox" name="tz">
+        <property name="text">
+         <string>TZ</string>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item row="9" column="1">
+    <widget class="QCheckBox" name="fisheye_correction">
+     <property name="text">
+      <string>Check if yaw/X interconnect</string>
+     </property>
+    </widget>
+   </item>
+   <item row="9" column="0">
+    <widget class="QLabel" name="label_11">
+     <property name="text">
+      <string>Fisheye correction</string>
      </property>
     </widget>
    </item>
@@ -357,6 +371,7 @@
   <tabstop>resolution</tabstop>
   <tabstop>red_only</tabstop>
   <tabstop>marker_pitch</tabstop>
+  <tabstop>fisheye_correction</tabstop>
   <tabstop>cx</tabstop>
   <tabstop>cy</tabstop>
   <tabstop>cz</tabstop>
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 776d443a..67c88e3e 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -18,7 +18,8 @@
 #include <cstdio>
 
 #if defined(_WIN32)
-#define NOMINMAX
+#   undef NOMINMAX
+#   define NOMINMAX
 #   define NO_DSHOW_STRSAFE
 #   include <dshow.h>
 #else
@@ -322,17 +323,13 @@ void Tracker::run()
                 last_roi.width = std::min<int>(grayscale.cols - last_roi.x, last_roi.width);
                 last_roi.height = std::min<int>(grayscale.rows - last_roi.y, last_roi.height);
             }
-            
-            cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, !first, cv::ITERATIVE);
 
+            cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, !first, cv::ITERATIVE);
             first = false;
-            
             cv::Mat rotation_matrix = cv::Mat::zeros(3, 3, CV_64FC1);
-            
             cv::Mat junk1(3, 3, CV_64FC1), junk2(3, 3, CV_64FC1);
-        
             cv::Rodrigues(rvec, rotation_matrix);
-        
+
             {
                 cv::Vec3d euler = cv::RQDecomp3x3(rotation_matrix, junk1, junk2);
 
@@ -350,6 +347,12 @@ void Tracker::run()
                 pose[Yaw] = euler[1];
                 pose[Pitch] = -euler[0];
                 pose[Roll] = euler[2];
+
+                if (s.fisheye_correction)
+                {
+                    pose[Yaw] -= atan(pose[TX] / pose[TZ]) * 180 / HT_PI;
+                    pose[Pitch] -= atan(pose[TY] / pose[TZ]) * 180 / HT_PI;
+                }
             }
 
             std::vector<cv::Point2f> repr2;
@@ -473,6 +476,7 @@ TrackerControls::TrackerControls()
     tie_setting(s.headpos_z, ui.cz);
     tie_setting(s.red_only, ui.red_only);
     tie_setting(s.marker_pitch, ui.marker_pitch);
+    tie_setting(s.fisheye_correction, ui.fisheye_correction);
     connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
     connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
     ui.cameraName->addItems(get_camera_names());
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
index 4cab84b5..e32e304a 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
@@ -28,6 +28,7 @@ struct settings {
     value<bool> red_only;
     value<bool> eyaw, epitch, eroll, ex, ey, ez;
     value<double> marker_pitch;
+    value<bool> fisheye_correction;
     settings() :
         b(bundle("aruco-tracker")),
         fov(b, "field-of-view", 56),
@@ -44,7 +45,8 @@ struct settings {
         ex(b, "enable-x", true),
         ey(b, "enable-y", true),
         ez(b, "enable-z", true),
-        marker_pitch(b, "marker-pitch", 0)
+        marker_pitch(b, "marker-pitch", 0),
+        fisheye_correction(b, "fisheye-correction", false)
     {}
 };