Tryout with lowPassFilter after smoothing...

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

11 files changed, 249 insertions, 100 deletions

diff --git a/FaceTrackNoIR/FTServer.cpp b/FaceTrackNoIR/FTServer.cpp
index ebf16458..79d54ec6 100644
--- a/FaceTrackNoIR/FTServer.cpp
+++ b/FaceTrackNoIR/FTServer.cpp
@@ -140,7 +140,7 @@ void FTServer::run() {
 		}

 

 		// just for lower cpu load

-		msleep(30);	

+		msleep(15);	

 		yieldCurrentThread();

 		pMemData->data.DataID += 1;

 	}

diff --git a/FaceTrackNoIR/FaceTrackNoIR.cpp b/FaceTrackNoIR/FaceTrackNoIR.cpp
index 19af030e..05de8edd 100644
--- a/FaceTrackNoIR/FaceTrackNoIR.cpp
+++ b/FaceTrackNoIR/FaceTrackNoIR.cpp
@@ -420,6 +420,8 @@ void FaceTrackNoIR::startTracker( ) {
 	tracker->setRedY (ui.redY->value() );

 	tracker->setRedZ (ui.redZ->value() );

 

+	tracker->start( QThread::TimeCriticalPriority );

+

 	ui.headPoseWidget->show();

 

 	ui.btnStartTracker->setEnabled ( false );

@@ -638,6 +640,7 @@ void FaceTrackNoIR::createIconGroupBox()
 	ui.iconcomboBox->addItem(QIcon("images/Freetrack.ico"), tr("Freetrack"));

 	ui.iconcomboBox->addItem(QIcon("images/FlightGear.ico"), tr("FlightGear"));

 	ui.iconcomboBox->addItem(QIcon("images/FaceTrackNoIR.ico"), tr("FTNoir client"));

+	ui.iconcomboBox->addItem(QIcon("images/PPJoy.ico"), tr("Virtual Joystick"));

 

 	ui.iconcomboTrackerSource->addItem(QIcon("images/SeeingMachines.ico"), tr("Face API"));

 	ui.iconcomboTrackerSource->addItem(QIcon("images/FaceTrackNoIR.ico"), tr("FTNoir server"));

diff --git a/FaceTrackNoIR/FaceTrackNoIR.vcproj b/FaceTrackNoIR/FaceTrackNoIR.vcproj
index 74381133..da799291 100644
--- a/FaceTrackNoIR/FaceTrackNoIR.vcproj
+++ b/FaceTrackNoIR/FaceTrackNoIR.vcproj
@@ -210,6 +210,10 @@
 				>

 			</File>

 			<File

+				RelativePath=".\PPJoyServer.cpp"

+				>

+			</File>

+			<File

 				RelativePath=".\tracker.cpp"

 				>

 			</File>

@@ -332,6 +336,36 @@
 				>

 			</File>

 			<File

+				RelativePath=".\PPJIoctl.h"

+				>

+			</File>

+			<File

+				RelativePath=".\PPJoyServer.h"

+				>

+				<FileConfiguration

+					Name="Release|Win32"

+					>

+					<Tool

+						Name="VCCustomBuildTool"

+						Description="Moc&apos;ing $(InputFileName)..."

+						CommandLine="&quot;$(QTDIR)\bin\moc.exe&quot;  &quot;$(InputPath)&quot; -o &quot;.\GeneratedFiles\$(ConfigurationName)\moc_$(InputName).cpp&quot; -DQT_CORE_LIB -DQT_GUI_LIB -DQT_LARGEFILE_SUPPORT -DQT_NETWORK_LIB -DQT_NO_DEBUG -DQT_OPENGL_LIB -DQT_THREAD_SUPPORT -DUNICODE -DWIN32 -I&quot;$(QTDIR)\include\.&quot; -I&quot;$(QTDIR)\include\QtCore\.&quot; -I&quot;$(QTDIR)\include\QtGui\.&quot; -I&quot;$(QTDIR)\include\QtNetwork\.&quot; -I&quot;$(QTDIR)\include\QtOpenGL\.&quot; -I&quot;$(QTDIR)\include\QtTest\.&quot; -I&quot;$(QTDIR)\include\QtWebKit\.&quot; -I&quot;$(SM_API_CPP_WRAPPERS)\include\.&quot; -I&quot;$(SM_API_PATH)\include\.&quot; -I&quot;$(SM_API_QTDIR)\include\.&quot; -I&quot;$(SM_API_WIDGETS)\include\.&quot; -I&quot;.\GeneratedFiles\$(ConfigurationName)\.&quot; -I&quot;.\GeneratedFiles\.&quot;&#x0D;&#x0A;"

+						AdditionalDependencies="&quot;$(QTDIR)\bin\moc.exe&quot;;$(InputPath)"

+						Outputs="&quot;.\GeneratedFiles\$(ConfigurationName)\moc_$(InputName).cpp&quot;"

+					/>

+				</FileConfiguration>

+				<FileConfiguration

+					Name="Debug|Win32"

+					>

+					<Tool

+						Name="VCCustomBuildTool"

+						Description="Moc&apos;ing $(InputFileName)..."

+						CommandLine="&quot;$(QTDIR)\bin\moc.exe&quot;  &quot;$(InputPath)&quot; -o &quot;.\GeneratedFiles\$(ConfigurationName)\moc_$(InputName).cpp&quot; -DQT_CORE_LIB -DQT_GUI_LIB -DQT_LARGEFILE_SUPPORT -DQT_OPENGL_LIB -DQT_THREAD_SUPPORT -DUNICODE -DWIN32 -I&quot;$(QTDIR)\include\.&quot; -I&quot;$(QTDIR)\include\QtCore\.&quot; -I&quot;$(QTDIR)\include\QtGui\.&quot; -I&quot;$(QTDIR)\include\QtOpenGL\.&quot; -I&quot;$(QTDIR)\include\QtTest\.&quot; -I&quot;$(QTDIR)\include\QtWebKit\.&quot; -I&quot;.\.&quot; -I&quot;.\GeneratedFiles\$(ConfigurationName)\.&quot; -I&quot;.\GeneratedFiles\.&quot;&#x0D;&#x0A;"

+						AdditionalDependencies="&quot;$(QTDIR)\bin\moc.exe&quot;;$(InputPath)"

+						Outputs="&quot;.\GeneratedFiles\$(ConfigurationName)\moc_$(InputName).cpp&quot;"

+					/>

+				</FileConfiguration>

+			</File>

+			<File

 				RelativePath=".\resource.h"

 				>

 			</File>

@@ -586,6 +620,18 @@
 					</FileConfiguration>

 				</File>

 				<File

+					RelativePath=".\GeneratedFiles\Release\moc_PPJoyServer.cpp"

+					>

+					<FileConfiguration

+						Name="Debug|Win32"

+						ExcludedFromBuild="true"

+						>

+						<Tool

+							Name="VCCLCompilerTool"

+						/>

+					</FileConfiguration>

+				</File>

+				<File

 					RelativePath=".\GeneratedFiles\Release\moc_tracker.cpp"

 					>

 					<FileConfiguration

@@ -652,6 +698,18 @@
 					</FileConfiguration>

 				</File>

 				<File

+					RelativePath=".\GeneratedFiles\Debug\moc_PPJoyServer.cpp"

+					>

+					<FileConfiguration

+						Name="Release|Win32"

+						ExcludedFromBuild="true"

+						>

+						<Tool

+							Name="VCCLCompilerTool"

+						/>

+					</FileConfiguration>

+				</File>

+				<File

 					RelativePath=".\GeneratedFiles\Debug\moc_tracker.cpp"

 					>

 					<FileConfiguration

diff --git a/FaceTrackNoIR/FaceTrackNoIR.vcproj.VRIEND200810.Wim.user b/FaceTrackNoIR/FaceTrackNoIR.vcproj.VRIEND200810.Wim.user
index 27b5819e..adea3653 100644
--- a/FaceTrackNoIR/FaceTrackNoIR.vcproj.VRIEND200810.Wim.user
+++ b/FaceTrackNoIR/FaceTrackNoIR.vcproj.VRIEND200810.Wim.user
@@ -1,7 +1,7 @@
 <?xml version="1.0" encoding="Windows-1252"?>

 <VisualStudioUserFile

 	ProjectType="Visual C++"

-	Version="9,00"

+	Version="9.00"

 	ShowAllFiles="false"

 	>

 	<Configurations>

diff --git a/FaceTrackNoIR/GeneratedFiles/qrc_FaceTrackNoIR.cpp b/FaceTrackNoIR/GeneratedFiles/qrc_FaceTrackNoIR.cpp
index f66a19b2..cc294b4b 100644
--- a/FaceTrackNoIR/GeneratedFiles/qrc_FaceTrackNoIR.cpp
+++ b/FaceTrackNoIR/GeneratedFiles/qrc_FaceTrackNoIR.cpp
@@ -1,7 +1,7 @@
 /****************************************************************************

 ** Resource object code

 **

-** Created: Fri 4. Jun 14:33:52 2010

+** Created: Tue 8. Jun 22:08:02 2010

 **      by: The Resource Compiler for Qt version 4.6.2

 **

 ** WARNING! All changes made in this file will be lost!

diff --git a/FaceTrackNoIR/GeneratedFiles/ui_FaceTrackNoIR.h b/FaceTrackNoIR/GeneratedFiles/ui_FaceTrackNoIR.h
index 2f8224bf..e6a0fa02 100644
--- a/FaceTrackNoIR/GeneratedFiles/ui_FaceTrackNoIR.h
+++ b/FaceTrackNoIR/GeneratedFiles/ui_FaceTrackNoIR.h
@@ -1,7 +1,7 @@
 /********************************************************************************

 ** Form generated from reading UI file 'FaceTrackNoIR.ui'

 **

-** Created: Fri 4. Jun 14:33:49 2010

+** Created: Tue 8. Jun 22:08:00 2010

 **      by: Qt User Interface Compiler version 4.6.2

 **

 ** WARNING! All changes made in this file will be lost when recompiling UI file!

@@ -91,11 +91,12 @@ public:
     QLabel *lblSensYaw_3;

     QSlider *slideSmoothing;

     QSpinBox *spinSmoothing;

-    QSlider *slideNeutralZone;

-    QSpinBox *spinNeutralZone;

-    QLabel *lblSensYaw_4;

     QCheckBox *chkUseEWMA;

     QLabel *lblSensYaw_5;

+    QLabel *lblSensYaw_4;

+    QSlider *slideNeutralZone;

+    QSpinBox *spinNeutralZone;

+    QLabel *lblSensYaw_6;

     QSpacerItem *horizontalSpacer_3;

     QSpacerItem *verticalSpacer;

     QHBoxLayout *horizontalLayout_11;

@@ -546,7 +547,7 @@ public:
         slideSmoothing->setOrientation(Qt::Horizontal);

         slideSmoothing->setTickPosition(QSlider::NoTicks);

 

-        gridLayout_2->addWidget(slideSmoothing, 1, 0, 1, 1);

+        gridLayout_2->addWidget(slideSmoothing, 2, 0, 1, 1);

 

         spinSmoothing = new QSpinBox(widget);

         spinSmoothing->setObjectName(QString::fromUtf8("spinSmoothing"));

@@ -554,14 +555,37 @@ public:
         spinSmoothing->setMaximum(120);

         spinSmoothing->setValue(10);

 

-        gridLayout_2->addWidget(spinSmoothing, 1, 1, 1, 1);

+        gridLayout_2->addWidget(spinSmoothing, 2, 1, 1, 1);

+

+        chkUseEWMA = new QCheckBox(widget);

+        chkUseEWMA->setObjectName(QString::fromUtf8("chkUseEWMA"));

+

+        gridLayout_2->addWidget(chkUseEWMA, 3, 1, 1, 1);

+

+        lblSensYaw_5 = new QLabel(widget);

+        lblSensYaw_5->setObjectName(QString::fromUtf8("lblSensYaw_5"));

+        lblSensYaw_5->setMinimumSize(QSize(25, 0));

+        lblSensYaw_5->setMaximumSize(QSize(150, 16777215));

+        lblSensYaw_5->setStyleSheet(QString::fromUtf8("color:#ccc;\n"

+"background:none;"));

+

+        gridLayout_2->addWidget(lblSensYaw_5, 3, 0, 1, 1);

+

+        lblSensYaw_4 = new QLabel(widget);

+        lblSensYaw_4->setObjectName(QString::fromUtf8("lblSensYaw_4"));

+        lblSensYaw_4->setMinimumSize(QSize(25, 0));

+        lblSensYaw_4->setMaximumSize(QSize(150, 16777215));

+        lblSensYaw_4->setStyleSheet(QString::fromUtf8("color:#ccc;\n"

+"background:none;"));

+

+        gridLayout_2->addWidget(lblSensYaw_4, 1, 0, 1, 1);

 

         slideNeutralZone = new QSlider(widget);

         slideNeutralZone->setObjectName(QString::fromUtf8("slideNeutralZone"));

         slideNeutralZone->setMinimumSize(QSize(50, 15));

-        slideNeutralZone->setMinimum(0);

+        slideNeutralZone->setMinimum(1);

         slideNeutralZone->setMaximum(45);

-        slideNeutralZone->setPageStep(2);

+        slideNeutralZone->setPageStep(5);

         slideNeutralZone->setValue(5);

         slideNeutralZone->setOrientation(Qt::Horizontal);

         slideNeutralZone->setTickPosition(QSlider::NoTicks);

@@ -572,32 +596,19 @@ public:
         spinNeutralZone->setObjectName(QString::fromUtf8("spinNeutralZone"));

         spinNeutralZone->setMinimumSize(QSize(50, 22));

         spinNeutralZone->setMaximum(45);

+        spinNeutralZone->setSingleStep(5);

         spinNeutralZone->setValue(5);

 

         gridLayout_2->addWidget(spinNeutralZone, 5, 1, 1, 1);

 

-        lblSensYaw_4 = new QLabel(widget);

-        lblSensYaw_4->setObjectName(QString::fromUtf8("lblSensYaw_4"));

-        lblSensYaw_4->setMinimumSize(QSize(25, 0));

-        lblSensYaw_4->setMaximumSize(QSize(150, 16777215));

-        lblSensYaw_4->setStyleSheet(QString::fromUtf8("color:#ccc;\n"

+        lblSensYaw_6 = new QLabel(widget);

+        lblSensYaw_6->setObjectName(QString::fromUtf8("lblSensYaw_6"));

+        lblSensYaw_6->setMinimumSize(QSize(25, 0));

+        lblSensYaw_6->setMaximumSize(QSize(150, 16777215));

+        lblSensYaw_6->setStyleSheet(QString::fromUtf8("color:#ccc;\n"

 "background:none;"));

 

-        gridLayout_2->addWidget(lblSensYaw_4, 3, 0, 1, 1);

-

-        chkUseEWMA = new QCheckBox(widget);

-        chkUseEWMA->setObjectName(QString::fromUtf8("chkUseEWMA"));

-

-        gridLayout_2->addWidget(chkUseEWMA, 2, 1, 1, 1);

-

-        lblSensYaw_5 = new QLabel(widget);

-        lblSensYaw_5->setObjectName(QString::fromUtf8("lblSensYaw_5"));

-        lblSensYaw_5->setMinimumSize(QSize(25, 0));

-        lblSensYaw_5->setMaximumSize(QSize(150, 16777215));

-        lblSensYaw_5->setStyleSheet(QString::fromUtf8("color:#ccc;\n"

-"background:none;"));

-

-        gridLayout_2->addWidget(lblSensYaw_5, 2, 0, 1, 1);

+        gridLayout_2->addWidget(lblSensYaw_6, 4, 0, 1, 1);

 

 

         horizontalLayout_3->addLayout(gridLayout_2);

@@ -1096,9 +1107,7 @@ public:
         QWidget::setTabOrder(sensZ, spinSensZ);

         QWidget::setTabOrder(spinSensZ, slideSmoothing);

         QWidget::setTabOrder(slideSmoothing, spinSmoothing);

-        QWidget::setTabOrder(spinSmoothing, slideNeutralZone);

-        QWidget::setTabOrder(slideNeutralZone, spinNeutralZone);

-        QWidget::setTabOrder(spinNeutralZone, headXLine);

+        QWidget::setTabOrder(spinSmoothing, headXLine);

         QWidget::setTabOrder(headXLine, headRotXLine);

         QWidget::setTabOrder(headRotXLine, headYLine);

         QWidget::setTabOrder(headYLine, headRotYLine);

@@ -1130,8 +1139,6 @@ public:
         QObject::connect(spinSensY, SIGNAL(valueChanged(int)), sensY, SLOT(setValue(int)));

         QObject::connect(sensZ, SIGNAL(valueChanged(int)), spinSensZ, SLOT(setValue(int)));

         QObject::connect(spinSensZ, SIGNAL(valueChanged(int)), sensZ, SLOT(setValue(int)));

-        QObject::connect(slideNeutralZone, SIGNAL(valueChanged(int)), spinNeutralZone, SLOT(setValue(int)));

-        QObject::connect(spinNeutralZone, SIGNAL(valueChanged(int)), slideNeutralZone, SLOT(setValue(int)));

         QObject::connect(slideSmoothing, SIGNAL(valueChanged(int)), spinSmoothing, SLOT(setValue(int)));

         QObject::connect(spinSmoothing, SIGNAL(valueChanged(int)), slideSmoothing, SLOT(setValue(int)));

         QObject::connect(redYaw, SIGNAL(valueChanged(int)), spinRedYaw, SLOT(setValue(int)));

@@ -1146,6 +1153,8 @@ public:
         QObject::connect(spinRedY, SIGNAL(valueChanged(int)), redY, SLOT(setValue(int)));

         QObject::connect(redZ, SIGNAL(valueChanged(int)), spinRedZ, SLOT(setValue(int)));

         QObject::connect(spinRedZ, SIGNAL(valueChanged(int)), redZ, SLOT(setValue(int)));

+        QObject::connect(slideNeutralZone, SIGNAL(valueChanged(int)), spinNeutralZone, SLOT(setValue(int)));

+        QObject::connect(spinNeutralZone, SIGNAL(valueChanged(int)), slideNeutralZone, SLOT(setValue(int)));

 

         iconcomboTrackerSource->setCurrentIndex(-1);

         iconcomboBox->setCurrentIndex(-1);

@@ -1209,10 +1218,11 @@ public:
 #endif // QT_NO_TOOLTIP

         btnStopTracker->setText(QApplication::translate("FaceTrackNoIRClass", "Stop", 0, QApplication::UnicodeUTF8));

         groupGameProtocol->setTitle(QApplication::translate("FaceTrackNoIRClass", "Game protocol", 0, QApplication::UnicodeUTF8));

-        lblSensYaw_3->setText(QApplication::translate("FaceTrackNoIRClass", "Smoothing (samples)", 0, QApplication::UnicodeUTF8));

-        lblSensYaw_4->setText(QApplication::translate("FaceTrackNoIRClass", "Rotation Neutral Zone (degr.)", 0, QApplication::UnicodeUTF8));

+        lblSensYaw_3->setText(QApplication::translate("FaceTrackNoIRClass", "Global settings", 0, QApplication::UnicodeUTF8));

         chkUseEWMA->setText(QString());

-        lblSensYaw_5->setText(QApplication::translate("FaceTrackNoIRClass", "Use EWMA filtering", 0, QApplication::UnicodeUTF8));

+        lblSensYaw_5->setText(QApplication::translate("FaceTrackNoIRClass", "Use EWMA filtering:", 0, QApplication::UnicodeUTF8));

+        lblSensYaw_4->setText(QApplication::translate("FaceTrackNoIRClass", "Smoothing (samples):", 0, QApplication::UnicodeUTF8));

+        lblSensYaw_6->setText(QApplication::translate("FaceTrackNoIRClass", "Neutral Zone:", 0, QApplication::UnicodeUTF8));

         cameraName->setText(QApplication::translate("FaceTrackNoIRClass", "Camera Name", 0, QApplication::UnicodeUTF8));

         lblSensitivity->setText(QApplication::translate("FaceTrackNoIRClass", "Sensitivity (100 = x1)", 0, QApplication::UnicodeUTF8));

         lblSensYaw->setText(QApplication::translate("FaceTrackNoIRClass", "Yaw", 0, QApplication::UnicodeUTF8));

diff --git a/FaceTrackNoIR/Release/BuildLog.htm b/FaceTrackNoIR/Release/BuildLog.htm
index 18bd1e16..4c7f9692 100644
--- a/FaceTrackNoIR/Release/BuildLog.htm
+++ b/FaceTrackNoIR/Release/BuildLog.htm
diff --git a/FaceTrackNoIR/Release/mt.dep b/FaceTrackNoIR/Release/mt.dep
index b12f071f..ad86a4c9 100644
--- a/FaceTrackNoIR/Release/mt.dep
+++ b/FaceTrackNoIR/Release/mt.dep
@@ -1 +1 @@
-Manifest resource last updated at 21:17:07,84 on ma 31-05-2010 

+Manifest resource last updated at 13:38:23.50 on vr 11-06-2010 

diff --git a/FaceTrackNoIR/Release/vc90.idb b/FaceTrackNoIR/Release/vc90.idb
index fbbc4b50..a737ef03 100644
--- a/FaceTrackNoIR/Release/vc90.idb
+++ b/FaceTrackNoIR/Release/vc90.idb
diff --git a/FaceTrackNoIR/tracker.cpp b/FaceTrackNoIR/tracker.cpp
index be428fac..9141dde7 100644
--- a/FaceTrackNoIR/tracker.cpp
+++ b/FaceTrackNoIR/tracker.cpp
@@ -161,6 +161,10 @@ void Tracker::run() {
 	bool lastBackKey = false;						// Remember state, to detect rising edge

 	bool lastEqualsKey = false;

 

+	SYSTEMTIME now;

+	long newHeadPoseTime;

+	float dT;

+

 	//QFile data("output.txt");

 	//if (data.open(QFile::WriteOnly | QFile::Truncate)) {

 	//	QTextStream out(&data);

@@ -261,6 +265,17 @@ void Tracker::run() {
 		   }

 		}

 

+		//

+		// Get the System-time and substract the time from the previous call.

+		// dT will be used for the EWMA-filter.

+		//

+		GetSystemTime ( &now );

+		newHeadPoseTime = (((now.wHour * 3600) + (now.wMinute * 60) + now.wSecond) * 1000) + now.wMilliseconds;

+		dT = (newHeadPoseTime - Tracker::prevHeadPoseTime) / 1000.0f;

+

+		// Remember time for next call

+		Tracker::prevHeadPoseTime = newHeadPoseTime;

+

 		//if the confidence is good enough the headpose will be updated **/

 		if (Tracker::confid) {

 

@@ -318,9 +333,33 @@ void Tracker::run() {
 			//

 			// Also send the Virtual Pose to FT-server and FG-server

 			//

-			server_FT->setVirtRotX ( Tracker::Pitch.invert * Tracker::Pitch.sens * (getSmoothFromList( &Pitch.rawList ) - Pitch.offset_headPos) );

-			server_FT->setVirtRotY ( Tracker::Yaw.invert   * Tracker::Yaw.sens   * (getSmoothFromList( &Yaw.rawList ) - Yaw.offset_headPos) );

-			server_FT->setVirtRotZ ( Tracker::Roll.invert  * Tracker::Roll.sens  * (getSmoothFromList( &Roll.rawList ) - Roll.offset_headPos) );

+			if (Tracker::useFilter) {

+				Pitch.newPos = lowPassFilter ( getSmoothFromList( &Pitch.rawList ) - Pitch.offset_headPos, 

+											   &Pitch.prevPos, dT, Tracker::Pitch.red );

+			}

+			else {

+				Pitch.newPos = getSmoothFromList( &Pitch.rawList ) - Pitch.offset_headPos;

+			}

+			server_FT->setVirtRotX ( Tracker::Pitch.invert * Tracker::Pitch.sens * Pitch.newPos );

+

+			if (Tracker::useFilter) {

+				Yaw.newPos = lowPassFilter ( getSmoothFromList( &Yaw.rawList ) - Yaw.offset_headPos, 

+											   &Yaw.prevPos, dT, Tracker::Yaw.red );

+			}

+			else {

+				Yaw.newPos = getSmoothFromList( &Yaw.rawList ) - Yaw.offset_headPos;

+			}

+			server_FT->setVirtRotY ( Tracker::Yaw.invert   * Tracker::Yaw.sens   *  Yaw.newPos );

+

+			if (Tracker::useFilter) {

+				Roll.newPos = lowPassFilter ( getSmoothFromList( &Roll.rawList ) - Roll.offset_headPos, 

+											   &Roll.prevPos, dT, Tracker::Roll.red );

+			}

+			else {

+				Roll.newPos = getSmoothFromList( &Roll.rawList ) - Roll.offset_headPos;

+			}

+			server_FT->setVirtRotZ ( Tracker::Roll.invert  * Tracker::Roll.sens  * Roll.newPos );

+

 			server_FT->setVirtPosX ( ( Tracker::X.invert   * Tracker::X.sens     * (getSmoothFromList( &X.rawList ) - X.offset_headPos) ) * 1000.0f);

 			server_FT->setVirtPosY ( ( Tracker::Y.invert   * Tracker::Y.sens     * (getSmoothFromList( &Y.rawList ) - Y.offset_headPos) ) * 1000.0f );

 			server_FT->setVirtPosZ ( ( Tracker::Z.invert   * Tracker::Z.sens     * (getSmoothFromList( &Z.rawList ) - Z.offset_headPos - Tracker::Z.initial_headPos) ) * 1000.0f );

@@ -362,15 +401,15 @@ void Tracker::registerHeadPoseCallback() {
 	Q_ASSERT(_engine_handle);

 	smReturnCode error = smHTRegisterHeadPoseCallback( _engine->handle(), 0, receiveHeadPose);

 	//showErrorBox(0, "Register HeadPose Callback", error);

-	start(LowestPriority);

 }

 

 /** Callback function for head-pose - only static methods could be called **/

 void Tracker::receiveHeadPose(void *,smEngineHeadPoseData head_pose, smCameraVideoFrame video_frame)

 {

-	SYSTEMTIME now;

-	long newHeadPoseTime;

-	float dT;

+	//SYSTEMTIME now;

+	//long newHeadPoseTime;

+	//float dT;

+//	float rate;

 

 	//

 	// Perform actions, when valid data is received from faceAPI.

@@ -385,25 +424,17 @@ void Tracker::receiveHeadPose(void *,smEngineHeadPoseData head_pose, smCameraVid
 		Tracker::setHeadRotY(head_pose.head_rot.y_rads);

 		Tracker::setHeadRotZ(head_pose.head_rot.z_rads);	

 

-		//

-		// Get the System-time and substract the time from the previous call.

-		// dT will be used for the EWMA-filter.

-		//

-		GetSystemTime ( &now );

-		newHeadPoseTime = (((now.wHour * 3600) + (now.wMinute * 60) + now.wSecond) * 1000) + now.wMilliseconds;

-		dT = (newHeadPoseTime - Tracker::prevHeadPoseTime) / 1000.0f;

+		////

+		//// Get the System-time and substract the time from the previous call.

+		//// dT will be used for the EWMA-filter.

+		////

+		//GetSystemTime ( &now );

+		//newHeadPoseTime = (((now.wHour * 3600) + (now.wMinute * 60) + now.wSecond) * 1000) + now.wMilliseconds;

+		//dT = (newHeadPoseTime - Tracker::prevHeadPoseTime) / 1000.0f;

 

-		// Remember time for next call

-		Tracker::prevHeadPoseTime = newHeadPoseTime;

+		//// Remember time for next call

+		//Tracker::prevHeadPoseTime = newHeadPoseTime;

 

-		//

-		// Log something

-		//

-		//QFile data("output.txt");

-		//if (data.open(QFile::WriteOnly | QFile::Append)) {

-		//	QTextStream out(&data);

-		//	out << "ReceiveHeadPose:" << dT << '\n';

-		//}

 

 

 		//

@@ -411,60 +442,72 @@ void Tracker::receiveHeadPose(void *,smEngineHeadPoseData head_pose, smCameraVid
 		// Add the values to their respective QList, for further smoothing

 		//

 		// Pitch

-		if (Tracker::useFilter) {

-			Pitch.newPos = lowPassFilter ( getCorrectedNewRaw ( Tracker::Pitch.headPos, rotNeutralZone ), 

-				                           &Pitch.prevPos, dT, Tracker::Pitch.red );

-		}

-		else {

+		//if (Tracker::useFilter) {

+		//	Pitch.newPos = lowPassFilter ( getCorrectedNewRaw ( rateLimiter ( Tracker::Pitch.headPos, &Tracker::Pitch.prevRawPos, dT, 1.0f ), rotNeutralZone ), 

+		//		                           &Pitch.prevPos, dT, Tracker::Pitch.red );

+		//}

+		//else {

 			Pitch.newPos = getCorrectedNewRaw ( Tracker::Pitch.headPos, rotNeutralZone );

-		}

+		//}

 		addRaw2List ( &Pitch.rawList, Pitch.maxItems, Pitch.newPos );

 

+		//

+		// Log something

+		//

+		//rate = rateLimiter ( Tracker::Pitch.headPos, &Tracker::Pitch.prevRawPos, dT, 1.0f );

+		//QFile data("output.txt");

+		//if (data.open(QFile::WriteOnly | QFile::Append)) {

+		//	QTextStream out(&data);

+		//	out << "Limited Raw= " << rate << " dT= " << dT << " Raw= " << Tracker::Pitch.headPos << " Filtered= " << Pitch.newPos << '\n';

+		//}

+//		Tracker::Pitch.prevRawPos = Tracker::Pitch.headPos;

+

+

 		// Yaw

-		if (Tracker::useFilter) {

-			Yaw.newPos = lowPassFilter ( getCorrectedNewRaw ( Tracker::Yaw.headPos, rotNeutralZone ), 

-										 &Yaw.prevPos, dT, Tracker::Yaw.red );

-		}

-		else {

+		//if (Tracker::useFilter) {

+		//	Yaw.newPos = lowPassFilter ( getCorrectedNewRaw ( rateLimiter ( Tracker::Yaw.headPos, &Tracker::Yaw.prevRawPos, dT, 1.0f ), rotNeutralZone ), 

+		//								 &Yaw.prevPos, dT, Tracker::Yaw.red );

+		//}

+		//else {

 			Yaw.newPos = getCorrectedNewRaw ( Tracker::Yaw.headPos, rotNeutralZone );

-		}

+		//}

 		addRaw2List ( &Yaw.rawList, Yaw.maxItems, Yaw.newPos );

 

 		// Roll

-		if (Tracker::useFilter) {

-			Roll.newPos = lowPassFilter ( getCorrectedNewRaw (Tracker::Roll.headPos, rotNeutralZone ),

-										  &Roll.prevPos, dT, Tracker::Roll.red );

-		}

-		else {

-			Roll.newPos = getCorrectedNewRaw (Tracker::Roll.headPos, rotNeutralZone );

-		}

+		//if (Tracker::useFilter) {

+		//	Roll.newPos = lowPassFilter ( getCorrectedNewRaw (rateLimiter ( Tracker::Roll.headPos, &Tracker::Roll.prevRawPos, dT, 1.0f ), rotNeutralZone ),

+		//								  &Roll.prevPos, dT, Tracker::Roll.red );

+		//}

+		//else {

+			Roll.newPos = getCorrectedNewRaw ( Tracker::Roll.headPos, rotNeutralZone );

+		//}

 		addRaw2List ( &Roll.rawList, Roll.maxItems, Roll.newPos );

 

 		// X-position

-		if (Tracker::useFilter) {

-			X.newPos = lowPassFilter ( Tracker::X.headPos, &X.prevPos, dT, Tracker::X.red );

-		}

-		else {

+		//if (Tracker::useFilter) {

+		//	X.newPos = lowPassFilter ( Tracker::X.headPos, &X.prevPos, dT, Tracker::X.red );

+		//}

+		//else {

 			X.newPos = Tracker::X.headPos;

-		}

+		//}

 		addRaw2List ( &X.rawList, X.maxItems, X.newPos );

 

 		// Y-position

-		if (Tracker::useFilter) {

-			Y.newPos = lowPassFilter ( Tracker::Y.headPos, &Y.prevPos, dT, Tracker::Y.red );

-		}

-		else {

+		//if (Tracker::useFilter) {

+		//	Y.newPos = lowPassFilter ( Tracker::Y.headPos, &Y.prevPos, dT, Tracker::Y.red );

+		//}

+		//else {

 			Y.newPos = Tracker::Y.headPos;

-		}

+		//}

 		addRaw2List ( &Y.rawList, Y.maxItems, Y.newPos );

 

 		// Z-position (distance to camera, absolute!)

-		if (Tracker::useFilter) {

-			Z.newPos = lowPassFilter ( Tracker::Z.headPos, &Z.prevPos, dT, Tracker::Z.red );

-		}

-		else {

+		//if (Tracker::useFilter) {

+		//	Z.newPos = lowPassFilter ( Tracker::Z.headPos, &Z.prevPos, dT, Tracker::Z.red );

+		//}

+		//else {

 			Z.newPos = Tracker::Z.headPos;

-		}

+		//}

 		addRaw2List ( &Z.rawList, Z.maxItems, Z.newPos );

 

 	} else {

@@ -472,7 +515,7 @@ void Tracker::receiveHeadPose(void *,smEngineHeadPoseData head_pose, smCameraVid
 	}

 

 	// for lower cpu load

-	msleep(30);

+	msleep(15);

 	yieldCurrentThread(); 

 }

 

@@ -573,10 +616,11 @@ float Tracker::getCorrectedNewRaw ( float NewRaw, float rotNeutral ) {
 }

 

 //

-// Implementation of an Exponentially Weighed Moving Average, used to serve as a low-pass filter.

+// Implementation of an Exponentially Weighted Moving Average, used to serve as a low-pass filter.

 // The code was adopted from Melchior Franz, who created it for FlightGear (aircraft.nas).

 //

 // The function takes the new value, the delta-time (sec) and a weighing coefficient (>0 and <1)

+// All previou values are taken into account, the weight of this is determined by 'coeff'.

 //

 float Tracker::lowPassFilter ( float newvalue, float *oldvalue, float dt, float coeff) {

 float c = 0.0f;

@@ -588,3 +632,35 @@ float fil = 0.0f;
 

 	return fil;

 }

+

+//

+// Implementation of a Rate Limiter, used to eliminate spikes in the raw data.

+//

+// The function takes the new value, the delta-time (sec) and the positive max. slew-rate (engineering units/sec)

+//

+float Tracker::rateLimiter ( float newvalue, float *oldvalue, float dt, float max_rate) {

+float rate = 0.0f;

+float clamped_value = 0.0f;

+

+	rate = (newvalue - *oldvalue) / dt;

+	clamped_value = newvalue;									// If all is well, the newvalue is returned

+

+	//

+	// One max-rate is used for ramp-up and ramp-down

+	// If the rate exceeds max_rate, return the maximum value that the max_rate allows

+	//

+	if (fabs(rate) > max_rate) {

+		//

+		// For ramp-down, apply a factor -1 to the max_rate

+		//

+		if (rate < 0.0f) {

+			clamped_value = (-1.0f * dt * max_rate) + *oldvalue;

+		}

+		else {

+			clamped_value = (dt * max_rate) + *oldvalue;

+		}

+	}

+	*oldvalue = clamped_value;

+

+	return clamped_value;

+}
\ No newline at end of file
diff --git a/FaceTrackNoIR/tracker.h b/FaceTrackNoIR/tracker.h
index dcb075f6..23768f95 100644
--- a/FaceTrackNoIR/tracker.h
+++ b/FaceTrackNoIR/tracker.h
@@ -61,6 +61,7 @@ struct THeadPoseDOF {
 	int maxItems;					// Maximum number of elements is rawList

 	float newPos;					// New Position (used locally)

 	float prevPos;					// Previous Position

+	float prevRawPos;				// Previous Raw Position

 };

 

 class Tracker : public QThread {

@@ -81,6 +82,7 @@ private:
 	static void STDCALL receiveHeadPose(void *,smEngineHeadPoseData head_pose, smCameraVideoFrame video_frame);

 	static void addRaw2List ( QList<float> *rawList, float maxIndex, float raw );

 	static float lowPassFilter ( float newvalue, float *oldvalue, float dt, float coeff);

+	static float rateLimiter ( float newvalue, float *oldvalue, float dt, float max_rate);

 	static float getCorrectedNewRaw ( float NewRaw, float rotNeutral );

 

 	/** static member variables for saving the head pose **/