Revert "Remove Kalman filter again, unsuitable for nonlinear systems"

This reverts commit d20c56f439c05528967b2bc408a6594a5605675e.

With best regards to dbaarda :-)

Conflicts:
	CMakeLists.txt

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

6 files changed, 475 insertions, 0 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 420114bb..e0b0f017 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -196,6 +196,13 @@ file(GLOB opentrack-filter-accela-rc "ftnoir_filter_accela/*.qrc")
 QT5_WRAP_UI(opentrack-filter-accela-uih ${opentrack-filter-accela-ui})
 QT5_ADD_RESOURCES(opentrack-filter-accela-rcc ${opentrack-filter-accela-rc})
 
+file(GLOB opentrack-filter-kalman-c "ftnoir_filter_kalman/*.cpp")
+file(GLOB opentrack-filter-kalman-h "ftnoir_filter_kalman/*.h")
+file(GLOB opentrack-filter-kalman-ui "ftnoir_filter_kalman/*.ui")
+file(GLOB opentrack-filter-kalman-rc "ftnoir_filter_kalman/*.qrc")
+QT5_WRAP_UI(opentrack-filter-kalman-uih ${opentrack-filter-kalman-ui})
+QT5_ADD_RESOURCES(opentrack-filter-kalman-rcc ${opentrack-filter-kalman-rc})
+
 file(GLOB opentrack-filter-ewma-c "ftnoir_filter_ewma2/*.cpp")
 file(GLOB opentrack-filter-ewma-h "ftnoir_filter_ewma2/*.h")
 file(GLOB opentrack-filter-ewma-ui "ftnoir_filter_ewma2/*.ui")
@@ -402,6 +409,14 @@ target_link_libraries(opentrack-spline-widget ${MY_QT_LIBS})
 add_library(opentrack-filter-accela SHARED ${opentrack-filter-accela-c} ${opentrack-filter-accela-h} ${opentrack-filter-accela-moc} ${opentrack-filter-accela-uih} ${opentrack-filter-accela-rcc})
 target_link_libraries(opentrack-filter-accela ${MY_QT_LIBS})
 
+add_library(opentrack-filter-kalman SHARED ${opentrack-filter-kalman-c} ${opentrack-filter-kalman-h} ${opentrack-filter-kalman-moc} ${opentrack-filter-kalman-uih} ${opentrack-filter-kalman-rcc})
+target_link_libraries(opentrack-filter-kalman ${MY_QT_LIBS} ${OpenCV_LIBS})
+
+if(CMAKE_COMPILER_IS_GNUCXX)
+    SET_TARGET_PROPERTIES(opentrack-filter-kalman
+    PROPERTIES LINK_FLAGS "-Wl,--version-script=${CMAKE_SOURCE_DIR}/facetracknoir/${version-script}-version-script.txt")
+endif()
+
 if(CMAKE_COMPILER_IS_GNUCXX)
     SET_TARGET_PROPERTIES(opentrack-filter-accela
     PROPERTIES LINK_FLAGS "-Wl,--version-script=${CMAKE_SOURCE_DIR}/facetracknoir/${version-script}-version-script.txt")
@@ -766,6 +781,7 @@ install(TARGETS
         opentrack-pose-widget
         opentrack-spline-widget
         opentrack-filter-accela
+        opentrack-filter-kalman
         opentrack-filter-ewma
         opentrack-tracker-ht
         opentrack-tracker-udp
diff --git a/ftnoir_filter_kalman/ftnoir_filter_kalman.h b/ftnoir_filter_kalman/ftnoir_filter_kalman.h
new file mode 100644
index 00000000..59169612
--- /dev/null
+++ b/ftnoir_filter_kalman/ftnoir_filter_kalman.h
@@ -0,0 +1,87 @@
+#pragma once
+/* Copyright (c) 2013 Stanisław Halik <sthalik@misaki.pl>
+ * 
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ */
+#ifndef INCLUDED_FTN_FILTER_H
+#define INCLUDED_FTN_FILTER_H
+
+#undef FTNOIR_TRACKER_BASE_LIB
+#define FTNOIR_TRACKER_BASE_EXPORT Q_DECL_IMPORT
+
+#include "ftnoir_filter_base/ftnoir_filter_base.h"
+#include "ui_ftnoir_kalman_filtercontrols.h"
+#include "facetracknoir/global-settings.h"
+#include <opencv2/opencv.hpp>
+#include <vector>
+#include <QString>
+#include <QIcon>
+#include <QWidget>
+#include <QObject>
+
+class FTNOIR_FILTER_BASE_EXPORT FTNoIR_Filter : public IFilter
+{
+public:
+    FTNoIR_Filter();
+    virtual ~FTNoIR_Filter() {
+    }
+    void Initialize();
+    void FilterHeadPoseData(double *current_camera_position,
+                            double *target_camera_position,
+                            double *new_camera_position,
+                            double *last_post_filter_values);
+    cv::KalmanFilter kalman;
+    double prev_position[6];
+};
+
+void kalman_load_settings(FTNoIR_Filter& self);
+void kalman_save_settings(FTNoIR_Filter& self);
+
+class FTNOIR_FILTER_BASE_EXPORT FTNoIR_FilterDll : public Metadata
+{
+public:
+    void getFullName(QString *strToBeFilled) { *strToBeFilled = QString("Kalman filter"); }
+    void getShortName(QString *strToBeFilled) { *strToBeFilled = QString("Kalman filter"); }
+    void getDescription(QString *strToBeFilled) { *strToBeFilled = QString("Kalman filter"); }
+    void getIcon(QIcon *icon){ *icon = QIcon(":/images/filter-16.png"); }
+};
+
+class FTNOIR_FILTER_BASE_EXPORT FilterControls: public QWidget, Ui::KalmanUICFilterControls, public IFilterDialog
+{
+    Q_OBJECT
+public:
+    explicit FilterControls() : settingsDirty(false) {
+        ui.setupUi(this);
+        QSettings settings("Abbequerque Inc.", "FaceTrackNoIR");    // Registry settings (in HK_USER)
+        
+        QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString();
+        QSettings iniFile( currentFile, QSettings::IniFormat );     // Application settings (in INI-file)
+        
+        iniFile.beginGroup("ftnoir-filter-kalman");
+        iniFile.endGroup();
+        connect(ui.btnOk, SIGNAL(clicked()), this, SLOT(doOK()));
+        connect(ui.btnCancel, SIGNAL(clicked()), this, SLOT(doCancel()));
+        show();
+    }
+    virtual ~FilterControls() {}
+    void showEvent ( QShowEvent * event ) {
+        show();
+    }
+    
+    void Initialize(QWidget *parent, IFilter* ptr) {
+    }
+    
+    bool settingsDirty;
+    Ui::KalmanUICFilterControls ui;
+    
+public slots:
+    void doOK();
+    void doCancel();
+    void settingsChanged(double unused) {
+        settingsDirty = true;
+    }
+};
+
+#endif
diff --git a/ftnoir_filter_kalman/ftnoir_kalman_filtercontrols.ui b/ftnoir_filter_kalman/ftnoir_kalman_filtercontrols.ui
new file mode 100644
index 00000000..7b71712a
--- /dev/null
+++ b/ftnoir_filter_kalman/ftnoir_kalman_filtercontrols.ui
@@ -0,0 +1,211 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>KalmanUICFilterControls</class>
+ <widget class="QWidget" name="KalmanUICFilterControls">
+  <property name="windowModality">
+   <enum>Qt::ApplicationModal</enum>
+  </property>
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>334</width>
+    <height>100</height>
+   </rect>
+  </property>
+  <property name="sizePolicy">
+   <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+    <horstretch>0</horstretch>
+    <verstretch>0</verstretch>
+   </sizepolicy>
+  </property>
+  <property name="windowTitle">
+   <string>Filter settings</string>
+  </property>
+  <property name="windowIcon">
+   <iconset>
+    <normaloff>
+    images/facetracknoir.png</normaloff>
+    images/facetracknoir.png</iconset>
+  </property>
+  <property name="layoutDirection">
+   <enum>Qt::LeftToRight</enum>
+  </property>
+  <property name="autoFillBackground">
+   <bool>false</bool>
+  </property>
+  <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>
+  <widget class="QLabel" name="label_2">
+   <property name="geometry">
+    <rect>
+     <x>9</x>
+     <y>30</y>
+     <width>169</width>
+     <height>16</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>process-noise-covariance</string>
+   </property>
+  </widget>
+  <widget class="QDoubleSpinBox" name="post">
+   <property name="geometry">
+    <rect>
+     <x>180</x>
+     <y>26</y>
+     <width>150</width>
+     <height>22</height>
+    </rect>
+   </property>
+   <property name="alignment">
+    <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
+   </property>
+   <property name="decimals">
+    <number>14</number>
+   </property>
+   <property name="maximum">
+    <double>1.000000000000000</double>
+   </property>
+   <property name="singleStep">
+    <double>0.000001000000000</double>
+   </property>
+   <property name="value">
+    <double>0.500000000000000</double>
+   </property>
+  </widget>
+  <widget class="QDoubleSpinBox" name="pnoise">
+   <property name="geometry">
+    <rect>
+     <x>180</x>
+     <y>6</y>
+     <width>150</width>
+     <height>22</height>
+    </rect>
+   </property>
+   <property name="alignment">
+    <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
+   </property>
+   <property name="decimals">
+    <number>14</number>
+   </property>
+   <property name="maximum">
+    <double>1.000000000000000</double>
+   </property>
+   <property name="singleStep">
+    <double>0.000001000000000</double>
+   </property>
+   <property name="value">
+    <double>0.500000000000000</double>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label">
+   <property name="geometry">
+    <rect>
+     <x>9</x>
+     <y>10</y>
+     <width>165</width>
+     <height>16</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>post-error-matrix</string>
+   </property>
+   <property name="alignment">
+    <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
+   </property>
+   <property name="margin">
+    <number>0</number>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_3">
+   <property name="geometry">
+    <rect>
+     <x>9</x>
+     <y>55</y>
+     <width>109</width>
+     <height>16</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>accel-coefficient</string>
+   </property>
+  </widget>
+  <widget class="QDoubleSpinBox" name="accl">
+   <property name="geometry">
+    <rect>
+     <x>181</x>
+     <y>47</y>
+     <width>150</width>
+     <height>22</height>
+    </rect>
+   </property>
+   <property name="alignment">
+    <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
+   </property>
+   <property name="decimals">
+    <number>6</number>
+   </property>
+   <property name="maximum">
+    <double>1000000000.000000000000000</double>
+   </property>
+   <property name="singleStep">
+    <double>0.000001000000000</double>
+   </property>
+   <property name="value">
+    <double>1.000000000000000</double>
+   </property>
+  </widget>
+ </widget>
+ <resources/>
+ <connections/>
+ <designerdata>
+  <property name="gridDeltaX">
+   <number>10</number>
+  </property>
+  <property name="gridDeltaY">
+   <number>10</number>
+  </property>
+  <property name="gridSnapX">
+   <bool>false</bool>
+  </property>
+  <property name="gridSnapY">
+   <bool>false</bool>
+  </property>
+  <property name="gridVisible">
+   <bool>true</bool>
+  </property>
+ </designerdata>
+ <slots>
+  <slot>startEngineClicked()</slot>
+  <slot>stopEngineClicked()</slot>
+  <slot>cameraSettingsClicked()</slot>
+ </slots>
+</ui>
diff --git a/ftnoir_filter_kalman/images/filter-16-ac.png b/ftnoir_filter_kalman/images/filter-16-ac.png
new file mode 100644
index 00000000..d263db2d
--- /dev/null
+++ b/ftnoir_filter_kalman/images/filter-16-ac.png
diff --git a/ftnoir_filter_kalman/kalman-filter.qrc b/ftnoir_filter_kalman/kalman-filter.qrc
new file mode 100644
index 00000000..9a7d75fa
--- /dev/null
+++ b/ftnoir_filter_kalman/kalman-filter.qrc
@@ -0,0 +1,5 @@
+<RCC>
+    <qresource prefix="/">
+        <file>images/filter-16-ac.png</file>
+    </qresource>
+</RCC>
diff --git a/ftnoir_filter_kalman/kalman.cpp b/ftnoir_filter_kalman/kalman.cpp
new file mode 100644
index 00000000..51af35e1
--- /dev/null
+++ b/ftnoir_filter_kalman/kalman.cpp
@@ -0,0 +1,156 @@
+/* Copyright (c) 2013 Stanisław Halik <sthalik@misaki.pl>
+ * 
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ */
+#include "ftnoir_filter_kalman.h"
+#include "facetracknoir/global-settings.h"
+#include <QDebug>
+#include <math.h>
+
+void kalman_load_settings(FTNoIR_Filter& self) {
+    QSettings settings("Abbequerque Inc.", "FaceTrackNoIR");    // Registry settings (in HK_USER)
+    
+    QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString();
+    QSettings iniFile( currentFile, QSettings::IniFormat );     // Application settings (in INI-file)
+    
+    iniFile.beginGroup("ftnoir-filter-kalman");
+    iniFile.endGroup();
+}
+
+void kalman_save_settings(FilterControls& self) {
+    QSettings settings("Abbequerque Inc.", "FaceTrackNoIR");    // Registry settings (in HK_USER)
+    
+    QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString();
+    QSettings iniFile( currentFile, QSettings::IniFormat );     // Application settings (in INI-file)
+    
+    iniFile.beginGroup("ftnoir-filter-kalman");
+    iniFile.endGroup();
+}
+
+FTNoIR_Filter::FTNoIR_Filter() {
+    kalman_load_settings(*this);
+    Initialize();
+}
+
+// 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;
+    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,
+    0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
+    0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0,
+    0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0,
+    0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0,
+    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1,
+    0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1);
+    double a = 0.25 * accel_variance;
+    double b = 0.5 * accel_variance;
+    double c = 1.0 * accel_variance;
+    kalman.processNoiseCov = (cv::Mat_<double>(12, 12) <<
+    a, 0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0,
+    0, a, 0, 0, 0, 0, 0, b, 0, 0, 0, 0,
+    0, 0, a, 0, 0, 0, 0, 0, b, 0, 0, 0,
+    0, 0, 0, a, 0, 0, 0, 0, 0, b, 0, 0,
+    0, 0, 0, 0, a, 0, 0, 0, 0, 0, b, 0,
+    0, 0, 0, 0, 0, a, 0, 0, 0, 0, 0, b,
+    b, 0, 0, 0, 0, 0, c, 0, 0, 0, 0, 0,
+    0, b, 0, 0, 0, 0, 0, c, 0, 0, 0, 0,
+    0, 0, b, 0, 0, 0, 0, 0, c, 0, 0, 0,
+    0, 0, 0, b, 0, 0, 0, 0, 0, c, 0, 0,
+    0, 0, 0, 0, b, 0, 0, 0, 0, 0, c, 0,
+    0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, c);
+    cv::setIdentity(kalman.measurementMatrix);
+    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;
+}
+
+void FTNoIR_Filter::FilterHeadPoseData(double *current_camera_position,
+                                       double *target_camera_position,
+                                       double *new_camera_position,
+                                       double *last_post_filter_values)
+{
+    bool new_target = false;
+    
+    for (int i = 0; i < 6; i++)
+        if (prev_position[i] != target_camera_position[i])
+        {
+            new_target = true;
+            break;
+        }
+    
+    cv::Mat output = kalman.predict();
+    
+    if (new_target) {
+        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];
+    }
+}
+
+void FilterControls::doOK() {
+    kalman_save_settings(*this);
+    close();
+}
+
+void FilterControls::doCancel() {
+    if (settingsDirty) {
+        int ret = QMessageBox::question ( this, "Settings have changed", "Do you want to save the settings?", QMessageBox::Save | QMessageBox::Discard | QMessageBox::Cancel, QMessageBox::Discard );
+        
+        qDebug() << "doCancel says: answer =" << ret;
+        
+        switch (ret) {
+            case QMessageBox::Save:
+                kalman_save_settings(*this);
+                this->close();
+                break;
+            case QMessageBox::Discard:
+                this->close();
+                break;
+            case QMessageBox::Cancel:
+                // Cancel was clicked
+                break;
+            default:
+                // should never be reached
+                break;
+        }
+    }
+    else {
+        this->close();
+    }
+}
+
+extern "C" FTNOIR_FILTER_BASE_EXPORT Metadata* CALLING_CONVENTION GetMetadata()
+{
+    return new FTNoIR_FilterDll;
+}
+
+extern "C" FTNOIR_FILTER_BASE_EXPORT IFilter* CALLING_CONVENTION GetConstructor()
+{
+    return new FTNoIR_Filter;
+}
+
+extern "C" FTNOIR_FILTER_BASE_EXPORT IFilterDialog* CALLING_CONVENTION GetDialog() {
+    return new FilterControls;
+}