diff options
| -rw-r--r-- | ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp | 259 | ||||
| -rw-r--r-- | ftnoir_filter_ewma2/ftnoir_filter_ewma2.h | 116 |
2 files changed, 174 insertions, 201 deletions
diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp index 3de1794a..8e9196ba 100644 --- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp +++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp @@ -1,26 +1,26 @@ /********************************************************************************
-* 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/>. *
-* *
+* 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"
@@ -30,150 +30,123 @@ #include <algorithm>
//#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;
- alpha_smoothing = 0.02f; // this is a constant for now, might be a parameter later
- loadSettings(); // Load the Settings
-
+ 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;
+ loadSettings(); // Load the Settings
}
FTNoIR_Filter::~FTNoIR_Filter()
{
-
}
//
// Load the current Settings from the currently 'active' INI-file.
//
void FTNoIR_Filter::loadSettings() {
- qDebug() << "FTNoIR_Filter::loadSettings says: Starting ";
- QSettings settings("opentrack"); // 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)
-
- qDebug() << "FTNoIR_Filter::loadSettings says: iniFile = " << currentFile;
-
- //
- // The EWMA2-filter-settings are in the Tracking group: this is because they used to be on the Main Form of FaceTrackNoIR
- //
- iniFile.beginGroup ( "Tracking" );
- kMinSmoothing = iniFile.value ( "minSmooth", 15 ).toInt();
- kMaxSmoothing = iniFile.value ( "maxSmooth", 50 ).toInt();
- kSmoothingScaleCurve = iniFile.value ( "powCurve", 10 ).toInt();
- iniFile.endGroup ();
-
+ qDebug() << "FTNoIR_Filter::loadSettings says: Starting ";
+ QSettings settings("opentrack"); // 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)
+
+ qDebug() << "FTNoIR_Filter::loadSettings says: iniFile = " << currentFile;
+
+ //
+ // The EWMA2-filter-settings are in the Tracking group: this is because they used to be on the Main Form of FaceTrackNoIR
+ //
+ iniFile.beginGroup ( "Tracking" );
+ // TODO(abo): change UI to have range 1-120 frames.
+ kMinSmoothing = iniFile.value ( "minSmooth", 2 ).toInt();
+ kMaxSmoothing = iniFile.value ( "maxSmooth", 15 ).toInt();
+ // TODO(abo): change powCurve to a float with default=1.0.
+ kSmoothingScaleCurve = iniFile.value ( "powCurve", 10 ).toInt();
+ iniFile.endGroup ();
}
-void FTNoIR_Filter::FilterHeadPoseData(double *current_camera_position, double *target_camera_position, double *new_camera_position, double *last_post_filter)
+void FTNoIR_Filter::FilterHeadPoseData(double *current_camera_position,
+ double *target_camera_position,
+ double *new_camera_position,
+ double *last_post_filter)
{
- //non-optimised version for clarity
- double prev_output[6];
- double target[6];
- double output_delta[6];
- double scale[]={0.025f,0.025f,0.025f,6.0f,6.0f,6.0f};
- double norm_output_delta[6];
- double output[6];
-
- for (int i = 0; i < 6; i++)
- {
- prev_output[i] = current_camera_position[i];
- target[i] = target_camera_position[i];
+ 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;
}
- if (first_run==true)
- {
- //on the first run, output=target
- for (int i=0;i<6;i++)
- {
- output[i]=target[i];
- prev_alpha[i] = 0.0f;
- }
-
- for (int i = 0; i < 6; i++)
- new_camera_position[i] = target[i];
-
- first_run=false;
-
- //we can bail
- return;
- }
-
- //how far does the camera need to move to catch up?
- for (int i=0;i<6;i++)
- {
- output_delta[i]=(target[i]-prev_output[i]);
- }
-
- //normalise the deltas
- for (int i=0;i<6;i++)
- {
- norm_output_delta[i]=std::min<double>(std::max<double>(fabs(output_delta[i])/scale[i],0.0),1.0);
- }
-
- //calculate the alphas
- //work out the dynamic smoothing factors
-// if (newTarget) {
- for (int i=0;i<6;i++)
- {
- alpha[i]=1.0/(kMinSmoothing+((1.0-pow(norm_output_delta[i],kSmoothingScaleCurve))*(kMaxSmoothing - kMinSmoothing)));
- smoothed_alpha[i]=(alpha_smoothing*alpha[i])+((1.0f-alpha_smoothing)*prev_alpha[i]);
- }
-// }
-
- //qDebug() << "FTNoIR_Filter::FilterHeadPoseData() smoothing frames = " << smoothing_frames_range;
- //qDebug() << "FTNoIR_Filter::FilterHeadPoseData() alpha[3] = " << alpha[3];
-
- //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++)
- {
- if (smoothed_alpha[i]>=largest_alpha)
- {
- largest_alpha=smoothed_alpha[i];
- }
- }
-
- //move the camera
- for (int i=0;i<6;i++)
- {
- output[i]=(largest_alpha*target[i])+((1.0f-largest_alpha)*prev_output[i]);
-// output[i]=(smoothed_alpha[i]*target[i])+((1.0f-smoothed_alpha[i])*prev_output[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 << "output:\t" << output[0] << "\t" << output[1] << "\t" << output[2] << "\t" << output[3] << "\t" << output[4] << "\t" << output[5] << '\n';
- out << "target:\t" << target[0] << "\t" << target[1] << "\t" << target[2] << "\t" << target[3] << "\t" << target[4] << "\t" << target[5] << '\n';
- out << "prev_output:\t" << prev_output[0] << "\t" << prev_output[1] << "\t" << prev_output[2] << "\t" << prev_output[3] << "\t" << prev_output[4] << "\t" << prev_output[5] << '\n';
- out << "largest_alpha:\t" << largest_alpha << '\n';
- }
- #endif
+ // 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/(kMinSmoothing+(1.0-pow(norm_noise,kSmoothingScaleCurve/20.0))*(kMaxSmoothing-kMinSmoothing));
+ new_camera_position[i] = alpha*target_camera_position[i] + (1.0-alpha)*current_camera_position[i];
+ }
- for (int i = 0; i < 6; i++)
- {
- new_camera_position[i] = output[i];
- current_camera_position[i] = output[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 filter memories ready for next sample
- for (int i=0;i<6;i++)
- {
- prev_alpha[i]=smoothed_alpha[i];
- }
- return;
+ // Update the current camera position to the new position.
+ for (int i = 0; i < 6; i++) {
+ current_camera_position[i] = new_camera_position[i];
+ }
}
////////////////////////////////////////////////////////////////////////////////
@@ -181,7 +154,7 @@ void FTNoIR_Filter::FilterHeadPoseData(double *current_camera_position, double * // Export both decorated and undecorated names.
// GetFilter - Undecorated name, which can be easily used with GetProcAddress
-// Win32 API function.
+// Win32 API function.
// _GetFilter@0 - Common name decoration for __stdcall functions in C language.
//#pragma comment(linker, "/export:GetFilter=_GetFilter@0")
diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h index 28824d2d..72dc6f0a 100644 --- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h +++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h @@ -1,26 +1,26 @@ /********************************************************************************
-* 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/>. *
-* *
+* 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
@@ -39,25 +39,28 @@ class FTNoIR_Filter : public IFilter
{
public:
- FTNoIR_Filter();
+ FTNoIR_Filter();
~FTNoIR_Filter();
void Initialize() {}
- void FilterHeadPoseData(double *current_camera_position, double *target_camera_position, double *new_camera_position, double *last_post_filter);
+ void FilterHeadPoseData(double *current_camera_position,
+ double *target_camera_position,
+ double *new_camera_position,
+ double *last_post_filter);
private:
- void loadSettings(); // Load the settings from the INI-file
- double newHeadPose; // Structure with new headpose
-
- bool first_run;
- double alpha_smoothing;
- double prev_alpha[6];
- double alpha[6];
- double smoothed_alpha[6];
-
- double kMinSmoothing;
- double kMaxSmoothing;
- double kSmoothingScaleCurve;
+ void loadSettings(); // Load the settings from the INI-file
+ double newHeadPose; // Structure with new headpose
+
+ bool first_run;
+ double delta_smoothing;
+ double noise_smoothing;
+ double delta[6];
+ double noise[6];
+
+ double kMinSmoothing;
+ double kMaxSmoothing;
+ double kSmoothingScaleCurve;
};
//*******************************************************************************************************
@@ -69,27 +72,26 @@ class FilterControls: public QWidget, public IFilterDialog {
Q_OBJECT
public:
-
- explicit FilterControls();
+ explicit FilterControls();
virtual ~FilterControls();
- void showEvent ( QShowEvent * event );
+ void showEvent ( QShowEvent * event );
void Initialize(QWidget *parent, IFilter* ptr);
private:
- Ui::UICFilterControls ui;
- void loadSettings();
- void save();
+ Ui::UICFilterControls ui;
+ void loadSettings();
+ void save();
- /** helper **/
- bool settingsDirty;
+ /** helper **/
+ bool settingsDirty;
- IFilter* pFilter; // If the filter was active when the dialog was opened, this will hold a pointer to the Filter instance
+ IFilter* pFilter; // If the filter was active when the dialog was opened, this will hold a pointer to the Filter instance
private slots:
- void doOK();
- void doCancel();
- void settingChanged() { settingsDirty = true; };
- void settingChanged( int ) { settingsDirty = true; };
+ void doOK();
+ void doCancel();
+ void settingChanged() { settingsDirty = true; };
+ void settingChanged( int ) { settingsDirty = true; };
};
//*******************************************************************************************************
@@ -98,15 +100,13 @@ private slots: class FTNoIR_FilterDll : public Metadata
{
public:
- FTNoIR_FilterDll();
- ~FTNoIR_FilterDll();
- 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"); }
+ FTNoIR_FilterDll();
+ ~FTNoIR_FilterDll();
+ 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
+#endif //INCLUDED_FTN_FILTER_H
//END
-
|