merge continues

5 files changed, 463 insertions, 0 deletions

diff --git a/ftnoir_filter_kalman/ftnoir_filter_kalman.h b/ftnoir_filter_kalman/ftnoir_filter_kalman.h
new file mode 100644
index 00000000..15ccc8c5
--- /dev/null
+++ b/ftnoir_filter_kalman/ftnoir_filter_kalman.h
@@ -0,0 +1,94 @@
+#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>
+
+#define DEFAULT_ACCL 750
+
+class FTNOIR_FILTER_BASE_EXPORT FTNoIR_Filter : public IFilter
+{
+public:
+    FTNoIR_Filter();
+    virtual ~FTNoIR_Filter() {
+    }
+    void Initialize();
+    void FilterHeadPoseData(THeadPoseData *current_camera_position, THeadPoseData *target_camera_position, THeadPoseData *new_camera_position, THeadPoseData *last_post_filter, bool newTarget);
+    cv::KalmanFilter kalman;
+    double process_noise_covariance_matrix_all_values;
+    double posteriori_error_covariance_matrix_all_values;
+    double accl;
+};
+
+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");
+        ui.post->setValue(iniFile.value("posteriori-error-covariance-matrix-all-values", "1e-12").toDouble());
+        ui.pnoise->setValue(iniFile.value("process-noise-covariance-matrix-all-values", "1e-14").toDouble());
+        ui.accl->setValue(iniFile.value("accel-coefficient", DEFAULT_ACCL).toDouble());
+        iniFile.endGroup();
+        connect(ui.btnOk, SIGNAL(clicked()), this, SLOT(doOK()));
+        connect(ui.btnCancel, SIGNAL(clicked()), this, SLOT(doCancel()));
+        connect(ui.post, SIGNAL(valueChanged(double)), this, SLOT(settingsChanged(double)));
+        connect(ui.pnoise, SIGNAL(valueChanged(double)), this, SLOT(settingsChanged(double)));
+        connect(ui.accl, SIGNAL(valueChanged(double)), this, SLOT(settingsChanged(double)));
+        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..92c3fc27
--- /dev/null
+++ b/ftnoir_filter_kalman/kalman.cpp
@@ -0,0 +1,153 @@
+/* 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");
+    self.process_noise_covariance_matrix_all_values = iniFile.value("process-noise-covariance-matrix-all-values", "1e-14").toDouble();
+    self.posteriori_error_covariance_matrix_all_values = iniFile.value("posteriori-error-covariance-matrix-all-values", "1e-12").toDouble();
+    self.accl = iniFile.value("accel-coefficient", DEFAULT_ACCL).toDouble();
+    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.setValue("process-noise-covariance-matrix-all-values", self.ui.pnoise->value());
+    iniFile.setValue("posteriori-error-covariance-matrix-all-values", self.ui.post->value());
+    iniFile.setValue("accel-coefficient", self.ui.accl->value());
+    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
+// minor changes to order of magnitude -sh
+void FTNoIR_Filter::Initialize() {
+    kalman.init(12, 6, 0, CV_64F);
+    double accel_variance = 1e-2;
+    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(accl));
+    cv::setIdentity(kalman.errorCovPost, cv::Scalar::all(accel_variance * 1e-4));
+}
+
+void FTNoIR_Filter::FilterHeadPoseData(THeadPoseData *current_camera_position, THeadPoseData *target_camera_position, THeadPoseData *new_camera_position, THeadPoseData *last_post_filter, bool newTarget) {
+    cv::Mat output = kalman.predict();
+    if (newTarget) {
+        cv::Mat measurement(6, 1, CV_64F);
+        measurement.at<double>(0) = target_camera_position->yaw;
+        measurement.at<double>(1) = target_camera_position->pitch;
+        measurement.at<double>(2) = target_camera_position->roll;
+        measurement.at<double>(3) = target_camera_position->x;
+        measurement.at<double>(4) = target_camera_position->y;
+        measurement.at<double>(5) = target_camera_position->z;
+        kalman.correct(measurement);
+    }
+    new_camera_position->yaw = output.at<double>(0);
+    new_camera_position->pitch = output.at<double>(1);
+    new_camera_position->roll = output.at<double>(2);
+    new_camera_position->x = output.at<double>(3);
+    new_camera_position->y = output.at<double>(4);
+    new_camera_position->z = output.at<double>(5);
+    target_camera_position->yaw = output.at<double>(0);
+    target_camera_position->pitch = output.at<double>(1);
+    target_camera_position->roll = output.at<double>(2);
+    target_camera_position->x = output.at<double>(3);
+    target_camera_position->y = output.at<double>(4);
+    target_camera_position->z = output.at<double>(5);
+}
+
+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 void* CALLING_CONVENTION GetConstructor()
+{
+    return (IFilter*) new FTNoIR_Filter;
+}
+
+extern "C" FTNOIR_FILTER_BASE_EXPORT void* CALLING_CONVENTION GetDialog() {
+    return (IFilterDialog*) new FilterControls;
+}