summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp275
-rw-r--r--ftnoir_filter_ewma2/ftnoir_filter_ewma2.h206
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