Merge pull request #1 from dbaarda/tidy/ewma2

Tidy/ewma2

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

-