nix market pitch, aruco credit doesn't fit anymore sadly

3 files changed, 117 insertions, 170 deletions

diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
index e5654bd5..240ef5f8 100644
--- a/ftnoir_tracker_aruco/aruco-trackercontrols.ui
+++ b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
@@ -9,8 +9,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>636</width>
-    <height>368</height>
+    <width>560</width>
+    <height>214</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -22,16 +22,17 @@
   <property name="windowTitle">
    <string>Tracker settings</string>
   </property>
-  <layout class="QGridLayout" name="gridLayout_3">
-   <item row="0" column="0" rowspan="2" colspan="2">
-    <widget class="QGroupBox" name="groupBox">
-     <property name="title">
-      <string/>
-     </property>
-     <property name="flat">
-      <bool>true</bool>
+  <layout class="QGridLayout" name="gridLayout">
+   <item row="1" column="1">
+    <widget class="QDialogButtonBox" name="buttonBox">
+     <property name="standardButtons">
+      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
      </property>
-     <layout class="QGridLayout" name="gridLayout">
+    </widget>
+   </item>
+   <item row="0" column="0">
+    <widget class="QFrame" name="frame">
+     <layout class="QGridLayout" name="gridLayout_3">
       <item row="0" column="0">
        <widget class="QLabel" name="label">
         <property name="text">
@@ -41,6 +42,12 @@
       </item>
       <item row="0" column="1">
        <widget class="QDoubleSpinBox" name="cameraFOV">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+          <horstretch>0</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
         <property name="locale">
          <locale language="English" country="UnitedStates"/>
         </property>
@@ -64,6 +71,12 @@
       </item>
       <item row="1" column="1">
        <widget class="QComboBox" name="cameraFPS">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+          <horstretch>0</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
         <item>
          <property name="text">
           <string notr="true">Default</string>
@@ -99,7 +112,14 @@
        </widget>
       </item>
       <item row="2" column="1">
-       <widget class="QComboBox" name="cameraName"/>
+       <widget class="QComboBox" name="cameraName">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+          <horstretch>0</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
+       </widget>
       </item>
       <item row="3" column="0">
        <widget class="QLabel" name="label_4">
@@ -110,6 +130,12 @@
       </item>
       <item row="3" column="1">
        <widget class="QComboBox" name="resolution">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+          <horstretch>0</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
         <item>
          <property name="text">
           <string>640x480</string>
@@ -146,154 +172,93 @@
         </property>
        </widget>
       </item>
-      <item row="5" column="0">
-       <widget class="QLabel" name="label_9">
+     </layout>
+    </widget>
+   </item>
+   <item row="0" column="1">
+    <widget class="QFrame" name="frame_2">
+     <layout class="QGridLayout" name="gridLayout_2">
+      <item row="1" column="0">
+       <widget class="QLabel" name="label_5">
         <property name="text">
-         <string>Marker pitch</string>
+         <string>Head X</string>
         </property>
        </widget>
       </item>
-      <item row="5" column="1">
-       <widget class="QDoubleSpinBox" name="marker_pitch">
+      <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>-180.000000000000000</double>
+         <double>-10000.000000000000000</double>
         </property>
         <property name="maximum">
-         <double>180.000000000000000</double>
+         <double>10000.000000000000000</double>
+        </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>-10000.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>10000.000000000000000</double>
+        </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>-10000.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>10000.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="3" column="0">
+       <widget class="QLabel" name="label_8">
+        <property name="text">
+         <string>Head Z </string>
         </property>
        </widget>
       </item>
-      <item row="6" column="1">
-       <widget class="QGroupBox" name="groupBox_2">
-        <property name="title">
-         <string>Head center</string>
+      <item row="2" column="0">
+       <widget class="QLabel" name="label_7">
+        <property name="text">
+         <string>Head Y</string>
+        </property>
+       </widget>
+      </item>
+      <item row="4" column="1">
+       <widget class="QPushButton" name="btn_calibrate">
+        <property name="text">
+         <string>Calibrate</string>
         </property>
-        <layout class="QGridLayout" name="gridLayout_2">
-         <item row="4" column="1">
-          <widget class="QPushButton" name="btn_calibrate">
-           <property name="text">
-            <string>Calibrate</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="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>-10000.000000000000000</double>
-           </property>
-           <property name="maximum">
-            <double>10000.000000000000000</double>
-           </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>-10000.000000000000000</double>
-           </property>
-           <property name="maximum">
-            <double>10000.000000000000000</double>
-           </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>-10000.000000000000000</double>
-           </property>
-           <property name="maximum">
-            <double>10000.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="2" column="0">
-          <widget class="QLabel" name="label_7">
-           <property name="text">
-            <string>TY</string>
-           </property>
-          </widget>
-         </item>
-        </layout>
        </widget>
       </item>
      </layout>
     </widget>
    </item>
-   <item row="0" column="2">
-    <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::AlignVCenter</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="2">
-    <widget class="QDialogButtonBox" name="buttonBox">
-     <property name="standardButtons">
-      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
-     </property>
-    </widget>
-   </item>
   </layout>
  </widget>
- <tabstops>
-  <tabstop>buttonBox</tabstop>
- </tabstops>
  <resources/>
  <connections/>
  <designerdata>
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 1787ef2b..31aa2372 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -198,7 +198,6 @@ void Tracker::run()
     int cur_fps = 0;
     int last_fps = 0;
     cv::Point2f last_centroid;
-    bool first = true;
 
     while (!stop)
     {
@@ -284,23 +283,19 @@ void Tracker::run()
             const auto& m = markers.at(0);
             const float size = 40;
 
-            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 * cq + s.headpos_y;
-            obj_points.at<float>(1,2)=-size * sq + s.headpos_z;
+            obj_points.at<float>(1,1)=-size + s.headpos_y;
+            obj_points.at<float>(1,2)=-size + s.headpos_z;
             obj_points.at<float>(2,0)=size + s.headpos_x;
-            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>(2,1)=-size + s.headpos_y;
+            obj_points.at<float>(2,2)=-size + s.headpos_z;
             obj_points.at<float>(3,0)=size + s.headpos_x;
-            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>(3,1)=size + s.headpos_y;
+            obj_points.at<float>(3,2)=size + s.headpos_z;
             obj_points.at<float>(0,0)=-size + s.headpos_x;
-            obj_points.at<float>(0,1)=size * cq + s.headpos_y;
-            obj_points.at<float>(0,2)=size * sq + s.headpos_z;
+            obj_points.at<float>(0,1)=size + s.headpos_y;
+            obj_points.at<float>(0,2)=size + s.headpos_z;
 
             last_roi = cv::Rect(65535, 65535, 0, 0);
 
@@ -325,8 +320,7 @@ void Tracker::run()
                 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);
-            first = false;
+            cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, false, cv::ITERATIVE);
             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);
@@ -334,12 +328,6 @@ void Tracker::run()
             {
                 cv::Vec3d euler = cv::RQDecomp3x3(rotation_matrix, junk1, junk2);
 
-                if (fabs(euler[0]) + fabs(s.marker_pitch) > 60)
-                {
-                    first = true;
-                    qDebug() << "reset levmarq due to pitch breakage";
-                }
-
                 QMutexLocker lck(&mtx);
 
                 for (int i = 0; i < 3; i++)
@@ -366,10 +354,7 @@ void Tracker::run()
             last_centroid = repr2[0];
         }
         else
-        {
             last_roi = cv::Rect(65535, 65535, 0, 0);
-            first = true;
-        }
 
         if (frame.rows > 0)
             videoWidget->update_image(frame);
@@ -454,7 +439,6 @@ 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()));
     connect(ui.btn_calibrate, SIGNAL(clicked()), this, SLOT(toggleCalibrate()));
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
index 9ac57417..c53a49a4 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
@@ -28,7 +28,6 @@ struct settings {
     value<double> fov, headpos_x, headpos_y, headpos_z;
     value<int> camera_index, force_fps, resolution;
     value<bool> red_only;
-    value<double> marker_pitch;
     settings() :
         b(bundle("aruco-tracker")),
         fov(b, "field-of-view", 56),
@@ -38,8 +37,7 @@ struct settings {
         camera_index(b, "camera-index", 0),
         force_fps(b, "force-fps", 0),
         resolution(b, "force-resolution", 0),
-        red_only(b, "red-only", false),
-        marker_pitch(b, "marker-pitch", 0)
+        red_only(b, "red-only", false)
     {}
 };