implement head centroid for aruco

Signed-off-by: Stanislaw Halik <sthalik@misaki.pl>

3 files changed, 124 insertions, 179 deletions

diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
index c007e93b..76d750af 100644
--- a/ftnoir_tracker_aruco/aruco-trackercontrols.ui
+++ b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
@@ -32,154 +32,7 @@
    <property name="spacing">
     <number>-1</number>
    </property>
-   <item row="7" column="0">
-    <widget class="QLabel" name="label_5">
-     <property name="text">
-      <string>Distortion coefficients</string>
-     </property>
-    </widget>
-   </item>
-   <item row="7" column="1">
-    <widget class="QDoubleSpinBox" name="doubleSpinBox">
-     <property name="sizePolicy">
-      <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
-       <horstretch>0</horstretch>
-       <verstretch>0</verstretch>
-      </sizepolicy>
-     </property>
-     <property name="frame">
-      <bool>true</bool>
-     </property>
-     <property name="buttonSymbols">
-      <enum>QAbstractSpinBox::NoButtons</enum>
-     </property>
-     <property name="decimals">
-      <number>11</number>
-     </property>
-     <property name="minimum">
-      <double>-1000.000000000000000</double>
-     </property>
-     <property name="maximum">
-      <double>1000.000000000000000</double>
-     </property>
-     <property name="singleStep">
-      <double>0.000000000000000</double>
-     </property>
-    </widget>
-   </item>
-   <item row="9" column="1">
-    <widget class="QDoubleSpinBox" name="doubleSpinBox_3">
-     <property name="sizePolicy">
-      <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
-       <horstretch>0</horstretch>
-       <verstretch>0</verstretch>
-      </sizepolicy>
-     </property>
-     <property name="frame">
-      <bool>true</bool>
-     </property>
-     <property name="buttonSymbols">
-      <enum>QAbstractSpinBox::NoButtons</enum>
-     </property>
-     <property name="decimals">
-      <number>11</number>
-     </property>
-     <property name="minimum">
-      <double>-1000.000000000000000</double>
-     </property>
-     <property name="maximum">
-      <double>1000.000000000000000</double>
-     </property>
-     <property name="singleStep">
-      <double>0.000000000000000</double>
-     </property>
-    </widget>
-   </item>
-   <item row="10" column="1">
-    <widget class="QDoubleSpinBox" name="doubleSpinBox_4">
-     <property name="sizePolicy">
-      <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
-       <horstretch>0</horstretch>
-       <verstretch>0</verstretch>
-      </sizepolicy>
-     </property>
-     <property name="frame">
-      <bool>true</bool>
-     </property>
-     <property name="buttonSymbols">
-      <enum>QAbstractSpinBox::NoButtons</enum>
-     </property>
-     <property name="decimals">
-      <number>11</number>
-     </property>
-     <property name="minimum">
-      <double>-1000.000000000000000</double>
-     </property>
-     <property name="maximum">
-      <double>1000.000000000000000</double>
-     </property>
-     <property name="singleStep">
-      <double>0.000000000000000</double>
-     </property>
-    </widget>
-   </item>
-   <item row="11" column="1">
-    <widget class="QDoubleSpinBox" name="doubleSpinBox_5">
-     <property name="sizePolicy">
-      <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
-       <horstretch>0</horstretch>
-       <verstretch>0</verstretch>
-      </sizepolicy>
-     </property>
-     <property name="frame">
-      <bool>true</bool>
-     </property>
-     <property name="buttonSymbols">
-      <enum>QAbstractSpinBox::NoButtons</enum>
-     </property>
-     <property name="decimals">
-      <number>11</number>
-     </property>
-     <property name="minimum">
-      <double>-1000.000000000000000</double>
-     </property>
-     <property name="maximum">
-      <double>1000.000000000000000</double>
-     </property>
-     <property name="singleStep">
-      <double>0.000000000000000</double>
-     </property>
-    </widget>
-   </item>
-   <item row="8" column="1">
-    <widget class="QDoubleSpinBox" name="doubleSpinBox_2">
-     <property name="sizePolicy">
-      <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
-       <horstretch>0</horstretch>
-       <verstretch>0</verstretch>
-      </sizepolicy>
-     </property>
-     <property name="frame">
-      <bool>true</bool>
-     </property>
-     <property name="buttonSymbols">
-      <enum>QAbstractSpinBox::NoButtons</enum>
-     </property>
-     <property name="decimals">
-      <number>11</number>
-     </property>
-     <property name="minimum">
-      <double>-1000.000000000000000</double>
-     </property>
-     <property name="maximum">
-      <double>1000.000000000000000</double>
-     </property>
-     <property name="singleStep">
-      <double>0.000000000000000</double>
-     </property>
-    </widget>
-   </item>
-   <item row="6" column="2" rowspan="5" colspan="3">
+   <item row="6" column="2" rowspan="2" colspan="3">
     <widget class="QLabel" name="label_6">
      <property name="sizePolicy">
       <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
@@ -267,7 +120,7 @@
      </property>
     </widget>
    </item>
-   <item row="11" column="3" colspan="2">
+   <item row="8" column="3" colspan="2">
     <widget class="QDialogButtonBox" name="buttonBox">
      <property name="standardButtons">
       <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
@@ -378,6 +231,72 @@
      </layout>
     </widget>
    </item>
+   <item row="7" column="1" rowspan="2">
+    <widget class="QGroupBox" name="groupBox_2">
+     <property name="title">
+      <string>Head centroid position</string>
+     </property>
+     <property name="alignment">
+      <set>Qt::AlignCenter</set>
+     </property>
+     <layout class="QFormLayout" name="formLayout_2">
+      <property name="formAlignment">
+       <set>Qt::AlignHCenter|Qt::AlignTop</set>
+      </property>
+      <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="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="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="minimum">
+         <double>-200.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>200.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+   </item>
   </layout>
  </widget>
  <resources/>
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index b532ac9c..7ad80b46 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -120,8 +120,11 @@ void Tracker::load_settings()
 	enableTX = iniFile.value("enable-tx", true).toBool();
 	enableTY = iniFile.value("enable-ty", true).toBool();
 	enableTZ = iniFile.value("enable-tz", true).toBool();
-    for (int i = 0; i < 5; i++)
-        dc[i] = iniFile.value(QString("dc%1").arg(i), 0).toFloat();
+
+    for (int i = 0; i < 3; i++)
+    {
+        headpos[i] = iniFile.value(QString("headpos-%1").arg(i), 0).toDouble();
+    }
 	iniFile.endGroup();
 }
 
@@ -194,6 +197,7 @@ void Tracker::run()
     double error = 0;
     std::vector<cv::Point2f> reprojection;
     auto kernel = cv::createGaussianFilter(CV_8U, cv::Size(5, 5), 0);
+    cv::Point2f last_centroid;
     while (!stop)
     {
         if (!camera.read(color_))
@@ -218,7 +222,7 @@ void Tracker::run()
         cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1);
         
         for (int i = 0; i < 5; i++)
-            dist_coeffs.at<float>(i) = dc[i];
+            dist_coeffs.at<float>(i) = 0;
         
         std::vector< aruco::Marker > markers;
         
@@ -239,6 +243,8 @@ void Tracker::run()
                        3);
         }
 
+        cv::circle(frame, last_centroid, 4, cv::Scalar(0, 0, 0), -1);
+
         auto time = cv::getTickCount();
 
         if ((long) (time / freq) != (long) (last_time / freq))
@@ -271,18 +277,18 @@ void Tracker::run()
             const float size = 7;
             
             cv::Mat obj_points(4,3,CV_32FC1);
-            obj_points.at<float>(1,0)=-size;
-            obj_points.at<float>(1,1)=-size;
-            obj_points.at<float>(1,2)=0;
-            obj_points.at<float>(2,0)=size;
-            obj_points.at<float>(2,1)=-size;
-            obj_points.at<float>(2,2)=0;
-            obj_points.at<float>(3,0)=size;
-            obj_points.at<float>(3,1)=size;
-            obj_points.at<float>(3,2)=0;
-            obj_points.at<float>(0,0)=-size;
-            obj_points.at<float>(0,1)=size;
-            obj_points.at<float>(0,2)=0;
+            obj_points.at<float>(1,0)=-size + headpos[0];
+            obj_points.at<float>(1,1)=-size + headpos[1];
+            obj_points.at<float>(1,2)=0 + headpos[2];
+            obj_points.at<float>(2,0)=size + headpos[0];
+            obj_points.at<float>(2,1)=-size + headpos[1];
+            obj_points.at<float>(2,2)=0 + headpos[2];
+            obj_points.at<float>(3,0)=size + headpos[0];
+            obj_points.at<float>(3,1)=size + headpos[1];
+            obj_points.at<float>(3,2)=0 + headpos[2];
+            obj_points.at<float>(0,0)=-size + headpos[0];
+            obj_points.at<float>(0,1)=size + headpos[1];
+            obj_points.at<float>(0,2)=0 + headpos[2];
             
             cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, false, cv::ITERATIVE);
             
@@ -315,6 +321,14 @@ void Tracker::run()
                 error += std::sqrt(x * x + y * y);
             }
 
+            reprojection.clear();
+            reprojection.resize(1);
+            std::vector<cv::Point3f> centroid;
+            centroid.push_back(cv::Point3f(0, 0, 0));
+            cv::projectPoints(centroid, rvec, tvec, intrinsics, dist_coeffs, reprojection);
+
+            last_centroid = reprojection[0];
+
             //pose[Yaw] -= atan(pose[TX] / pose[TZ]) * 180 / HT_PI;
             //pose[Pitch] -= atan(pose[TY] / pose[TZ]) * 180 / HT_PI;
         }
@@ -414,6 +428,9 @@ TrackerControls::TrackerControls()
 	connect(ui.tx, SIGNAL(stateChanged(int)), this, SLOT(settingChanged(int)));
 	connect(ui.ty, SIGNAL(stateChanged(int)), this, SLOT(settingChanged(int)));
 	connect(ui.tz, SIGNAL(stateChanged(int)), this, SLOT(settingChanged(int)));
+    connect(ui.cx, SIGNAL(valueChanged(double)), this, SLOT(settingChanged(double)));
+    connect(ui.cy, SIGNAL(valueChanged(double)), this, SLOT(settingChanged(double)));
+    connect(ui.cz, SIGNAL(valueChanged(double)), this, SLOT(settingChanged(double)));
     //connect(ui.buttonCancel, SIGNAL(clicked()), this, SLOT(doCancel()));
     //connect(ui.buttonOK, SIGNAL(clicked()), this, SLOT(doOK()));
     //connect(ui.buttonSettings, SIGNAL(clicked()), this, SLOT(cameraSettings()));
@@ -474,13 +491,18 @@ void TrackerControls::loadSettings()
 	ui.ty->setCheckState(iniFile.value("enable-ty", true).toBool() ? Qt::Checked : Qt::Unchecked);
 	ui.tz->setCheckState(iniFile.value("enable-tz", true).toBool() ? Qt::Checked : Qt::Unchecked);
     ui.resolution->setCurrentIndex(iniFile.value("resolution", 0).toInt());
-    
-    ui.doubleSpinBox->setValue(iniFile.value("dc0").toDouble());
-    ui.doubleSpinBox_2->setValue(iniFile.value("dc1").toDouble());
-    ui.doubleSpinBox_3->setValue(iniFile.value("dc2").toDouble());
-    ui.doubleSpinBox_4->setValue(iniFile.value("dc3").toDouble());
-    ui.doubleSpinBox_5->setValue(iniFile.value("dc4").toDouble());
-    
+
+    QDoubleSpinBox* headpos[] = {
+        ui.cx,
+        ui.cy,
+        ui.cz
+    };
+
+    for (int i = 0; i < 3; i++)
+    {
+        headpos[i]->setValue(iniFile.value(QString("headpos-%1").arg(i)).toDouble());
+    }
+
 	iniFile.endGroup();
 	settingsDirty = false;
 }
@@ -519,13 +541,17 @@ 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("dc0", ui.doubleSpinBox->value());
-    iniFile.setValue("dc1", ui.doubleSpinBox_2->value());
-    iniFile.setValue("dc2", ui.doubleSpinBox_3->value());
-    iniFile.setValue("dc3", ui.doubleSpinBox_4->value());
-    iniFile.setValue("dc4", ui.doubleSpinBox_5->value());
-    
+
+    QDoubleSpinBox* headpos[] = {
+        ui.cx,
+        ui.cy,
+        ui.cz
+    };
+
+    for (int i = 0; i < 3; i++)
+    {
+        iniFile.setValue(QString("headpos-%1").arg(i), headpos[i]->value());
+    }
 	iniFile.endGroup();
 	settingsDirty = false;
 }
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
index 8d532b4c..5d3d4ed6 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
@@ -35,11 +35,11 @@ private:
     volatile bool stop;
     float fov;
     int camera_index;
-    float dc[5];
     int force_fps, force_width, force_height;
     void load_settings();
     double pose[6];
     cv::Mat frame;
+    double headpos[3];
 };
 
 // Widget that has controls for FTNoIR protocol client-settings.