accela: implement a third-order highpass filter

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

6 files changed, 166 insertions, 42 deletions

diff --git a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
index 5ba33614..b4b12061 100644
--- a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
+++ b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
@@ -10,7 +10,7 @@
     <x>0</x>
     <y>0</y>
     <width>347</width>
-    <height>268</height>
+    <height>339</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -109,7 +109,7 @@
          <enum>QAbstractSpinBox::CorrectToPreviousValue</enum>
         </property>
         <property name="decimals">
-         <number>3</number>
+         <number>4</number>
         </property>
         <property name="minimum">
          <double>0.100000000000000</double>
@@ -147,7 +147,7 @@
          <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
         </property>
         <property name="decimals">
-         <number>3</number>
+         <number>4</number>
         </property>
         <property name="minimum">
          <double>0.100000000000000</double>
@@ -160,7 +160,7 @@
         </property>
        </widget>
       </item>
-      <item row="2" column="0">
+      <item row="4" column="0">
        <widget class="QLabel" name="lblSensYaw_5">
         <property name="sizePolicy">
          <sizepolicy hsizetype="Fixed" vsizetype="Expanding">
@@ -189,7 +189,7 @@ background:none;</string>
         </property>
        </widget>
       </item>
-      <item row="2" column="1">
+      <item row="4" column="1">
        <widget class="QSpinBox" name="spinZoom">
         <property name="sizePolicy">
          <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
@@ -229,7 +229,7 @@ background:none;</string>
         </property>
        </widget>
       </item>
-      <item row="3" column="0">
+      <item row="5" column="0">
        <widget class="QLabel" name="lblSensYaw_6">
         <property name="sizePolicy">
          <sizepolicy hsizetype="Fixed" vsizetype="Expanding">
@@ -258,7 +258,7 @@ background:none;</string>
         </property>
        </widget>
       </item>
-      <item row="3" column="1">
+      <item row="5" column="1">
        <widget class="QDoubleSpinBox" name="deadzone">
         <property name="sizePolicy">
          <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
@@ -286,14 +286,14 @@ background:none;</string>
         </property>
        </widget>
       </item>
-      <item row="4" column="0">
+      <item row="6" column="0">
        <widget class="QLabel" name="label_10">
         <property name="text">
          <string>Exponent</string>
         </property>
        </widget>
       </item>
-      <item row="4" column="1">
+      <item row="6" column="1">
        <widget class="QDoubleSpinBox" name="expt">
         <property name="sizePolicy">
          <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
@@ -315,6 +315,76 @@ background:none;</string>
         </property>
        </widget>
       </item>
+      <item row="2" column="0">
+       <widget class="QLabel" name="label_11">
+        <property name="text">
+         <string>Order #2</string>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="1">
+       <widget class="QDoubleSpinBox" name="order_2nd">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
+          <horstretch>0</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
+        <property name="alignment">
+         <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+        </property>
+        <property name="correctionMode">
+         <enum>QAbstractSpinBox::CorrectToPreviousValue</enum>
+        </property>
+        <property name="decimals">
+         <number>4</number>
+        </property>
+        <property name="minimum">
+         <double>0.100000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>65535.000000000000000</double>
+        </property>
+        <property name="value">
+         <double>1.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="3" column="1">
+       <widget class="QDoubleSpinBox" name="order_3rd">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
+          <horstretch>0</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
+        <property name="alignment">
+         <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+        </property>
+        <property name="correctionMode">
+         <enum>QAbstractSpinBox::CorrectToPreviousValue</enum>
+        </property>
+        <property name="decimals">
+         <number>4</number>
+        </property>
+        <property name="minimum">
+         <double>0.100000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>65535.000000000000000</double>
+        </property>
+        <property name="value">
+         <double>1.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="3" column="0">
+       <widget class="QLabel" name="label_12">
+        <property name="text">
+         <string>Order #3</string>
+        </property>
+       </widget>
+      </item>
      </layout>
     </widget>
    </item>
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
index 9189ec8a..1f9b493f 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
@@ -34,7 +34,8 @@ void FTNoIR_Filter::loadSettings() {
     zoom_factor = iniFile.value("zoom-slowness", ACCELA_ZOOM_SLOWNESS).toDouble();

     rotation_alpha = iniFile.value("rotation-alpha", ACCELA_SMOOTHING_ROTATION).toDouble();

     translation_alpha = iniFile.value("translation-alpha", ACCELA_SMOOTHING_TRANSLATION).toDouble();

-

+    second_order_alpha = iniFile.value("second-order-alpha", ACCELA_SECOND_ORDER_ALPHA).toDouble();

+    third_order_alpha = iniFile.value("third-order-alpha", ACCELA_THIRD_ORDER_ALPHA).toDouble();

     deadzone = iniFile.value("deadzone", 0.0).toDouble();

     // bigger means less filtering

     static const double init_scaling[] = {

@@ -81,6 +82,8 @@ void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position,
         {

             new_camera_position[i] = target_camera_position[i];

             current_camera_position[i] = target_camera_position[i];

+            current_camera_position_2[i] = target_camera_position[i];

+            current_camera_position_3[i] = target_camera_position[i];

         }

 

 		first_run = false;

@@ -95,10 +98,19 @@ void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position,
 		const int sign = vec < 0 ? -1 : 1;

 		const double x = fabs(vec);

         const double a = i >= 3 ? rotation_alpha : translation_alpha;

+        const double a2 = a * second_order_alpha;

+        const double a3 = a * third_order_alpha;

+        const double x2 = fabs(target_camera_position[i] - current_camera_position_2[i]);

+        const double x3 = fabs(target_camera_position[i] - current_camera_position_3[i]);

         const double reduction = 1. / std::max(1., 1. + zoom_factor * -last_post_filter_values[TZ] / 1000);

-        const double velocity = parabola(a, x * scaling[i], deadzone, expt) * reduction;

+        const double velocity =

+                parabola(a, x * scaling[i], deadzone, expt) * reduction +

+                parabola(a2, x2 * scaling[i], deadzone, expt) * reduction +

+                parabola(a3, x3 * scaling[i], deadzone, expt) * reduction;

 		const double result = current_camera_position[i] + velocity * sign;

         const bool done = sign > 0 ? result >= target_camera_position[i] : result <= target_camera_position[i];

+        current_camera_position_3[i] = current_camera_position_2[i];

+        current_camera_position_2[i] = current_camera_position[i];

         new_camera_position[i] = current_camera_position[i] = done ? target_camera_position[i] : result;

 	}

 }

diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.h b/ftnoir_filter_accela/ftnoir_filter_accela.h
index 8c770df9..eec97758 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.h
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.h
@@ -33,7 +33,9 @@
 

 #define ACCELA_SMOOTHING_ROTATION 60.0

 #define ACCELA_SMOOTHING_TRANSLATION 40.0

-#define ACCELA_ZOOM_SLOWNESS 35

+#define ACCELA_ZOOM_SLOWNESS 0

+#define ACCELA_SECOND_ORDER_ALPHA 100.0

+#define ACCELA_THIRD_ORDER_ALPHA 180.0

 

 //*******************************************************************************************************

 // FaceTrackNoIR Filter class.

@@ -53,7 +55,10 @@ private:
 	void loadSettings();

 	bool first_run;

     double rotation_alpha, translation_alpha, zoom_factor;

+    double second_order_alpha, third_order_alpha;

     double current_camera_position[6];

+    double current_camera_position_2[6];

+    double current_camera_position_3[6];

     double scaling[6];

     double deadzone;

     double expt;

diff --git a/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp b/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp
index 88c0a794..68484e77 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp
+++ b/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp
@@ -160,6 +160,8 @@ void FilterControls::loadSettings() {
     ui.spinZoom->setValue(iniFile.value("zoom-slowness", ACCELA_ZOOM_SLOWNESS).toInt());
     ui.rotation_alpha->setValue(iniFile.value("rotation-alpha", ACCELA_SMOOTHING_ROTATION).toDouble());
     ui.translation_alpha->setValue(iniFile.value("translation-alpha", ACCELA_SMOOTHING_TRANSLATION).toDouble());
+    ui.order_2nd->setValue(iniFile.value("second-order-alpha", ACCELA_SECOND_ORDER_ALPHA).toDouble());
+    ui.order_3rd->setValue(iniFile.value("third-order-alpha", ACCELA_THIRD_ORDER_ALPHA).toDouble());
     ui.deadzone->setValue(iniFile.value("deadzone", 0).toDouble());
 
     // bigger means less filtering
@@ -212,6 +214,8 @@ void FilterControls::save() {
     iniFile.setValue("zoom-slowness", zoom = ui.spinZoom->value());
     iniFile.setValue("deadzone", deadzone = ui.deadzone->value());
     iniFile.setValue("exponent", expt = ui.expt->value());
+    iniFile.setValue("second-order-alpha", rot = ui.order_2nd->value());
+    iniFile.setValue("third-order-alpha", rot = ui.order_3rd->value());
 
     QDoubleSpinBox* boxen[] = {
         ui.doubleSpinBox,
diff --git a/ftnoir_filter_kalman/ftnoir_filter_kalman.h b/ftnoir_filter_kalman/ftnoir_filter_kalman.h
index 59169612..6c2cb6a9 100644
--- a/ftnoir_filter_kalman/ftnoir_filter_kalman.h
+++ b/ftnoir_filter_kalman/ftnoir_filter_kalman.h
@@ -19,25 +19,29 @@
 #include <QString>
 #include <QIcon>
 #include <QWidget>
+#include <QElapsedTimer>
 #include <QObject>
 
 class FTNOIR_FILTER_BASE_EXPORT FTNoIR_Filter : public IFilter
 {
 public:
     FTNoIR_Filter();
-    virtual ~FTNoIR_Filter() {
+    ~FTNoIR_Filter() virt_override {
     }
-    void Initialize();
-    void FilterHeadPoseData(double *current_camera_position,
-                            double *target_camera_position,
+    void Initialize() virt_override;
+    void FilterHeadPoseData(const double *target_camera_position,
                             double *new_camera_position,
-                            double *last_post_filter_values);
+                            const double *) virt_override;
     cv::KalmanFilter kalman;
     double prev_position[6];
+    double prev2_filter_pos[6];
+    double prev_filter_pos[6];
+    QElapsedTimer timer;
+    qint64 timedelta;
 };
 
-void kalman_load_settings(FTNoIR_Filter& self);
-void kalman_save_settings(FTNoIR_Filter& self);
+void kalman_load_settings(FTNoIR_Filter&);
+void kalman_save_settings(FTNoIR_Filter&);
 
 class FTNOIR_FILTER_BASE_EXPORT FTNoIR_FilterDll : public Metadata
 {
@@ -48,7 +52,7 @@ public:
     void getIcon(QIcon *icon){ *icon = QIcon(":/images/filter-16.png"); }
 };
 
-class FTNOIR_FILTER_BASE_EXPORT FilterControls: public QWidget, Ui::KalmanUICFilterControls, public IFilterDialog
+class FTNOIR_FILTER_BASE_EXPORT FilterControls: public QWidget, public IFilterDialog
 {
     Q_OBJECT
 public:
@@ -65,21 +69,24 @@ public:
         connect(ui.btnCancel, SIGNAL(clicked()), this, SLOT(doCancel()));
         show();
     }
-    virtual ~FilterControls() {}
-    void showEvent ( QShowEvent * event ) {
+    ~FilterControls() {}
+    void showEvent ( QShowEvent * ) virt_override {
         show();
     }
     
-    void Initialize(QWidget *parent, IFilter* ptr) {
+    void Initialize(QWidget *) virt_override {
+        show();
+        raise();
     }
     
     bool settingsDirty;
     Ui::KalmanUICFilterControls ui;
-    
+    virtual void registerFilter(IFilter*) virt_override {}
+    virtual void unregisterFilter() virt_override {}
 public slots:
     void doOK();
     void doCancel();
-    void settingsChanged(double unused) {
+    void settingsChanged(double) {
         settingsDirty = true;
     }
 };
diff --git a/ftnoir_filter_kalman/kalman.cpp b/ftnoir_filter_kalman/kalman.cpp
index 51af35e1..5ecd417c 100644
--- a/ftnoir_filter_kalman/kalman.cpp
+++ b/ftnoir_filter_kalman/kalman.cpp
@@ -9,7 +9,7 @@
 #include <QDebug>
 #include <math.h>
 
-void kalman_load_settings(FTNoIR_Filter& self) {
+void kalman_load_settings(FTNoIR_Filter&) {
     QSettings settings("Abbequerque Inc.", "FaceTrackNoIR");    // Registry settings (in HK_USER)
     
     QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString();
@@ -19,7 +19,7 @@ void kalman_load_settings(FTNoIR_Filter& self) {
     iniFile.endGroup();
 }
 
-void kalman_save_settings(FilterControls& self) {
+void kalman_save_settings(FilterControls&) {
     QSettings settings("Abbequerque Inc.", "FaceTrackNoIR");    // Registry settings (in HK_USER)
     
     QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString();
@@ -37,8 +37,8 @@ FTNoIR_Filter::FTNoIR_Filter() {
 // the following was written by Donovan Baarda <abo@minkirri.apana.org.au>
 // https://sourceforge.net/p/facetracknoir/discussion/1150909/thread/418615e1/?limit=25#af75/084b
 void FTNoIR_Filter::Initialize() {
-    const double accel_variance = 1e-4;
-    const double noise_variance = 1e1;
+    const double accel_variance = 1e-3;
+    const double noise_variance = 5e2;
     kalman.init(12, 6, 0, CV_64F);
     kalman.transitionMatrix = (cv::Mat_<double>(12, 12) <<
     1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
@@ -73,13 +73,28 @@ void FTNoIR_Filter::Initialize() {
     cv::setIdentity(kalman.measurementNoiseCov, cv::Scalar::all(noise_variance));
     cv::setIdentity(kalman.errorCovPost, cv::Scalar::all(accel_variance * 1e4));
     for (int i = 0; i < 6; i++)
+    {
         prev_position[i] = 0;
+        prev2_filter_pos[i] = 0;
+        prev_filter_pos[i] = 0;
+        timedelta = 1;
+        timer.invalidate();
+    }
+}
+
+template<typename T>
+static inline T clamp(const T min, const T max, const T value)
+{
+    if (value < min)
+        return min;
+    if (value > max)
+        return max;
+    return value;
 }
 
-void FTNoIR_Filter::FilterHeadPoseData(double *current_camera_position,
-                                       double *target_camera_position,
+void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position,
                                        double *new_camera_position,
-                                       double *last_post_filter_values)
+                                       const double *)
 {
     bool new_target = false;
     
@@ -89,23 +104,34 @@ void FTNoIR_Filter::FilterHeadPoseData(double *current_camera_position,
             new_target = true;
             break;
         }
-    
-    cv::Mat output = kalman.predict();
-    
+
     if (new_target) {
+        cv::Mat output = kalman.predict();
         cv::Mat measurement(6, 1, CV_64F);
         for (int i = 0; i < 3; i++) {
             measurement.at<double>(i) = target_camera_position[i+3];
             measurement.at<double>(i+3) = target_camera_position[i];
         }
         kalman.correct(measurement);
-    }
-    
-    for (int i = 0; i < 3; i++) {
-        new_camera_position[i] = output.at<double>(i+3);
-        new_camera_position[i+3] = output.at<double>(i);
-        prev_position[i] = target_camera_position[i];
-        prev_position[i+3] = target_camera_position[i+3];
+        for (int i = 0; i < 6; i++)
+        {
+            prev_position[i] = target_camera_position[i];
+        }
+        if (timer.isValid())
+            timedelta = timer.elapsed();
+        else
+            timedelta = 1;
+        for (int i = 0; i < 6; i++)
+        {
+            prev2_filter_pos[i] = prev_filter_pos[i];
+            prev_filter_pos[i] = new_camera_position[i] = output.at<double>((i + 3) % 6);
+        }
+        timer.start();
+    } else {
+        auto d = timer.isValid() ? timer.elapsed() : 1;
+        auto c = clamp(0.0, 1.0, d / (double) timedelta);
+        for (int i = 0; i < 6; i++)
+            new_camera_position[i] = prev2_filter_pos[i] + (prev_filter_pos[i] - prev2_filter_pos[i]) * c;
     }
 }