Implement Aruco pitch change

3 files changed, 53 insertions, 7 deletions

diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
index 76d750af..8cf28057 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>704</width>
-    <height>273</height>
+    <height>308</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -294,6 +294,32 @@
         </property>
        </widget>
       </item>
+      <item row="3" column="0">
+       <widget class="QLabel" name="label_9">
+        <property name="text">
+         <string>Pitch</string>
+        </property>
+       </widget>
+      </item>
+      <item row="3" column="1">
+       <widget class="QDoubleSpinBox" name="pitch_deg">
+        <property name="suffix">
+         <string>°</string>
+        </property>
+        <property name="decimals">
+         <number>2</number>
+        </property>
+        <property name="minimum">
+         <double>-60.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>60.000000000000000</double>
+        </property>
+        <property name="value">
+         <double>0.000000000000000</double>
+        </property>
+       </widget>
+      </item>
      </layout>
     </widget>
    </item>
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 41bf0edd..ddbdadb0 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -128,6 +128,8 @@ void Tracker::load_settings()
     {
         headpos[i] = iniFile.value(QString("headpos-%1").arg(i), 0).toDouble();
     }
+    headpitch = iniFile.value("pitch", 0).toDouble();
+
 	iniFile.endGroup();
 }
 
@@ -328,17 +330,27 @@ void Tracker::run()
         
             cv::Rodrigues(rvec, rotation_matrix);
         
-            cv::Vec3d foo = cv::RQDecomp3x3(rotation_matrix, junk1, junk2);
-
             {
+                const double beta = headpitch * M_PI / 180;
+                double pitch[] = {
+                    1, 0, 0,
+                    0, cos(beta), -sin(beta),
+                    0, sin(beta), cos(beta)
+                };
+                cv::Mat rot(3, 3, CV_64F, pitch);
+                tvec = rot * tvec;
+                rotation_matrix = rot * rotation_matrix;
+
+                cv::Vec3d euler = cv::RQDecomp3x3(rotation_matrix, junk1, junk2);
+
                 QMutexLocker lck(&mtx);
 
                 for (int i = 0; i < 3; i++)
                     pose[i] = tvec.at<double>(i);
 
-                pose[Yaw] = foo[1];
-                pose[Pitch] = -foo[0];
-                pose[Roll] = foo[2];
+                pose[Yaw] = euler[1];
+                pose[Pitch] = -euler[0];
+                pose[Roll] = euler[2];
             }
 
             std::vector<cv::Point2f> repr2;
@@ -346,6 +358,11 @@ void Tracker::run()
             centroid.push_back(cv::Point3f(0, 0, 0));
             cv::projectPoints(centroid, rvec, tvec, intrinsics, dist_coeffs, repr2);
 
+            {
+                auto s = cv::Scalar(255, 0, 255);
+                cv::circle(frame, repr2.at(0), 4, s, -1);
+            }
+
             last_centroid = repr2[0];
         }
         else
@@ -527,6 +544,8 @@ void TrackerControls::loadSettings()
         headpos[i]->setValue(iniFile.value(QString("headpos-%1").arg(i)).toDouble());
     }
 
+    ui.pitch_deg->setValue(iniFile.value("pitch", 0).toDouble());
+
 	iniFile.endGroup();
 	settingsDirty = false;
 }
@@ -565,6 +584,7 @@ void TrackerControls::save()
 	iniFile.setValue("enable-ty", ui.ty->checkState() != Qt::Unchecked ? true : false);
 	iniFile.setValue("enable-tz", ui.tz->checkState() != Qt::Unchecked ? true : false);
 	iniFile.setValue("resolution", ui.resolution->currentIndex());
+    iniFile.setValue("pitch", ui.pitch_deg->value());
 
     QDoubleSpinBox* headpos[] = {
         ui.cx,
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
index 3a2ebeea..1518ba4d 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
@@ -40,7 +40,7 @@ private:
     int force_fps, force_width, force_height;
     double pose[6];
     cv::Mat frame;
-    double headpos[3];
+    double headpos[3], headpitch;
     cv::VideoCapture camera;
 };