implement optional aruco frame smoothing ewma scheme

3 files changed, 233 insertions, 132 deletions

diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
index d4b64c23..340ed0ad 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>308</height>
+    <height>399</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -32,7 +32,106 @@
    <property name="spacing">
     <number>-1</number>
    </property>
-   <item row="7" column="1" rowspan="2">
+   <item row="0" column="0">
+    <widget class="QLabel" name="label">
+     <property name="text">
+      <string>Horizontal FOV</string>
+     </property>
+    </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>-1</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="1" column="0">
+    <widget class="QLabel" name="label_2">
+     <property name="text">
+      <string>Frames per second</string>
+     </property>
+    </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="8" column="1">
     <widget class="QGroupBox" name="groupBox_2">
      <property name="title">
       <string>Head centroid position</string>
@@ -124,79 +223,6 @@
      </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>-1</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="6" column="0">
-    <widget class="QLabel" name="label_4">
-     <property name="text">
-      <string>Resolution</string>
-     </property>
-    </widget>
-   </item>
    <item row="1" column="1" colspan="2">
     <widget class="QComboBox" name="cameraFPS">
      <item>
@@ -221,48 +247,14 @@
      </item>
     </widget>
    </item>
-   <item row="1" column="0">
-    <widget class="QLabel" name="label_2">
+   <item row="6" column="0">
+    <widget class="QLabel" name="label_4">
      <property name="text">
-      <string>Frames per second</string>
-     </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="8" column="3" colspan="2">
-    <widget class="QDialogButtonBox" name="buttonBox">
-     <property name="standardButtons">
-      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
+      <string>Resolution</string>
      </property>
     </widget>
    </item>
-   <item row="2" column="1" colspan="2">
-    <widget class="QComboBox" name="cameraName"/>
-   </item>
-   <item row="6" column="2" rowspan="2" colspan="3">
+   <item row="6" column="2" rowspan="3" colspan="3">
     <widget class="QLabel" name="label_6">
      <property name="sizePolicy">
       <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
@@ -293,19 +285,34 @@
      </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>
+   <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="9" column="4">
+    <widget class="QDialogButtonBox" name="buttonBox">
+     <property name="standardButtons">
+      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
      </property>
     </widget>
    </item>
@@ -316,10 +323,54 @@
      </property>
     </widget>
    </item>
-   <item row="0" column="0">
-    <widget class="QLabel" name="label">
+   <item row="7" column="1">
+    <widget class="QComboBox" name="ewma">
+     <item>
+      <property name="text">
+       <string>0</string>
+      </property>
+     </item>
+     <item>
+      <property name="text">
+       <string>1</string>
+      </property>
+     </item>
+     <item>
+      <property name="text">
+       <string>2</string>
+      </property>
+     </item>
+     <item>
+      <property name="text">
+       <string>3</string>
+      </property>
+     </item>
+     <item>
+      <property name="text">
+       <string>4</string>
+      </property>
+     </item>
+     <item>
+      <property name="text">
+       <string>5</string>
+      </property>
+     </item>
+     <item>
+      <property name="text">
+       <string>6</string>
+      </property>
+     </item>
+     <item>
+      <property name="text">
+       <string>7</string>
+      </property>
+     </item>
+    </widget>
+   </item>
+   <item row="7" column="0">
+    <widget class="QLabel" name="label_10">
      <property name="text">
-      <string>Horizontal FOV</string>
+      <string>Frame EWMA</string>
      </property>
     </widget>
    </item>
@@ -340,7 +391,6 @@
   <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 9a8c09d5..fa83955c 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -16,7 +16,7 @@
 #include <opencv/highgui.h>
 #include <vector>
 #include <cstdio>
-#include <memory>
+#include <vector>
 
 #if defined(_WIN32)
 #   define NO_DSHOW_STRSAFE
@@ -129,12 +129,14 @@ void Tracker::load_settings()
         headpos[i] = iniFile.value(QString("headpos-%1").arg(i), 0).toDouble();
     }
     headpitch = iniFile.value("pitch", 0).toDouble();
+    N_hyst = iniFile.value("ewma", 0).toInt();
 
 	iniFile.endGroup();
 }
 
 Tracker::Tracker()
 {
+    N_hyst = 0;
     layout = nullptr;
     stop = false;
 	videoWidget = NULL;
@@ -192,7 +194,7 @@ void Tracker::run()
 
     cv::Mat color, color_, grayscale, rvec, tvec;
 
-    const double stateful_coeff = 0.9;
+    const double stateful_coeff = 0.88;
     
     if (!camera.isOpened())
     {
@@ -209,6 +211,29 @@ void Tracker::run()
     int fps = 0;
     int last_fps = 0;
     cv::Point2f last_centroid;
+
+    bool first_run = true;
+
+    std::vector<cv::Mat> lasts;
+
+    for (int i = 0; i < N_hyst; i++)
+        lasts.push_back(cv::Mat());
+
+    vector<float> weights;
+    {
+        const float a = 0.32;
+        float sum = 0;
+        float k = 1;
+        for (int i = 0; i < N_hyst; i++)
+        {
+            sum += k;
+            weights.push_back(k);
+            k *= 1./(a*N_hyst);
+        }
+        for (int i = 0; i < N_hyst; i++)
+            qDebug() << i << "w" << (weights[i] /= sum);
+    }
+
     while (!stop)
     {
         if (!camera.read(color_))
@@ -217,6 +242,28 @@ void Tracker::run()
         color_.copyTo(color);
         cv::cvtColor(color, grayscale, cv::COLOR_BGR2GRAY);
 
+        if (first_run)
+        {
+            first_run = false;
+            for (int i = 0; i < N_hyst; i++)
+                lasts[i] = grayscale;
+        }
+        if (N_hyst > 0)
+        {
+            cv::Mat hyst(grayscale.rows, grayscale.cols, CV_32F);
+            hyst.setTo(0);
+            for (int i = 0; i < N_hyst-1; i++)
+                lasts[i] = lasts[i+1];
+            lasts[N_hyst-1] = grayscale;
+            for (int i = 0; i < N_hyst; i++)
+                for (int y = 0; y < grayscale.rows; y++)
+                    for (int x = 0; x < grayscale.cols; x++)
+                        hyst.at<float>(y, x) += (float) lasts[i].at<unsigned char>(y, x) * weights[i];
+            hyst.convertTo(grayscale, CV_8U);
+        }
+
+        cv::cvtColor(grayscale, color, cv::COLOR_GRAY2BGR);
+
         const int scale = frame.cols > 480 ? 2 : 1;
         detector.setThresholdParams(scale > 1 ? 11 : 7, 4);
 
@@ -543,6 +590,7 @@ void TrackerControls::loadSettings()
     }
 
     ui.pitch_deg->setValue(iniFile.value("pitch", 0).toDouble());
+    ui.ewma->setCurrentIndex(iniFile.value("ewma", 0).toInt());
 
 	iniFile.endGroup();
 	settingsDirty = false;
@@ -583,6 +631,7 @@ void TrackerControls::save()
 	iniFile.setValue("enable-tz", ui.tz->checkState() != Qt::Unchecked ? true : false);
 	iniFile.setValue("resolution", ui.resolution->currentIndex());
     iniFile.setValue("pitch", ui.pitch_deg->value());
+    iniFile.setValue("ewma", ui.ewma->currentIndex());
 
     QDoubleSpinBox* headpos[] = {
         ui.cx,
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
index be2ad3d7..249c8829 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
@@ -42,6 +42,8 @@ private:
     cv::Mat frame;
     double headpos[3], headpitch;
     cv::VideoCapture camera;
+    int N_hyst;
+    cv::Mat lasts;
 };
 
 // Widget that has controls for FTNoIR protocol client-settings.