From 3faf980973e03b049782027f7aafc9a7f4c9c39e Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Sun, 12 Jul 2015 11:46:58 +0200
Subject: kalman: add settings support

Filter performance still not good though.
---
 ftnoir_filter_kalman/ftnoir_filter_kalman.h        | 19 ++++-
 .../ftnoir_kalman_filtercontrols.ui                | 85 ++++++++++++++--------
 ftnoir_filter_kalman/kalman.cpp                    | 11 ++-
 3 files changed, 80 insertions(+), 35 deletions(-)

diff --git a/ftnoir_filter_kalman/ftnoir_filter_kalman.h b/ftnoir_filter_kalman/ftnoir_filter_kalman.h
index f6224530..1f0b36fd 100755
--- a/ftnoir_filter_kalman/ftnoir_filter_kalman.h
+++ b/ftnoir_filter_kalman/ftnoir_filter_kalman.h
@@ -19,6 +19,14 @@
 #include "opentrack/options.hpp"
 using namespace options;
 
+struct settings : opts {
+    value<int> noise_stddev_slider;
+    // slider goes noise_stdder 0->50
+    static constexpr double mult_noise_stddev = .5;
+    settings() : opts("kalman-filter"), noise_stddev_slider(b, "noise-stddev", 10)
+    {}
+};
+
 class OPENTRACK_EXPORT FTNoIR_Filter : public IFilter
 {
 public:
@@ -28,10 +36,12 @@ public:
     // Set accel_stddev assuming moving 0.0->100.0 in dt=0.2 is 3 stddevs: (100.0*4/dt^2)/3.
     const double accel_stddev = (100.0*4/(0.2*0.2))/3.0;
     // TODO(abo): make noise_stddev a UI setting 0.0->10.0 with 0.1 resolution.
-    const double noise_stddev = 1.0;
+    //const double noise_stddev = 1.0;
     cv::KalmanFilter kalman;
     double last_input[6];
     QElapsedTimer timer;
+    settings s;
+    int prev_slider_pos;
 };
 
 class OPENTRACK_EXPORT FTNoIR_FilterDll : public Metadata
@@ -47,13 +57,14 @@ class OPENTRACK_EXPORT FilterControls: public IFilterDialog
 public:
     FilterControls() {
         ui.setupUi(this);
-        connect(ui.btnOk, SIGNAL(clicked()), this, SLOT(doOK()));
-        connect(ui.btnCancel, SIGNAL(clicked()), this, SLOT(doCancel()));
-        show();
+        connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
+        connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
+        tie_setting(s.noise_stddev_slider, ui.noise_slider);
     }
     Ui::KalmanUICFilterControls ui;
     void register_filter(IFilter*) override {}
     void unregister_filter() override {}
+    settings s;
 public slots:
     void doOK();
     void doCancel();
diff --git a/ftnoir_filter_kalman/ftnoir_kalman_filtercontrols.ui b/ftnoir_filter_kalman/ftnoir_kalman_filtercontrols.ui
index e0c849c9..69297102 100644
--- a/ftnoir_filter_kalman/ftnoir_kalman_filtercontrols.ui
+++ b/ftnoir_filter_kalman/ftnoir_kalman_filtercontrols.ui
@@ -9,8 +9,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>334</width>
-    <height>100</height>
+    <width>288</width>
+    <height>132</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -23,7 +23,7 @@
    <string>Kalman settings</string>
   </property>
   <property name="windowIcon">
-   <iconset resource="../ftnoir_filter_ewma2/ewma-filter.qrc">
+   <iconset resource="../facetracknoir/ui-res.qrc">
     <normaloff>:/images/filter-16.png</normaloff>:/images/filter-16.png</iconset>
   </property>
   <property name="layoutDirection">
@@ -35,35 +35,60 @@
   <property name="styleSheet">
    <string notr="true"/>
   </property>
-  <widget class="QPushButton" name="btnOk">
-   <property name="geometry">
-    <rect>
-     <x>173</x>
-     <y>70</y>
-     <width>73</width>
-     <height>25</height>
-    </rect>
-   </property>
-   <property name="text">
-    <string>OK</string>
-   </property>
-  </widget>
-  <widget class="QPushButton" name="btnCancel">
-   <property name="geometry">
-    <rect>
-     <x>250</x>
-     <y>70</y>
-     <width>73</width>
-     <height>25</height>
-    </rect>
-   </property>
-   <property name="text">
-    <string>Cancel</string>
-   </property>
-  </widget>
+  <layout class="QVBoxLayout" name="verticalLayout">
+   <item>
+    <widget class="QFrame" name="frame">
+     <property name="frameShape">
+      <enum>QFrame::NoFrame</enum>
+     </property>
+     <property name="frameShadow">
+      <enum>QFrame::Raised</enum>
+     </property>
+     <layout class="QGridLayout" name="gridLayout">
+      <item row="0" column="0">
+       <widget class="QLabel" name="label">
+        <property name="text">
+         <string>Noise standard deviation</string>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="1">
+       <widget class="QSlider" name="noise_slider">
+        <property name="singleStep">
+         <number>1</number>
+        </property>
+        <property name="orientation">
+         <enum>Qt::Horizontal</enum>
+        </property>
+        <property name="tickPosition">
+         <enum>QSlider::TicksAbove</enum>
+        </property>
+        <property name="tickInterval">
+         <number>10</number>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item>
+    <widget class="QLabel" name="label_2">
+     <property name="text">
+      <string>Written by Donovan Baarda, 2014</string>
+     </property>
+    </widget>
+   </item>
+   <item>
+    <widget class="QDialogButtonBox" name="buttonBox">
+     <property name="standardButtons">
+      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
+     </property>
+    </widget>
+   </item>
+  </layout>
  </widget>
  <resources>
-  <include location="../ftnoir_filter_ewma2/ewma-filter.qrc"/>
+  <include location="../facetracknoir/ui-res.qrc"/>
  </resources>
  <connections/>
  <designerdata>
diff --git a/ftnoir_filter_kalman/kalman.cpp b/ftnoir_filter_kalman/kalman.cpp
index c5a600af..4939610b 100644
--- a/ftnoir_filter_kalman/kalman.cpp
+++ b/ftnoir_filter_kalman/kalman.cpp
@@ -11,6 +11,7 @@
 
 FTNoIR_Filter::FTNoIR_Filter() {
     reset();
+    prev_slider_pos = s.noise_stddev_slider;
 }
 
 // the following was written by Donovan Baarda <abo@minkirri.apana.org.au>
@@ -56,7 +57,8 @@ void FTNoIR_Filter::reset() {
     0, 0, 0, 0, b, 0, 0, 0, 0, 0, a, 0,
     0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, a);
     cv::setIdentity(kalman.measurementMatrix);
-    double noise_variance = noise_stddev * noise_stddev;
+    const double noise_stddev = s.noise_stddev_slider * s.mult_noise_stddev;
+    const double noise_variance = noise_stddev * noise_stddev;
     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++) {
@@ -67,6 +69,11 @@ void FTNoIR_Filter::reset() {
 
 void FTNoIR_Filter::filter(const double* input, double *output)
 {
+    if (prev_slider_pos != s.noise_stddev_slider)
+    {
+        reset();
+        prev_slider_pos = s.noise_stddev_slider;
+    }
     // Start the timer if it's not running.
     if (!timer.isValid())
         timer.start();
@@ -108,10 +115,12 @@ void FTNoIR_Filter::filter(const double* input, double *output)
 }
 
 void FilterControls::doOK() {
+    s.b->save();
     close();
 }
 
 void FilterControls::doCancel() {
+    s.b->reload();
     close();
 }
 
-- 
cgit v1.2.3