Merge pull request #33 from dbaarda/dev/ewma2

Improve ewma2 filter.

2 files changed, 243 insertions, 238 deletions

diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
index 4f07443a..60c3a2e6 100644
--- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
+++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
@@ -1,136 +1,139 @@
-/********************************************************************************

-* FaceTrackNoIR      This program is a private project of some enthusiastic     *

-*                    gamers from Holland, who don't like to pay much for        *

-*                    head-tracking.                                             *

-*                                                                               *

-* Copyright (C) 2012  Wim Vriend (Developing)                                   *

-*                     Ron Hendriks (Researching and Testing)                    *

-*                                                                               *

-* Homepage                                                                      *

-*                                                                               *

-* This program is free software; you can redistribute it and/or modify it       *

-* under the terms of the GNU General Public License as published by the         *

-* Free Software Foundation; either version 3 of the License, or (at your        *

-* option) any later version.                                                    *

-*                                                                               *

-* This program is distributed in the hope that it will be useful, but           *

-* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY    *

-* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for   *

-* more details.                                                                 *

-*                                                                               *

-* You should have received a copy of the GNU General Public License along       *

-* with this program; if not, see <http://www.gnu.org/licenses/>.                *

-*                                                                               *

-********************************************************************************/

-#include "ftnoir_filter_ewma2.h"

-#include "math.h"

-#include <QDebug>

-#include <QWidget>

-#include "facetracknoir/global-settings.h"

-#include <algorithm>

-#include <QMutexLocker>

-//#define LOG_OUTPUT

-

-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

-//

-// EWMA Filter: Exponentially Weighted Moving Average filter with dynamic smoothing parameter

-//

-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

-

-FTNoIR_Filter::FTNoIR_Filter() : first_run(true), alpha_smoothing(0.02)

-{

-}

-

-void FTNoIR_Filter::receiveSettings()

-{

-    s.b->reload();

-}

-

-void FTNoIR_Filter::FilterHeadPoseData(const double *target_camera_position,

-                                       double *new_camera_position)

-{

-    double delta;

-    double new_alpha;

-    double scale[]={0.025f,0.025f,0.025f,6.0f,6.0f,6.0f};

-

-    //On the first run, initialize to output=target and return.

-    if (first_run==true) {

-        for (int i=0;i<6;i++) {

-            new_camera_position[i] = target_camera_position[i];

-            current_camera_position[i] = target_camera_position[i];

-            alpha[i] = 0.0f;

-        }

-        first_run=false;

-        return;

-    }

-    

-    for (int i=0;i<6;i++) {

-        // Calculate the delta.

-        delta=target_camera_position[i]-current_camera_position[i];

-        // Normalise the delta.

-        delta=std::min<double>(std::max<double>(fabs(delta)/scale[i],0.0),1.0);

-        // Calculate the new alpha from the normalized delta.

-        new_alpha=1.0/(s.kMinSmoothing+((1.0-pow(delta,s.kSmoothingScaleCurve))*(s.kMaxSmoothing-s.kMinSmoothing)));

-        // Update the smoothed alpha.

-        alpha[i]=(alpha_smoothing*new_alpha)+((1.0-alpha_smoothing)*alpha[i]);

-    }

-

-    // Use the same (largest) smoothed alpha for each channel

-    //NB: larger alpha = *less* lag (opposite to what you'd expect)

-    float largest_alpha=0.0f;

-    for (int i=0;i<6;i++) {

-        largest_alpha=std::min<double>(largest_alpha, alpha[i]);

-    }

-

-    // Calculate the new camera position.

-    for (int i=0;i<6;i++) {

-        new_camera_position[i]=(largest_alpha*target_camera_position[i])+((1.0-largest_alpha)*current_camera_position[i]);

-        //new_camera_position[i]=(alpha[i]*target_camera_position[i])+((1.0f-alpha[i])*current_camera_position[i]);

-    }

-

-#ifdef LOG_OUTPUT

-    // Use this for some debug-output to file...

-    QFile data(QCoreApplication::applicationDirPath() + "\\EWMA_output.txt");

-    if (data.open(QFile::WriteOnly | QFile::Append)) {

-        QTextStream out(&data);

-        out << "current:\t" << current_camera_position[0]

-            << "\t" << current_camera_position[1]

-            << "\t" << current_camera_position[2]

-            << "\t" << current_camera_position[3]

-            << "\t" << current_camera_position[4]

-            << "\t" << current_camera_position[5] << '\n';

-        out << "target:\t" << target_camera_position[0]

-            << "\t" << target_camera_position[1]

-            << "\t" << target_camera_position[2]

-            << "\t" << target_camera_position[3]

-            << "\t" << target_camera_position[4]

-            << "\t" << target_camera_position[5] << '\n';

-        out << "output:\t" << new_camera_position[0]

-            << "\t" << new_camera_position[1]

-            << "\t" << new_camera_position[2]

-            << "\t" << new_camera_position[3]

-            << "\t" << new_camera_position[4]

-            << "\t" << new_camera_position[5] << '\n';

-        out << "largest_alpha:\t" << largest_alpha << '\n';

-    }

-#endif

-

-    // Update the current camera position to the new position.

-    for (int i = 0; i < 6; i++) {

-        current_camera_position[i] = new_camera_position[i];

-    }

-}

-

-////////////////////////////////////////////////////////////////////////////////

-// Factory function that creates instances if the Filter object.

-

-// Export both decorated and undecorated names.

-//   GetFilter     - Undecorated name, which can be easily used with GetProcAddress

-//                   Win32 API function.

-//   _GetFilter@0  - Common name decoration for __stdcall functions in C language.

-//#pragma comment(linker, "/export:GetFilter=_GetFilter@0")

-

-extern "C" FTNOIR_FILTER_BASE_EXPORT IFilter* CALLING_CONVENTION GetConstructor()

-{

-    return new FTNoIR_Filter;

-}

+/********************************************************************************
+* FaceTrackNoIR      This program is a private project of some enthusiastic     *
+*                    gamers from Holland, who don't like to pay much for        *
+*                    head-tracking.                                             *
+*                                                                               *
+* Copyright (C) 2012  Wim Vriend (Developing)                                   *
+*                     Ron Hendriks (Researching and Testing)                    *
+*                                                                               *
+* Homepage                                                                      *
+*                                                                               *
+* This program is free software; you can redistribute it and/or modify it       *
+* under the terms of the GNU General Public License as published by the         *
+* Free Software Foundation; either version 3 of the License, or (at your        *
+* option) any later version.                                                    *
+*                                                                               *
+* This program is distributed in the hope that it will be useful, but           *
+* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY    *
+* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for   *
+* more details.                                                                 *
+*                                                                               *
+* You should have received a copy of the GNU General Public License along       *
+* with this program; if not, see <http://www.gnu.org/licenses/>.                *
+*                                                                               *
+********************************************************************************/
+#include "ftnoir_filter_ewma2.h"
+#include "math.h"
+#include <QDebug>
+#include <QWidget>
+#include "facetracknoir/global-settings.h"
+#include <algorithm>
+#include <QMutexLocker>
+//#define LOG_OUTPUT
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// EWMA Filter: Exponentially Weighted Moving Average filter with dynamic smoothing parameter
+//
+// This filter tries to adjust the amount of filtering to minimize lag when
+// moving, and minimize noise when still. It uses the delta filtered over the
+// last 3 frames (0.1secs) compared to the delta's average noise variance over
+// the last 3600 frames (~2mins) to try and detect movement vs noise. As the
+// delta increases from 0->3 stdevs of the noise, the filtering scales down
+// from maxSmooth->minSmooth at a rate controlled by the powCurve setting.
+//
+///////////////////////////////////////////////////////////////////////////////
+
+FTNoIR_Filter::FTNoIR_Filter() :
+    first_run(true),
+    // Deltas are smoothed over the last 3 frames (0.1sec at 30fps).
+    delta_smoothing(1.0/3.0),
+    // Noise is smoothed over the last 3600 frames (~2mins at 30fps).
+    noise_smoothing(1.0/3600.0)
+{
+}
+
+void FTNoIR_Filter::receiveSettings()
+{
+    s.b->reload();
+}
+
+void FTNoIR_Filter::FilterHeadPoseData(const double *target_camera_position,
+                                       double *new_camera_position)
+{
+    double new_delta, new_noise, norm_noise;
+    double alpha;
+
+    //On the first run, initialize to output=target and return.
+    if (first_run==true) {
+        for (int i=0;i<6;i++) {
+            new_camera_position[i] = target_camera_position[i];
+            current_camera_position[i] = target_camera_position[i];
+            delta[i] = 0.0f;
+            noise[i] = 0.0f;
+        }
+        first_run=false;
+        return;
+    }
+
+    // Calculate the new camera position.
+    for (int i=0;i<6;i++) {
+        // Calculate the current and smoothed delta.
+        new_delta = target_camera_position[i]-current_camera_position[i];
+        delta[i] = delta_smoothing*new_delta + (1.0-delta_smoothing)*delta[i];
+        // Calculate the current and smoothed noise variance.
+        new_noise = delta[i]*delta[i];
+        noise[i] = noise_smoothing*new_noise + (1.0-noise_smoothing)*noise[i];
+        // Normalise the noise between 0->1 for 0->9 variances (0->3 stddevs).
+        norm_noise = std::min<double>(new_noise/(9.0*noise[i]), 1.0);
+        // Calculate the alpha from the normalized noise.
+        // TODO(abo): change kSmoothingScaleCurve to a float where 1.0 is sqrt(norm_noise).
+        alpha = 1.0/(s.kMinSmoothing+(1.0-pow(norm_noise,s.kSmoothingScaleCurve/20.0))*(s.kMaxSmoothing-s.kMinSmoothing));
+        new_camera_position[i] = alpha*target_camera_position[i] + (1.0-alpha)*current_camera_position[i];
+    }
+
+#ifdef LOG_OUTPUT
+    // Use this for some debug-output to file...
+    QFile data(QCoreApplication::applicationDirPath() + "\\EWMA_output.txt");
+    if (data.open(QFile::WriteOnly | QFile::Append)) {
+        QTextStream out(&data);
+        out << "current:\t" << current_camera_position[0]
+            << "\t" << current_camera_position[1]
+            << "\t" << current_camera_position[2]
+            << "\t" << current_camera_position[3]
+            << "\t" << current_camera_position[4]
+            << "\t" << current_camera_position[5] << '\n';
+        out << "target:\t" << target_camera_position[0]
+            << "\t" << target_camera_position[1]
+            << "\t" << target_camera_position[2]
+            << "\t" << target_camera_position[3]
+            << "\t" << target_camera_position[4]
+            << "\t" << target_camera_position[5] << '\n';
+        out << "output:\t" << new_camera_position[0]
+            << "\t" << new_camera_position[1]
+            << "\t" << new_camera_position[2]
+            << "\t" << new_camera_position[3]
+            << "\t" << new_camera_position[4]
+            << "\t" << new_camera_position[5] << '\n';
+    }
+#endif
+
+    // Update the current camera position to the new position.
+    for (int i = 0; i < 6; i++) {
+        current_camera_position[i] = new_camera_position[i];
+    }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Factory function that creates instances if the Filter object.
+
+// Export both decorated and undecorated names.
+//   GetFilter     - Undecorated name, which can be easily used with GetProcAddress
+//                   Win32 API function.
+//   _GetFilter@0  - Common name decoration for __stdcall functions in C language.
+//#pragma comment(linker, "/export:GetFilter=_GetFilter@0")
+
+extern "C" FTNOIR_FILTER_BASE_EXPORT IFilter* CALLING_CONVENTION GetConstructor()
+{
+    return new FTNoIR_Filter;
+}
diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h
index e50a4284..33c4ac33 100644
--- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h
+++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h
@@ -1,102 +1,104 @@
-/********************************************************************************

-* FaceTrackNoIR      This program is a private project of some enthusiastic     *

-*                    gamers from Holland, who don't like to pay much for        *

-*                    head-tracking.                                             *

-*                                                                               *

-* Copyright (C) 2012  Wim Vriend (Developing)                                   *

-*                     Ron Hendriks (Researching and Testing)                    *

-*                                                                               *

-* Homepage                                                                      *

-*                                                                               *

-* This program is free software; you can redistribute it and/or modify it       *

-* under the terms of the GNU General Public License as published by the         *

-* Free Software Foundation; either version 3 of the License, or (at your        *

-* option) any later version.                                                    *

-*                                                                               *

-* This program is distributed in the hope that it will be useful, but           *

-* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY    *

-* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for   *

-* more details.                                                                 *

-*                                                                               *

-* You should have received a copy of the GNU General Public License along       *

-* with this program; if not, see <http://www.gnu.org/licenses/>.                *

-*                                                                               *

-********************************************************************************/

-#pragma once

-#ifndef INCLUDED_FTN_FILTER_H

-#define INCLUDED_FTN_FILTER_H

-

-#include "ftnoir_filter_base/ftnoir_filter_base.h"

-#include "facetracknoir/global-settings.h"

-#include "ui_ftnoir_ewma_filtercontrols.h"

-#include <QWidget>

-#include <QMutex>

-#include "facetracknoir/options.h"

-using namespace options;

-

-struct settings {

-    pbundle b;

-    value<int> kMinSmoothing, kMaxSmoothing, kSmoothingScaleCurve;

-    settings() :

-        b(bundle("ewma-filter")),

-        kMinSmoothing(b, "min-smoothing", 15),

-        kMaxSmoothing(b, "max-smoothing", 50),

-        kSmoothingScaleCurve(b, "smoothing-scale-curve", 10)

-    {}

-};

-

-

-class FTNoIR_Filter : public IFilter

-{

-public:

-    FTNoIR_Filter();

-    void reset() {}

-    void FilterHeadPoseData(const double *target_camera_position,

-                            double *new_camera_position);

-    void receiveSettings();

-private:

-    bool first_run;

-    double alpha_smoothing;

-    double alpha[6];

-    double current_camera_position[6];

-    settings s;

-};

-

-//*******************************************************************************************************

-// FaceTrackNoIR Filter Settings-dialog.

-//*******************************************************************************************************

-

-// Widget that has controls for FTNoIR protocol filter-settings.

-class FilterControls: public QWidget, public IFilterDialog

-{

-    Q_OBJECT

-public:

-    FilterControls();

-    void registerFilter(IFilter* flt);

-    void unregisterFilter();

-

-private:

-    Ui::UICFilterControls ui;

-    void save();

-    settings s;

-    FTNoIR_Filter* pFilter;

-

-private slots:

-    void doOK();

-    void doCancel();

-};

-

-//*******************************************************************************************************

-// FaceTrackNoIR Filter DLL. Functions used to get general info on the Filter

-//*******************************************************************************************************

-class FTNoIR_FilterDll : public Metadata

-{

-public:

-    void getFullName(QString *strToBeFilled) { *strToBeFilled = QString("EWMA Filter Mk2"); }

-    void getShortName(QString *strToBeFilled) { *strToBeFilled = QString("EWMA"); }

-    void getDescription(QString *strToBeFilled) { *strToBeFilled = QString("Exponentially Weighted Moving Average filter with dynamic smoothing parameter"); }

-    void getIcon(QIcon *icon){ *icon = QIcon(":/images/filter-16.png"); }

-};

-

-#endif  //INCLUDED_FTN_FILTER_H

-//END

+/********************************************************************************
+* FaceTrackNoIR      This program is a private project of some enthusiastic     *
+*                    gamers from Holland, who don't like to pay much for        *
+*                    head-tracking.                                             *
+*                                                                               *
+* Copyright (C) 2012  Wim Vriend (Developing)                                   *
+*                     Ron Hendriks (Researching and Testing)                    *
+*                                                                               *
+* Homepage                                                                      *
+*                                                                               *
+* This program is free software; you can redistribute it and/or modify it       *
+* under the terms of the GNU General Public License as published by the         *
+* Free Software Foundation; either version 3 of the License, or (at your        *
+* option) any later version.                                                    *
+*                                                                               *
+* This program is distributed in the hope that it will be useful, but           *
+* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY    *
+* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for   *
+* more details.                                                                 *
+*                                                                               *
+* You should have received a copy of the GNU General Public License along       *
+* with this program; if not, see <http://www.gnu.org/licenses/>.                *
+*                                                                               *
+********************************************************************************/
+#pragma once
+#ifndef INCLUDED_FTN_FILTER_H
+#define INCLUDED_FTN_FILTER_H
+
+#include "ftnoir_filter_base/ftnoir_filter_base.h"
+#include "facetracknoir/global-settings.h"
+#include "ui_ftnoir_ewma_filtercontrols.h"
+#include <QWidget>
+#include <QMutex>
+#include "facetracknoir/options.h"
+using namespace options;
+
+struct settings {
+    pbundle b;
+    value<int> kMinSmoothing, kMaxSmoothing, kSmoothingScaleCurve;
+    settings() :
+        b(bundle("ewma-filter")),
+        kMinSmoothing(b, "min-smoothing", 15),
+        kMaxSmoothing(b, "max-smoothing", 50),
+        kSmoothingScaleCurve(b, "smoothing-scale-curve", 10)
+    {}
+};
+
+
+class FTNoIR_Filter : public IFilter
+{
+public:
+    FTNoIR_Filter();
+    void reset() {}
+    void FilterHeadPoseData(const double *target_camera_position,
+                            double *new_camera_position);
+    void receiveSettings();
+private:
+    bool first_run;
+    double delta_smoothing;
+    double noise_smoothing;
+    double delta[6];
+    double noise[6];
+    double current_camera_position[6];
+    settings s;
+};
+
+//*******************************************************************************************************
+// FaceTrackNoIR Filter Settings-dialog.
+//*******************************************************************************************************
+
+// Widget that has controls for FTNoIR protocol filter-settings.
+class FilterControls: public QWidget, public IFilterDialog
+{
+    Q_OBJECT
+public:
+    FilterControls();
+    void registerFilter(IFilter* flt);
+    void unregisterFilter();
+
+private:
+    Ui::UICFilterControls ui;
+    void save();
+    settings s;
+    FTNoIR_Filter* pFilter;
+
+private slots:
+    void doOK();
+    void doCancel();
+};
+
+//*******************************************************************************************************
+// FaceTrackNoIR Filter DLL. Functions used to get general info on the Filter
+//*******************************************************************************************************
+class FTNoIR_FilterDll : public Metadata
+{
+public:
+    void getFullName(QString *strToBeFilled) { *strToBeFilled = QString("EWMA Filter Mk2"); }
+    void getShortName(QString *strToBeFilled) { *strToBeFilled = QString("EWMA"); }
+    void getDescription(QString *strToBeFilled) { *strToBeFilled = QString("Exponentially Weighted Moving Average filter with dynamic smoothing parameter"); }
+    void getIcon(QIcon *icon){ *icon = QIcon(":/images/filter-16.png"); }
+};
+
+#endif  //INCLUDED_FTN_FILTER_H
+//END