Tidy and optimize ewma2 filter.

2 files changed, 118 insertions, 150 deletions

diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
index 3de1794a..67a15afe 100644
--- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
+++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
@@ -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];

+    }

 }

 

 ////////////////////////////////////////////////////////////////////////////////

diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h
index 28824d2d..b4cb1d64 100644
--- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h
+++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.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

 //END

-