diff options
| -rw-r--r-- | ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp | 250 | ||||
| -rw-r--r-- | ftnoir_filter_ewma2/ftnoir_filter_ewma2.h | 110 |
2 files changed, 164 insertions, 196 deletions
diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp index 3de1794a..b5250593 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"
@@ -38,142 +38,112 @@ 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;
+ alpha_smoothing = 0.02f; // this is a constant for now, might be a parameter later
+ 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" );
+ kMinSmoothing = iniFile.value ( "minSmooth", 15 ).toInt();
+ kMaxSmoothing = iniFile.value ( "maxSmooth", 50 ).toInt();
+ 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 delta;
+ double new_alpha;
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];
+ //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;
}
- 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
-
- for (int i = 0; i < 6; i++)
- {
- new_camera_position[i] = output[i];
- current_camera_position[i] = output[i];
+ 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/(kMinSmoothing+((1.0-pow(delta,kSmoothingScaleCurve))*(kMaxSmoothing-kMinSmoothing)));
+ // Update the smoothed alpha.
+ alpha[i]=(alpha_smoothing*new_alpha)+((1.0f-alpha_smoothing)*alpha[i]);
}
- //update filter memories ready for next sample
- for (int i=0;i<6;i++)
- {
- prev_alpha[i]=smoothed_alpha[i];
- }
- return;
+ // 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::max<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.0f-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];
+ }
}
////////////////////////////////////////////////////////////////////////////////
@@ -181,7 +151,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..a5f3ef24 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,26 @@ 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
+ 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];
+ bool first_run;
+ double alpha_smoothing;
+ double alpha[6];
- double kMinSmoothing;
- double kMaxSmoothing;
- double kSmoothingScaleCurve;
+ double kMinSmoothing;
+ double kMaxSmoothing;
+ double kSmoothingScaleCurve;
};
//*******************************************************************************************************
@@ -69,27 +70,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 +98,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
-
|