aruco: allow for changing marker pitch

3 files changed, 189 insertions, 162 deletions

diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
index 3f5a1d2a..e699eccc 100644
--- a/ftnoir_tracker_aruco/aruco-trackercontrols.ui
+++ b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
@@ -32,158 +32,6 @@
    <property name="spacing">
     <number>7</number>
    </property>
-   <item row="2" column="1" colspan="2">
-    <widget class="QComboBox" name="cameraName"/>
-   </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="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="7" column="1">
-    <widget class="QCheckBox" name="red_only">
-     <property name="text">
-      <string>Recommended!</string>
-     </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="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>
-    </widget>
-   </item>
-   <item row="6" column="2" rowspan="4" colspan="3">
-    <widget class="QLabel" name="label_6">
-     <property name="sizePolicy">
-      <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
-       <horstretch>0</horstretch>
-       <verstretch>0</verstretch>
-      </sizepolicy>
-     </property>
-     <property name="text">
-      <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;The ARUCO Library has been developed by the Ava group of the Univeristy of Cordoba, Spain&lt;/p&gt;&lt;p&gt;Rafael Muñoz Salinas &amp;lt;&lt;a href=&quot;mailto:rmsalinas@uco.es&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0057ae;&quot;&gt;rmsalinas@uco.es&lt;/span&gt;&lt;/a&gt;&amp;gt;&lt;/p&gt;&lt;p&gt;&amp;lt;&lt;a href=&quot;https://github.com/rmsalinas/aruco&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0057ae;&quot;&gt;https://github.com/rmsalinas/aruco&lt;/span&gt;&lt;/a&gt;&amp;gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-     </property>
-     <property name="textFormat">
-      <enum>Qt::RichText</enum>
-     </property>
-     <property name="alignment">
-      <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
-     </property>
-     <property name="wordWrap">
-      <bool>true</bool>
-     </property>
-     <property name="margin">
-      <number>4</number>
-     </property>
-     <property name="openExternalLinks">
-      <bool>true</bool>
-     </property>
-     <property name="textInteractionFlags">
-      <set>Qt::LinksAccessibleByKeyboard|Qt::LinksAccessibleByMouse|Qt::TextBrowserInteraction|Qt::TextSelectableByKeyboard|Qt::TextSelectableByMouse</set>
-     </property>
-    </widget>
-   </item>
-   <item row="6" column="1">
-    <widget class="QComboBox" name="resolution">
-     <item>
-      <property name="text">
-       <string>640x480</string>
-      </property>
-     </item>
-     <item>
-      <property name="text">
-       <string>320x240</string>
-      </property>
-     </item>
-     <item>
-      <property name="text">
-       <string>320x200</string>
-      </property>
-     </item>
-     <item>
-      <property name="text">
-       <string>Default (not recommended!)</string>
-      </property>
-     </item>
-    </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="10" column="4">
-    <widget class="QDialogButtonBox" name="buttonBox">
-     <property name="standardButtons">
-      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
-     </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="0">
-    <widget class="QLabel" name="label_4">
-     <property name="text">
-      <string>Resolution</string>
-     </property>
-    </widget>
-   </item>
    <item row="0" column="3" rowspan="3" colspan="2">
     <widget class="QGroupBox" name="groupBox">
      <property name="title">
@@ -250,7 +98,7 @@
      </layout>
     </widget>
    </item>
-   <item row="8" column="1" rowspan="3">
+   <item row="9" column="1" rowspan="3">
     <widget class="QGroupBox" name="groupBox_2">
      <property name="title">
       <string>Head position</string>
@@ -331,6 +179,175 @@
      </layout>
     </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>
+     </property>
+    </widget>
+   </item>
+   <item row="7" column="1">
+    <widget class="QCheckBox" name="red_only">
+     <property name="text">
+      <string>Recommended!</string>
+     </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">
+    <widget class="QLabel" name="label_6">
+     <property name="sizePolicy">
+      <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
+       <horstretch>0</horstretch>
+       <verstretch>0</verstretch>
+      </sizepolicy>
+     </property>
+     <property name="text">
+      <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;The ARUCO Library has been developed by the Ava group of the Univeristy of Cordoba, Spain&lt;/p&gt;&lt;p&gt;Rafael Muñoz Salinas &amp;lt;&lt;a href=&quot;mailto:rmsalinas@uco.es&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0057ae;&quot;&gt;rmsalinas@uco.es&lt;/span&gt;&lt;/a&gt;&amp;gt;&lt;/p&gt;&lt;p&gt;&amp;lt;&lt;a href=&quot;https://github.com/rmsalinas/aruco&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0057ae;&quot;&gt;https://github.com/rmsalinas/aruco&lt;/span&gt;&lt;/a&gt;&amp;gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+     </property>
+     <property name="textFormat">
+      <enum>Qt::RichText</enum>
+     </property>
+     <property name="alignment">
+      <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
+     </property>
+     <property name="wordWrap">
+      <bool>true</bool>
+     </property>
+     <property name="margin">
+      <number>4</number>
+     </property>
+     <property name="openExternalLinks">
+      <bool>true</bool>
+     </property>
+     <property name="textInteractionFlags">
+      <set>Qt::LinksAccessibleByKeyboard|Qt::LinksAccessibleByMouse|Qt::TextBrowserInteraction|Qt::TextSelectableByKeyboard|Qt::TextSelectableByMouse</set>
+     </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>
+    </widget>
+   </item>
+   <item row="6" column="1">
+    <widget class="QComboBox" name="resolution">
+     <item>
+      <property name="text">
+       <string>640x480</string>
+      </property>
+     </item>
+     <item>
+      <property name="text">
+       <string>320x240</string>
+      </property>
+     </item>
+     <item>
+      <property name="text">
+       <string>320x200</string>
+      </property>
+     </item>
+     <item>
+      <property name="text">
+       <string>Default (not recommended!)</string>
+      </property>
+     </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">
+    <widget class="QDialogButtonBox" name="buttonBox">
+     <property name="standardButtons">
+      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
+     </property>
+    </widget>
+   </item>
+   <item row="6" column="0">
+    <widget class="QLabel" name="label_4">
+     <property name="text">
+      <string>Resolution</string>
+     </property>
+    </widget>
+   </item>
+   <item row="8" column="0">
+    <widget class="QLabel" name="label_9">
+     <property name="text">
+      <string>Marker pitch</string>
+     </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>
   </layout>
  </widget>
  <tabstops>
@@ -338,6 +355,8 @@
   <tabstop>cameraFPS</tabstop>
   <tabstop>cameraName</tabstop>
   <tabstop>resolution</tabstop>
+  <tabstop>red_only</tabstop>
+  <tabstop>marker_pitch</tabstop>
   <tabstop>cx</tabstop>
   <tabstop>cy</tabstop>
   <tabstop>cz</tabstop>
@@ -347,6 +366,7 @@
   <tabstop>tx</tabstop>
   <tabstop>ty</tabstop>
   <tabstop>tz</tabstop>
+  <tabstop>buttonBox</tabstop>
  </tabstops>
  <resources/>
  <connections/>
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 9c8cd52b..776d443a 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -282,19 +282,23 @@ void Tracker::run()
             const aruco::Marker& m = markers.at(0);
             const float size = 7;
             
+            const double p = s.marker_pitch;
+            const double sq = sin(p * HT_PI / 180);
+            const double cq = cos(p * HT_PI / 180);
+
             cv::Mat obj_points(4,3,CV_32FC1);
             obj_points.at<float>(1,0)=-size + s.headpos_x;
-            obj_points.at<float>(1,1)=-size + s.headpos_y;
-            obj_points.at<float>(1,2)=0 + s.headpos_z;
+            obj_points.at<float>(1,1)=-size * cq + s.headpos_y;
+            obj_points.at<float>(1,2)=-size * sq + s.headpos_z;
             obj_points.at<float>(2,0)=size + s.headpos_x;
-            obj_points.at<float>(2,1)=-size + s.headpos_y;
-            obj_points.at<float>(2,2)=0 + s.headpos_z;
+            obj_points.at<float>(2,1)=-size * cq + s.headpos_y;
+            obj_points.at<float>(2,2)=-size * sq + s.headpos_z;
             obj_points.at<float>(3,0)=size + s.headpos_x;
-            obj_points.at<float>(3,1)=size + s.headpos_y;
-            obj_points.at<float>(3,2)=0 + s.headpos_z;
+            obj_points.at<float>(3,1)=size * cq + s.headpos_y;
+            obj_points.at<float>(3,2)=size * sq + s.headpos_z;
             obj_points.at<float>(0,0)=-size + s.headpos_x;
-            obj_points.at<float>(0,1)=size + s.headpos_y;
-            obj_points.at<float>(0,2)=0 + s.headpos_z;
+            obj_points.at<float>(0,1)=size * cq + s.headpos_y;
+            obj_points.at<float>(0,2)=size * sq + s.headpos_z;
 
             last_roi = cv::Rect(65535, 65535, 0, 0);
 
@@ -468,6 +472,7 @@ TrackerControls::TrackerControls()
     tie_setting(s.headpos_y, ui.cy);
     tie_setting(s.headpos_z, ui.cz);
     tie_setting(s.red_only, ui.red_only);
+    tie_setting(s.marker_pitch, ui.marker_pitch);
     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 91a5ae8e..4cab84b5 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
@@ -27,6 +27,7 @@ struct settings {
     value<int> camera_index, force_fps, resolution;
     value<bool> red_only;
     value<bool> eyaw, epitch, eroll, ex, ey, ez;
+    value<double> marker_pitch;
     settings() :
         b(bundle("aruco-tracker")),
         fov(b, "field-of-view", 56),
@@ -42,7 +43,8 @@ struct settings {
         eroll(b, "enable-r", true),
         ex(b, "enable-x", true),
         ey(b, "enable-y", true),
-        ez(b, "enable-z", true)
+        ez(b, "enable-z", true),
+        marker_pitch(b, "marker-pitch", 0)
     {}
 };