Start merging new filter

git-svn-id: svn+ssh://svn.code.sf.net/p/facetracknoir/code@42 19e81ba0-9b1a-49c3-bd6c-561e1906d5fb

4 files changed, 569 insertions, 0 deletions

diff --git a/FTNoIR_Filter_EWMA2/FTNoIR_Filter_EWMA2.vcproj b/FTNoIR_Filter_EWMA2/FTNoIR_Filter_EWMA2.vcproj
new file mode 100644
index 00000000..4ff53df2
--- /dev/null
+++ b/FTNoIR_Filter_EWMA2/FTNoIR_Filter_EWMA2.vcproj
@@ -0,0 +1,235 @@
+<?xml version="1.0" encoding="Windows-1252"?>

+<VisualStudioProject

+	ProjectType="Visual C++"

+	Version="8.00"

+	Name="FTNoIR_Filter_EWMA2"

+	ProjectGUID="{7DD9F1AA-B7C0-4056-AC4A-1501840E6438}"

+	Keyword="Qt4VSv1.0"

+	>

+	<Platforms>

+		<Platform

+			Name="Win32"

+		/>

+	</Platforms>

+	<ToolFiles>

+	</ToolFiles>

+	<Configurations>

+		<Configuration

+			Name="Release|Win32"

+			OutputDirectory="$(SolutionDir)/bin"

+			IntermediateDirectory="$(ConfigurationName)"

+			ConfigurationType="2"

+			>

+			<Tool

+				Name="VCPreBuildEventTool"

+			/>

+			<Tool

+				Name="VCCustomBuildTool"

+			/>

+			<Tool

+				Name="VCXMLDataGeneratorTool"

+			/>

+			<Tool

+				Name="VCWebServiceProxyGeneratorTool"

+			/>

+			<Tool

+				Name="VCMIDLTool"

+			/>

+			<Tool

+				Name="VCCLCompilerTool"

+				AdditionalIncludeDirectories=".\GeneratedFiles;&quot;$(QTDIR)\include&quot;;&quot;.\GeneratedFiles\$(ConfigurationName)&quot;;&quot;$(QTDIR)\include\qtmain&quot;;&quot;$(QTDIR)\include\QtCore&quot;;&quot;$(SolutionDir)\FTNoIR_Tracker_UDP&quot;;.\"

+				PreprocessorDefinitions="UNICODE;WIN32;QT_LARGEFILE_SUPPORT;QT_THREAD_SUPPORT;QT_NO_DEBUG;NDEBUG;QT_CORE_LIB;FTNOIR_FILTER_BASE_LIB"

+				RuntimeLibrary="2"

+				TreatWChar_tAsBuiltInType="false"

+				DebugInformationFormat="0"

+			/>

+			<Tool

+				Name="VCManagedResourceCompilerTool"

+			/>

+			<Tool

+				Name="VCResourceCompilerTool"

+			/>

+			<Tool

+				Name="VCPreLinkEventTool"

+			/>

+			<Tool

+				Name="VCLinkerTool"

+				AdditionalDependencies="qtmain.lib QtCore4.lib"

+				OutputFile="$(OutDir)\$(ProjectName).dll"

+				AdditionalLibraryDirectories="$(QTDIR)\lib"

+				GenerateDebugInformation="false"

+				SubSystem="2"

+			/>

+			<Tool

+				Name="VCALinkTool"

+			/>

+			<Tool

+				Name="VCManifestTool"

+			/>

+			<Tool

+				Name="VCXDCMakeTool"

+			/>

+			<Tool

+				Name="VCBscMakeTool"

+			/>

+			<Tool

+				Name="VCFxCopTool"

+			/>

+			<Tool

+				Name="VCAppVerifierTool"

+			/>

+			<Tool

+				Name="VCWebDeploymentTool"

+			/>

+			<Tool

+				Name="VCPostBuildEventTool"

+			/>

+		</Configuration>

+		<Configuration

+			Name="Debug|Win32"

+			OutputDirectory="$(SolutionDir)$(ConfigurationName)"

+			IntermediateDirectory="$(ConfigurationName)"

+			ConfigurationType="2"

+			>

+			<Tool

+				Name="VCPreBuildEventTool"

+			/>

+			<Tool

+				Name="VCCustomBuildTool"

+			/>

+			<Tool

+				Name="VCXMLDataGeneratorTool"

+			/>

+			<Tool

+				Name="VCWebServiceProxyGeneratorTool"

+			/>

+			<Tool

+				Name="VCMIDLTool"

+			/>

+			<Tool

+				Name="VCCLCompilerTool"

+				Optimization="0"

+				AdditionalIncludeDirectories=".\GeneratedFiles;$(QTDIR)\include;.\GeneratedFiles\$(ConfigurationName);$(QTDIR)\include\qtmain;$(QTDIR)\include\QtCore;.\"

+				PreprocessorDefinitions="UNICODE,WIN32,QT_LARGEFILE_SUPPORT,QT_THREAD_SUPPORT,QT_CORE_LIB,FTNOIR_FILTER_EWMA2_LIB"

+				RuntimeLibrary="3"

+				TreatWChar_tAsBuiltInType="false"

+				DebugInformationFormat="3"

+			/>

+			<Tool

+				Name="VCManagedResourceCompilerTool"

+			/>

+			<Tool

+				Name="VCResourceCompilerTool"

+			/>

+			<Tool

+				Name="VCPreLinkEventTool"

+			/>

+			<Tool

+				Name="VCLinkerTool"

+				AdditionalDependencies="qtmaind.lib QtCored4.lib"

+				OutputFile="$(OutDir)\$(ProjectName).dll"

+				AdditionalLibraryDirectories="$(QTDIR)\lib"

+				GenerateDebugInformation="true"

+				SubSystem="2"

+			/>

+			<Tool

+				Name="VCALinkTool"

+			/>

+			<Tool

+				Name="VCManifestTool"

+			/>

+			<Tool

+				Name="VCXDCMakeTool"

+			/>

+			<Tool

+				Name="VCBscMakeTool"

+			/>

+			<Tool

+				Name="VCFxCopTool"

+			/>

+			<Tool

+				Name="VCAppVerifierTool"

+			/>

+			<Tool

+				Name="VCWebDeploymentTool"

+			/>

+			<Tool

+				Name="VCPostBuildEventTool"

+			/>

+		</Configuration>

+	</Configurations>

+	<References>

+	</References>

+	<Files>

+		<Filter

+			Name="Source Files"

+			Filter="cpp;cxx;c;def"

+			UniqueIdentifier="{4FC737F1-C7A5-4376-A066-2A32D752A2FF}"

+			>

+			<File

+				RelativePath=".\ftnoir_filter_ewma2.cpp"

+				>

+			</File>

+		</Filter>

+		<Filter

+			Name="Header Files"

+			Filter="h"

+			UniqueIdentifier="{93995380-89BD-4b04-88EB-625FBE52EBFB}"

+			>

+			<File

+				RelativePath=".\ftnoir_filter_base.h"

+				>

+			</File>

+			<File

+				RelativePath=".\ftnoir_filter_base_global.h"

+				>

+			</File>

+		</Filter>

+		<Filter

+			Name="Form Files"

+			Filter="ui"

+			UniqueIdentifier="{99349809-55BA-4b9d-BF79-8FDBB0286EB3}"

+			>

+		</Filter>

+		<Filter

+			Name="Resource Files"

+			Filter="qrc;*"

+			UniqueIdentifier="{D9D6E242-F8AF-46E4-B9FD-80ECBC20BA3E}"

+			ParseFiles="false"

+			>

+		</Filter>

+		<Filter

+			Name="Generated Files"

+			Filter="moc;h;cpp"

+			UniqueIdentifier="{71ED8ED8-ACB9-4CE9-BBE1-E00B30144E11}"

+			SourceControlFiles="false"

+			>

+		</Filter>

+	</Files>

+	<Globals>

+		<Global

+			Name="lupdateOnBuild"

+			Value="0"

+		/>

+		<Global

+			Name="MocDir"

+			Value=".\GeneratedFiles\$(ConfigurationName)"

+		/>

+		<Global

+			Name="MocOptions"

+			Value=""

+		/>

+		<Global

+			Name="QtVersion Win32"

+			Value="QT462_VS2005"

+		/>

+		<Global

+			Name="RccDir"

+			Value=".\GeneratedFiles"

+		/>

+		<Global

+			Name="UicDir"

+			Value=".\GeneratedFiles"

+		/>

+	</Globals>

+</VisualStudioProject>

diff --git a/FTNoIR_Filter_EWMA2/ftnoir_filter_base.h b/FTNoIR_Filter_EWMA2/ftnoir_filter_base.h
new file mode 100644
index 00000000..f05fe66a
--- /dev/null
+++ b/FTNoIR_Filter_EWMA2/ftnoir_filter_base.h
@@ -0,0 +1,47 @@
+#ifndef FTNOIR_FILTER_BASE_H

+#define FTNOIR_FILTER_BASE_H

+

+#include "ftnoir_filter_base_global.h"

+#include "ftnoir_tracker_base.h"

+#include <QString>

+#include <QList>

+

+// COM-Like abstract interface.

+// This interface doesn't require __declspec(dllexport/dllimport) specifier.

+// Method calls are dispatched via virtual table.

+// Any C++ compiler can use it.

+// Instances are obtained via factory function.

+struct IFilter

+{

+    virtual void Release() = 0;

+	virtual void Initialize() = 0;

+	virtual void FilterHeadPoseData(THeadPoseData *current_camera_position, THeadPoseData *target_camera_position, THeadPoseData *new_camera_position) = 0;

+

+	virtual void getFilterFullName(QString *strToBeFilled) = 0;

+	virtual void getFilterShortName(QString *strToBeFilled) = 0;

+	virtual void getFilterDescription(QString *strToBeFilled) = 0;

+

+	//parameter value get/set - returns true if successful, false if not

+	virtual bool setParameterValue(const int index, const float newvalue) = 0;

+};

+

+// Handle type. In C++ language the iterface type is used.

+typedef IFilter* FILTERHANDLE;

+

+////////////////////////////////////////////////////////////////////////////////

+// 

+#ifdef __cplusplus

+#   define EXTERN_C     extern "C"

+#else

+#   define EXTERN_C

+#endif // __cplusplus

+

+// Factory function that creates instances of the Filter object.

+EXTERN_C

+FTNOIR_FILTER_BASE_EXPORT

+FILTERHANDLE

+__stdcall

+GetFilter(

+    void);

+

+#endif // FTNOIR_FILTER_BASE_H

diff --git a/FTNoIR_Filter_EWMA2/ftnoir_filter_base_global.h b/FTNoIR_Filter_EWMA2/ftnoir_filter_base_global.h
new file mode 100644
index 00000000..aac4048e
--- /dev/null
+++ b/FTNoIR_Filter_EWMA2/ftnoir_filter_base_global.h
@@ -0,0 +1,12 @@
+#ifndef FTNOIR_FILTER_BASE_GLOBAL_H

+#define FTNOIR_FILTER_BASE_GLOBAL_H

+

+#include <Qt/qglobal.h>

+

+#ifdef FTNOIR_FILTER_BASE_LIB

+# define FTNOIR_FILTER_BASE_EXPORT Q_DECL_EXPORT

+#else

+# define FTNOIR_FILTER_BASE_EXPORT Q_DECL_IMPORT

+#endif

+

+#endif // FTNOIR_FILTER_BASE_GLOBAL_H

diff --git a/FTNoIR_Filter_EWMA2/ftnoir_filter_ewma2.cpp b/FTNoIR_Filter_EWMA2/ftnoir_filter_ewma2.cpp
new file mode 100644
index 00000000..059e2807
--- /dev/null
+++ b/FTNoIR_Filter_EWMA2/ftnoir_filter_ewma2.cpp
@@ -0,0 +1,275 @@
+#include "ftnoir_filter_base.h"

+#include "math.h"

+

+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+//

+// EWMA Filter: Exponentially Weighted Moving Average filter with dynamic smoothing parameter

+//

+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+class FTNoIR_Filter_EWMA2 : public IFilter

+{

+public:

+	FTNoIR_Filter_EWMA2();

+	~FTNoIR_Filter_EWMA2();

+

+	void Release();

+    void Initialize();

+    void StartFilter();

+	void FilterHeadPoseData(THeadPoseData *current_camera_position, THeadPoseData *target_camera_position, THeadPoseData *new_camera_position);

+

+	void getFilterFullName(QString *strToBeFilled);

+	void getFilterShortName(QString *strToBeFilled);

+	void getFilterDescription(QString *strToBeFilled);

+

+	bool setParameterValue(const int index, const float newvalue);

+

+private:

+	THeadPoseData newHeadPose;								// Structure with new headpose

+

+	bool	first_run;

+	float	smoothing_frames_range;

+	float	alpha_smoothing;

+	float	prev_alpha[6];

+

+	//parameter list for the filter-function(s)

+	enum

+	{

+		kMinSmoothing=0,

+		kMaxSmoothing,

+		kSmoothingScaleCurve,

+		kNumFilterParameters								// Indicate number of parameters used

+	};

+

+	QString filterFullName;									// Filters' name and description

+	QString filterShortName;

+	QString filterDescription;

+

+	QList<float>					parameterValueAsFloat;

+	QList<std::pair<float,float>>	parameterRange;

+	QList<float>					parameterSteps;

+	QList<QString>					parameterNameAsString;

+	QList<QString>					parameterValueAsString;

+	QList<QString>					parameterUnitsAsString;

+};

+

+FTNoIR_Filter_EWMA2::FTNoIR_Filter_EWMA2()

+{

+	//populate the description strings

+	filterFullName = "EWMA Filter Mk2";

+	filterShortName = "EWMA";

+	filterDescription = "Exponentially Weighted Moving Average filter with dynamic smoothing parameter";

+

+	//allocate memory for the parameters

+	parameterValueAsFloat.clear();

+	parameterRange.clear();

+	parameterSteps.clear();

+	parameterNameAsString.clear();

+	parameterValueAsString.clear();

+	parameterUnitsAsString.clear();

+

+	//set up parameters

+	parameterNameAsString.append("MinSmoothing");

+	parameterUnitsAsString.append("Frames");

+	parameterRange.append(std::pair<float,float>(1.0f,100.0f));

+	parameterSteps.append(1.0f);

+	parameterValueAsFloat.append(0.0f);

+	parameterValueAsString.append("");

+	setParameterValue(kMinSmoothing,2.0f);

+

+	parameterNameAsString.append("MaxSmoothing");

+	parameterUnitsAsString.append("Frames");

+	parameterRange.append(std::pair<float,float>(1.0f,100.0f));

+	parameterSteps.append(1.0f);

+	parameterValueAsFloat.append(0.0f);

+	parameterValueAsString.append("");

+	setParameterValue(kMaxSmoothing,10.0f);

+

+	parameterNameAsString.append("SmoothingScaleCurve");

+	parameterUnitsAsString.append("Power");

+	parameterRange.append(std::pair<float,float>(0.25f,10.0f));

+	parameterSteps.append(0.0f);

+	parameterValueAsFloat.append(0.0f);

+	parameterValueAsString.append("");

+	setParameterValue(kSmoothingScaleCurve,10.0f);

+

+	first_run = true;

+	alpha_smoothing = 0.1f;					//this is a constant for now, might be a parameter later

+

+}

+

+FTNoIR_Filter_EWMA2::~FTNoIR_Filter_EWMA2()

+{

+

+}

+

+void FTNoIR_Filter_EWMA2::Release()

+{

+    delete this;

+}

+

+void FTNoIR_Filter_EWMA2::Initialize()

+{

+	return;

+}

+

+void FTNoIR_Filter_EWMA2::FilterHeadPoseData(THeadPoseData *current_camera_position, THeadPoseData *target_camera_position, THeadPoseData *new_camera_position)

+{

+	//non-optimised version for clarity

+	float prev_output[6];

+	float target[6];

+	float output_delta[6];

+	float scale[]={0.25f,0.25f,0.25f,6.0f,6.0f,6.0f};

+	float norm_output_delta[6];

+	float alpha[6];

+	float smoothed_alpha[6];

+	float output[6];

+	int i=0;

+

+	#if PRE_FILTER_SCALING

+	//compensate for any prefilter scaling

+	scale[0]*=X_POS_SCALE;

+	scale[1]*=Y_POS_SCALE;

+	scale[2]*=Z_POS_SCALE;

+	scale[3]*=X_ROT_SCALE;

+	scale[4]*=Y_ROT_SCALE;

+	scale[5]*=Z_ROT_SCALE;

+	#endif

+

+	//find out how far the head has moved

+	prev_output[0]=current_camera_position->x;

+	prev_output[1]=current_camera_position->y;

+	prev_output[2]=current_camera_position->z;

+	prev_output[3]=current_camera_position->yaw;

+	prev_output[4]=current_camera_position->pitch;

+	prev_output[5]=current_camera_position->roll;

+

+	target[0]=target_camera_position->x;

+	target[1]=target_camera_position->y;

+	target[2]=target_camera_position->z;

+	target[3]=target_camera_position->yaw;

+	target[4]=target_camera_position->pitch;

+	target[5]=target_camera_position->roll;

+

+	if (first_run==true)

+	{

+		//on the first run, output=target

+		for (i=0;i<6;i++)

+		{

+			output[i]=target[i];

+		}

+

+		new_camera_position->x=target[0];

+		new_camera_position->y=target[1];

+		new_camera_position->z=target[2];

+		new_camera_position->yaw=target[3];

+		new_camera_position->pitch=target[4];

+		new_camera_position->roll=target[5];

+

+		first_run=false;

+		

+		//we can bail

+		return;

+	}

+

+	//how far does the camera need to move to catch up?

+	for (i=0;i<6;i++)

+	{

+		output_delta[i]=(target[i]-prev_output[i]);

+	}

+

+	//normalise the deltas

+	for (i=0;i<6;i++)

+	{

+		norm_output_delta[i]=std::min(std::max(fabs(output_delta[i])/scale[i],0.0f),1.0f);

+	}

+

+	//calculate the alphas

+	//work out the dynamic smoothing factors

+	for (i=0;i<6;i++)

+	{

+		alpha[i]=1.0f/(parameterValueAsFloat[kMinSmoothing]+((1.0f-pow(norm_output_delta[i],parameterValueAsFloat[kSmoothingScaleCurve]))*smoothing_frames_range));

+		smoothed_alpha[i]=(alpha_smoothing*alpha[i])+((1.0f-alpha_smoothing)*prev_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 (i=0;i<6;i++)

+	{

+		if (smoothed_alpha[i]>=largest_alpha)

+		{

+			largest_alpha=smoothed_alpha[i];

+		}

+	}

+

+	//move the camera

+	for (i=0;i<6;i++)

+	{

+		output[i]=(largest_alpha*target[i])+((1.0f-largest_alpha)*prev_output[i]);

+	}

+

+	new_camera_position->x=output[0];

+	new_camera_position->y=output[1];

+	new_camera_position->z=output[2];

+	new_camera_position->yaw=output[3];

+	new_camera_position->pitch=output[4];

+	new_camera_position->roll=output[5];

+

+	//update filter memories ready for next sample

+	for (i=0;i<6;i++)

+	{

+		prev_alpha[i]=smoothed_alpha[i];

+	}

+	return;

+}

+

+void FTNoIR_Filter_EWMA2::getFilterFullName(QString *strToBeFilled)

+{

+	*strToBeFilled = filterFullName;

+};

+

+

+void FTNoIR_Filter_EWMA2::getFilterShortName(QString *strToBeFilled)

+{

+	*strToBeFilled = filterShortName;

+};

+

+

+void FTNoIR_Filter_EWMA2::getFilterDescription(QString *strToBeFilled)

+{

+	*strToBeFilled = filterDescription;

+};

+

+bool FTNoIR_Filter_EWMA2::setParameterValue(const int index, const float newvalue)

+{

+	if ((index >= 0) && (index < parameterValueAsFloat.size()))

+	{

+		parameterValueAsFloat[index]=std::min(std::max(newvalue,parameterRange[index].first),parameterRange[index].second);

+//		updateParameterString(index);

+

+		if (index==kMinSmoothing || index==kMaxSmoothing)

+		{

+			smoothing_frames_range=parameterValueAsFloat[kMaxSmoothing]-parameterValueAsFloat[kMinSmoothing];

+		}

+		return true;

+	}

+	else

+	{

+		return false;

+	}

+};

+

+

+////////////////////////////////////////////////////////////////////////////////

+// 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")

+

+FTNOIR_FILTER_BASE_EXPORT FILTERHANDLE __stdcall GetFilter()

+{

+	return new FTNoIR_Filter_EWMA2;

+}