Timing faceAPI verbeterd en EWMA-filter voltooid.

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

8 files changed, 247 insertions, 258 deletions

diff --git a/FaceTrackNoIR/FaceTrackNoIR.cpp b/FaceTrackNoIR/FaceTrackNoIR.cpp
index 0183d3f9..18a06ecc 100644
--- a/FaceTrackNoIR/FaceTrackNoIR.cpp
+++ b/FaceTrackNoIR/FaceTrackNoIR.cpp
@@ -345,6 +345,7 @@ void FaceTrackNoIR::loadSettings() {
 

 	iniFile.beginGroup ( "GameProtocol" );

 	ui.iconcomboBox->setCurrentIndex(iniFile.value ( "Selection", 0 ).toInt());

+	setIcon( ui.iconcomboBox->currentIndex() );

 	iniFile.endGroup ();

 

 	settingsDirty = false;

@@ -390,11 +391,14 @@ void FaceTrackNoIR::startTracker( ) {
 	l->addWidget(_display);

 

 	//

-	// Setup the Tracker and send the settings, just to be sure...

+	// Setup the Tracker and send the settings.

+	// This is necessary, because the events are only triggered 'on change'

 	//

 	tracker->setup( ui.headPoseWidget , this);

 	tracker->setSmoothing ( ui.slideSmoothing->value() );

 	tracker->setNeutralZone ( ui.slideNeutralZone->value() );

+	tracker->setUseFilter (ui.chkUseEWMA->isChecked() );

+

 	tracker->setSensYaw (ui.sensYaw->value() );

 	tracker->setSensPitch (ui.sensPitch->value() );

 	tracker->setSensRoll (ui.sensRoll->value() );

@@ -402,6 +406,20 @@ void FaceTrackNoIR::startTracker( ) {
 	tracker->setSensY (ui.sensY->value() );

 	tracker->setSensZ (ui.sensZ->value() );

 

+	tracker->setInvertYaw (ui.chkInvertYaw->isChecked() );

+	tracker->setInvertPitch (ui.chkInvertPitch->isChecked() );

+	tracker->setInvertRoll (ui.chkInvertRoll->isChecked() );

+	tracker->setInvertX (ui.chkInvertX->isChecked() );

+	tracker->setInvertY (ui.chkInvertY->isChecked() );

+	tracker->setInvertZ (ui.chkInvertZ->isChecked() );

+

+	tracker->setRedYaw (ui.redYaw->value() );

+	tracker->setRedPitch (ui.redPitch->value() );

+	tracker->setRedRoll (ui.redRoll->value() );

+	tracker->setRedX (ui.redX->value() );

+	tracker->setRedY (ui.redY->value() );

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

+

 	ui.headPoseWidget->show();

 

 	ui.btnStartTracker->setEnabled ( false );

@@ -682,8 +700,8 @@ void FaceTrackNoIR::iconActivated(QSystemTrayIcon::ActivationReason reason)
      switch (reason) {

      case QSystemTrayIcon::Trigger:

      case QSystemTrayIcon::DoubleClick:

-         ui.iconcomboBox->setCurrentIndex((ui.iconcomboBox->currentIndex() + 1)

-                                       % ui.iconcomboBox->count());

+         //ui.iconcomboBox->setCurrentIndex((ui.iconcomboBox->currentIndex() + 1)

+         //                              % ui.iconcomboBox->count());

          break;

      ////case QSystemTrayIcon::MiddleClick:

      ////    showMessage();

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

 ** Resource object code

 **

-** Created: Fri 21. May 10:07:42 2010

+** Created: Fri 4. Jun 14:33:52 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 1adf2690..2f8224bf 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: Wed 2. Jun 19:35:54 2010

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

 **      by: Qt User Interface Compiler version 4.6.2

 **

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

diff --git a/FaceTrackNoIR/tracker.cpp b/FaceTrackNoIR/tracker.cpp
index 377f605d..fc4a8d20 100644
--- a/FaceTrackNoIR/tracker.cpp
+++ b/FaceTrackNoIR/tracker.cpp
@@ -23,6 +23,10 @@
 *********************************************************************************/

 /*

 	Modifications (last one on top):

+		20100604 - WVR: Created structure for DOF-data and changed timing of

+						ReceiveHeadPose end run().

+		20100602 - WVR: Implemented EWMA-filtering, according to the example of

+						Melchior Franz. Works like a charm...

 		20100601 - WVR: Added DirectInput keyboard-handling. '=' used for center,

 						'BACK' for start (+center)/stop.

 		20100517 - WVR: Added upstream command(s) from FlightGear

@@ -35,55 +39,21 @@
 using namespace sm::faceapi;

 using namespace sm::faceapi::qt;

 

-float Tracker::headPosX = 0.0f;

-float Tracker::headPosY = 0.0f;

-float Tracker::headPosZ = 0.0f;

-float Tracker::initial_headPosZ = 0.0f;

-

-float Tracker::headRotX = 0.0f;

-float Tracker::headRotY = 0.0f;

-float Tracker::headRotZ = 0.0f;

-

-// Offsets, to center headpos while tracking

-float Tracker::offset_headPosX = 0.0f;

-float Tracker::offset_headPosY = 0.0f;

-float Tracker::offset_headPosZ = 0.0f;

-

-float Tracker::offset_headRotX = 0.0f;

-float Tracker::offset_headRotY = 0.0f;

-float Tracker::offset_headRotZ = 0.0f;

-

 // Flags

 bool Tracker::confid = false;

-bool Tracker::newdata = false;

 bool Tracker::set_initial = false;

 bool Tracker::do_tracking = true;

 bool Tracker::do_center = false;

-

-float Tracker::sensYaw = 1.0f;

-float Tracker::sensPitch = 1.0f;

-float Tracker::sensRoll = 1.0f;

-float Tracker::sensX = 1.0f;

-float Tracker::sensY = 1.0f;

-float Tracker::sensZ = 1.0f;

-

-float Tracker::invertYaw = 1.0f;

-float Tracker::invertPitch = 1.0f;

-float Tracker::invertRoll = 1.0f;

-float Tracker::invertX = 1.0f;

-float Tracker::invertY = 1.0f;

-float Tracker::invertZ = 1.0f;

-

 bool Tracker::useFilter = false;

 

-float Tracker::redYaw = 0.7f;

-float Tracker::redPitch = 0.7f;

-float Tracker::redRoll = 0.7f;

-float Tracker::redX = 0.7f;

-float Tracker::redY = 0.7f;

-float Tracker::redZ = 0.7f;

-

 float Tracker::rotNeutralZone = 0.087f;					// Neutral Zone for rotations (rad)

+long Tracker::prevHeadPoseTime = 0;

+THeadPoseDOF Tracker::Pitch;							// One structure for each of 6DOF's

+THeadPoseDOF Tracker::Yaw;

+THeadPoseDOF Tracker::Roll;

+THeadPoseDOF Tracker::X;

+THeadPoseDOF Tracker::Y;

+THeadPoseDOF Tracker::Z;

 

 /** constructor empty **/

 Tracker::Tracker() {

@@ -186,23 +156,9 @@ void Tracker::run() {
 	LPDIRECTINPUTDEVICE8 dinkeyboard;				// the pointer to the keyboard device

 	BYTE keystate[256];								// the storage for the key-information

 	HRESULT retAcquire;

-	bool lastMinusKey = false;						// Remember state, to detect rising edge

+	bool lastBackKey = false;						// Remember state, to detect rising edge

 	bool lastEqualsKey = false;

 

-	float prevYaw = 0.0f;							// Remember previous Raw, to filter jitter

-	float prevPitch = 0.0f;

-	float prevRoll = 0.0f;

-	float prevX = 0.0f;

-	float prevY = 0.0f;

-	float prevZ = 0.0f;

-

-	float newYaw = 0.0f;							// Local new Raw, to filter jitter

-	float newPitch = 0.0f;

-	float newRoll = 0.0f;

-	float newX = 0.0f;

-	float newY = 0.0f;

-	float newZ = 0.0f;

-

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

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

 	//	QTextStream out(&data);

@@ -259,9 +215,9 @@ void Tracker::run() {
 		   }

 		   else {

 				//

-				// Check the state of the MINUS key (= Start/Stop tracking) and EQUALS key (= Center)

+				// Check the state of the BACK key (= Start/Stop tracking) and EQUALS key (= Center)

 				//

-				if ( (keystate[DIK_BACK] & 0x80) && (!lastMinusKey) ) {

+				if ( (keystate[DIK_BACK] & 0x80) && (!lastBackKey) ) {

 					Tracker::do_tracking = !Tracker::do_tracking;

 

 					//

@@ -272,7 +228,7 @@ void Tracker::run() {
 					}

 					qDebug() << "Tracker::run() says BACK pressed, do_tracking =" << Tracker::do_tracking;

 				}

-				lastMinusKey = (keystate[DIK_BACK] & 0x80);					// Remember

+				lastBackKey = (keystate[DIK_BACK] & 0x80);						// Remember

 

 				if ( (keystate[DIK_EQUALS] & 0x80) && (!lastEqualsKey) ) {

 					Tracker::do_center = true;

@@ -283,7 +239,7 @@ void Tracker::run() {
 		}

 

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

-		if (Tracker::confid && Tracker::newdata) {

+		if (Tracker::confid) {

 

 			//

 			// Most games need an offset to the initial position and NOT the

@@ -291,7 +247,7 @@ void Tracker::run() {
 			// to substract that later...

 			//

 			if(Tracker::set_initial == false) {

-				Tracker::initial_headPosZ = Tracker::getHeadPosZ();

+				Tracker::Z.initial_headPos = Tracker::getHeadPosZ();

 				Tracker::set_initial = true;

 			}

 

@@ -307,76 +263,30 @@ void Tracker::run() {
 			//

 			// Copy the Raw values directly to Free-track server

 			//

-			server_FT->setHeadRotX( Tracker::headRotX );				// rads

-			server_FT->setHeadRotY( Tracker::headRotY );

-			server_FT->setHeadRotZ( Tracker::headRotZ );

-

-			server_FT->setHeadPosX( Tracker::headPosX * 1000.0f);		// From m to mm

-			server_FT->setHeadPosY( Tracker::headPosY * 1000.0f);

-			server_FT->setHeadPosZ( ( Tracker::headPosZ - Tracker::initial_headPosZ ) * 1000.0f);

+			server_FT->setHeadRotX( Tracker::Pitch.headPos );			// rads

+			server_FT->setHeadRotY( Tracker::Yaw.headPos );

+			server_FT->setHeadRotZ( Tracker::Roll.headPos);

 

-			//

-			// Calculate the new values, applying a low-pass filter.

-			// Add the values to their respective QList, for further smoothing

-			//

-			if (Tracker::useFilter) {

-				newPitch = lowPassFilter ( Tracker::headRotX, &prevPitch, 0.020f, Tracker::redPitch );

-			}

-			else {

-				newPitch = Tracker::headRotX;

-			}

-			addRaw2List ( &rawPitchList, intMaxPitchItems, newPitch );

-			//QTextStream out(&data);

-			//out << "Raw:" << Tracker::headRotX << " filtered:" << newPitch << endl;

-			if (Tracker::useFilter) {

-				newYaw = lowPassFilter ( Tracker::headRotY, &prevYaw, 0.020f, Tracker::redYaw );

-			}

-			else {

-				newYaw = Tracker::headRotY;

-			}

-			addRaw2List ( &rawYawList, intMaxYawItems, newYaw );

-			if (Tracker::useFilter) {

-				newRoll = lowPassFilter ( Tracker::headRotZ, &prevRoll, 0.020f, Tracker::redRoll );

-			}

-			else {

-				newRoll = Tracker::headRotZ;

-			}

-			addRaw2List ( &rawRollList, intMaxRollItems, newRoll );

-			if (Tracker::useFilter) {

-				newX = lowPassFilter ( Tracker::headPosX, &prevX, 0.020f, Tracker::redX );

-			}

-			else {

-				newX = Tracker::headPosX;

-			}

-			addRaw2List ( &rawXList, intMaxXItems, newX );

-			if (Tracker::useFilter) {

-				newY = lowPassFilter ( Tracker::headPosY, &prevY, 0.020f, Tracker::redY );

-			}

-			else {

-				newY = Tracker::headPosY;

-			}

-			addRaw2List ( &rawYList, intMaxYItems, newY );

-			if (Tracker::useFilter) {

-				newZ = lowPassFilter ( Tracker::headPosZ, &prevZ, 0.020f, Tracker::redZ );

-			}

-			else {

-				newZ = Tracker::headPosZ;

-			}

-			addRaw2List ( &rawZList, intMaxZItems, newZ );

+			server_FT->setHeadPosX( Tracker::X.headPos * 1000.0f);		// From m to mm

+			server_FT->setHeadPosY( Tracker::Y.headPos * 1000.0f);

+			server_FT->setHeadPosZ( ( Tracker::Z.headPos - Tracker::Z.initial_headPos ) * 1000.0f);

 

-			Tracker::newdata = false;								// Reset flag for ReceiveHeadPose

 		}

 

 		//

 		// If Center is pressed, copy the current values to the offsets.

 		//

 		if (Tracker::do_center) {

-			offset_headRotX = getSmoothFromList( &rawPitchList );

-			offset_headRotY = getSmoothFromList( &rawYawList );

-			offset_headRotZ = getSmoothFromList( &rawRollList );

-			offset_headPosX = getSmoothFromList( &rawXList );

-			offset_headPosY = getSmoothFromList( &rawYList );

-			offset_headPosZ = getSmoothFromList( &rawZList );

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

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

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

+			X.offset_headPos = getSmoothFromList( &X.rawList );

+			Y.offset_headPos = getSmoothFromList( &Y.rawList );

+

+			//

+			// Reset the initial distance to the camera

+			//

+			Z.offset_headPos = getSmoothFromList( &Z.rawList ) - Tracker::Z.initial_headPos;

 			Tracker::do_center = false;

 		}

 

@@ -384,19 +294,19 @@ void Tracker::run() {
 			//

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

 			//

-			server_FT->setVirtRotX ( Tracker::invertPitch * Tracker::sensPitch * (getSmoothFromList( &rawPitchList ) - offset_headRotX) );

-			server_FT->setVirtRotY ( Tracker::invertYaw   * Tracker::sensYaw   * (getSmoothFromList( &rawYawList ) - offset_headRotY) );

-			server_FT->setVirtRotZ ( Tracker::invertRoll  * Tracker::sensRoll  * (getSmoothFromList( &rawRollList ) - offset_headRotZ) );

-			server_FT->setVirtPosX ( ( Tracker::invertX   * Tracker::sensX     * (getSmoothFromList( &rawXList ) - offset_headPosX) ) * 1000.0f);

-			server_FT->setVirtPosY ( ( Tracker::invertY   * Tracker::sensY     * (getSmoothFromList( &rawYList ) - offset_headPosY) ) * 1000.0f );

-			server_FT->setVirtPosZ ( ( Tracker::invertZ   * Tracker::sensZ     * (getSmoothFromList( &rawZList ) - offset_headPosZ) ) * 1000.0f );

-

-			server_FG->setVirtRotX ( getDegreesFromRads ( Tracker::invertPitch * Tracker::sensPitch * (getSmoothFromList( &rawPitchList ) - offset_headRotX) ) );

-			server_FG->setVirtRotY ( getDegreesFromRads ( Tracker::invertYaw   * Tracker::sensYaw   * (getSmoothFromList( &rawYawList )   - offset_headRotY) ) );

-			server_FG->setVirtRotZ ( getDegreesFromRads ( Tracker::invertRoll  * Tracker::sensRoll  * (getSmoothFromList( &rawRollList )  - offset_headRotZ) ) );

-			server_FG->setVirtPosX ( Tracker::invertX * Tracker::sensX * (getSmoothFromList( &rawXList ) - offset_headPosX) );

-			server_FG->setVirtPosY ( Tracker::invertY * Tracker::sensY * (getSmoothFromList( &rawYList ) - offset_headPosY) );

-			server_FG->setVirtPosZ ( Tracker::invertZ * Tracker::sensZ * (getSmoothFromList( &rawZList ) - offset_headPosZ) );

+			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) );

+			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 );

+

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

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

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

+			server_FG->setVirtPosX ( Tracker::X.invert * Tracker::X.sens * (getSmoothFromList( &X.rawList ) - X.offset_headPos) );

+			server_FG->setVirtPosY ( Tracker::Y.invert * Tracker::Y.sens * (getSmoothFromList( &Y.rawList ) - Y.offset_headPos) );

+			server_FG->setVirtPosZ ( Tracker::Z.invert * Tracker::Z.sens * (getSmoothFromList( &Z.rawList ) - Z.offset_headPos - Tracker::Z.initial_headPos) );

 		}

 		else {

 			//

@@ -418,7 +328,7 @@ void Tracker::run() {
 		}

 

 		//for lower cpu load 

-		msleep(20);

+		msleep(25);

 		yieldCurrentThread(); 

 	}

 }

@@ -434,7 +344,14 @@ void Tracker::registerHeadPoseCallback() {
 /** Callback function for head-pose - only static methods could be called **/

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

 {

-	if(head_pose.confidence>0) {

+	SYSTEMTIME now;

+	long newHeadPoseTime;

+	float dT;

+

+	//

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

+	//

+	if( head_pose.confidence > 0 ) {

 		Tracker::confid = true;

 		Tracker::setHeadPosX(head_pose.head_pos.x);

 		Tracker::setHeadPosY(head_pose.head_pos.y);

@@ -443,14 +360,93 @@ void Tracker::receiveHeadPose(void *,smEngineHeadPoseData head_pose, smCameraVid
 		Tracker::setHeadRotX(head_pose.head_rot.x_rads);

 		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;

+

+		// 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';

+		//}

+

+

+		//

+		// Calculate the new values, applying a low-pass filter.

+		// Add the values to their respective QList, for further smoothing

+		//

+		// Pitch

+		if (Tracker::useFilter) {

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

+		}

+		else {

+			Pitch.newPos = Tracker::Pitch.headPos;

+		}

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

+

+		// Yaw

+		if (Tracker::useFilter) {

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

+		}

+		else {

+			Yaw.newPos = Tracker::Yaw.headPos;

+		}

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

+

+		// Roll

+		if (Tracker::useFilter) {

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

+		}

+		else {

+			Roll.newPos = Tracker::Roll.headPos;

+		}

+		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 {

+			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 {

+			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 {

+			Z.newPos = Tracker::Z.headPos;

+		}

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

+

 	} else {

 		Tracker::confid = false;

 	}

-	Tracker::newdata = true;									// Set flag for run()

+

 	// for lower cpu load

-	msleep(40);

+	msleep(30);

 	yieldCurrentThread(); 

-

 }

 

 //

diff --git a/FaceTrackNoIR/tracker.h b/FaceTrackNoIR/tracker.h
index 8614d2e6..9cf89114 100644
--- a/FaceTrackNoIR/tracker.h
+++ b/FaceTrackNoIR/tracker.h
@@ -47,6 +47,22 @@ using namespace sm::faceapi::qt;
 

 class FaceTrackNoIR;				// pre-define parent-class to avoid circular includes

 

+//

+// Structure to hold all variables concerning one of 6 DOF's

+//

+struct THeadPoseDOF {

+	float headPos;					// Current position (from faceTracker, radials or meters)

+	float initial_headPos;			// Position on startup (first valid value)

+	float offset_headPos;			// Offset for centering

+	float sens;						// Sensitivity (multiplication factor)

+	float invert;					// Invert measured value (= 1.0f or -1.0f)

+	float red;						// Reduction factor (used for EWMA-filtering, between 0.0f and 1.0f)

+	QList<float> rawList;			// List of 'n' headPos values (used for moving average)

+	int maxItems;					// Maximum number of elements is rawList

+	float newPos;					// New Position (used locally)

+	float prevPos;					// Previous Position

+};

+

 class Tracker : public QThread {

 	Q_OBJECT

 

@@ -63,79 +79,27 @@ private:
 

 	/** static callback method for the head pose tracking **/

 	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 member varables for saving the head pose **/

-	static float headPosX;

-	static float headPosY;

-	static float headPosZ;							// Distance from camera

-	static float initial_headPosZ;					// Initial distance when headpose is valid

-

-	static float headRotX;

-	static float headRotY;

-	static float headRotZ;

-

-	// Offsets, used to center view while tracking

-	static float offset_headPosX;

-	static float offset_headPosY;

-	static float offset_headPosZ;					// Distance from camera

-

-	static float offset_headRotX;

-	static float offset_headRotY;

-	static float offset_headRotZ;

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

+	static THeadPoseDOF Pitch;						// Head-rotation X-direction (Up/Down)

+	static THeadPoseDOF Yaw;						// Head-rotation Y-direction ()

+	static THeadPoseDOF Roll;						// Head-rotation Z-direction ()

+	static THeadPoseDOF X;							// Head-movement X-direction (Left/Right)

+	static THeadPoseDOF Y;							// Head-movement Y-direction (Up/Down)

+	static THeadPoseDOF Z;							// Head-movement Z-direction (To/From camera)

 

 	// Flags to start/stop/reset tracking

-	static bool confid;

-	static bool newdata;

+	static bool confid;								// Tracker data is OK

 	static bool set_initial;						// initial headpose is set

 	static bool do_tracking;						// Start/stop tracking, using MINUS key on keyboard

 	static bool do_center;							// Center head-position, using EQUALS key on keyboard

 

-	/** static member varables for calculating the virtual head pose **/

-	static float sensYaw;

-	static float sensPitch;

-	static float sensRoll;

-	static float sensX;

-	static float sensY;

-	static float sensZ;

-

-	static float invertYaw;

-	static float invertPitch;

-	static float invertRoll;

-	static float invertX;

-	static float invertY;

-	static float invertZ;

-

 	static bool useFilter;

-

-	/** Factors to remove jitter **/

-	static float redYaw;

-	static float redPitch;

-	static float redRoll;

-	static float redX;

-	static float redY;

-	static float redZ;

-

-	static float rotNeutralZone;						// Neutral Zone for rotations (rad).

+	static float rotNeutralZone;					// Neutral Zone for rotations (rad).

+	static long prevHeadPoseTime;					// Time from previous sample

 	

-	//

-	// The Raw FaceAPI-data may need smoothing.

-	// We implement smoothing, by taking the last 'x' raw samples and making an average of this.

-	// After 'x' samples, the oldest sample is removed and the fresh 'pre-pended' to the QList

-	//

-	QList<float> rawYawList;							// List of last 'x' values from FaceAPI

-	QList<float> rawPitchList;

-	QList<float> rawRollList;

-	QList<float> rawXList;

-	QList<float> rawYList;

-	QList<float> rawZList;

-

-	int intMaxYawItems;									// Max. number of items in QList: more = smoother (yet slower!)

-	int intMaxPitchItems;

-	int intMaxRollItems;

-	int intMaxXItems;

-	int intMaxYItems;

-	int intMaxZItems;

-

 	/** QT objects **/

 	QLineEdit *headXLine;

 	QLineEdit *headYLine;

@@ -167,63 +131,61 @@ public:
 	QSharedPointer<EngineBase> getEngine() { return _engine; };

 //	smEngineHandle getEngineHandle() { return _engine->handle(); };

 

-	static float getHeadPosX() {return headPosX;}

-	static float getHeadPosY() {return headPosY;}

-	static float getHeadPosZ() {return headPosZ;}

+	static float getHeadPosX() {return X.headPos;}

+	static float getHeadPosY() {return Y.headPos;}

+	static float getHeadPosZ() {return Z.headPos;}

 

-	static void setHeadPosX(float x) { headPosX = x; }

-	static void setHeadPosY(float y) { headPosY = y; }

-	static void setHeadPosZ(float z) { headPosZ = z; }

+	static void setHeadPosX(float x) { X.headPos = x; }

+	static void setHeadPosY(float y) { Y.headPos = y; }

+	static void setHeadPosZ(float z) { Z.headPos = z; }

 

-	static float getHeadRotX() {return headRotX;}

-	static float getHeadRotY() {return headRotY;}

-	static float getHeadRotZ() {return headRotZ;}

+	static float getHeadRotX() {return Pitch.headPos;}

+	static float getHeadRotY() {return Yaw.headPos;}

+	static float getHeadRotZ() {return Roll.headPos;}

 

-	static void setHeadRotX(float x) { headRotX = x; }

-	static void setHeadRotY(float y) { headRotY = y; }

-	static void setHeadRotZ(float z) { headRotZ = z; }

+	static void setHeadRotX(float x) { Pitch.headPos = x; }

+	static void setHeadRotY(float y) { Yaw.headPos = y; }

+	static void setHeadRotZ(float z) { Roll.headPos = z; }

 

 	static bool getConfid() { return confid; }

 

-	static void setSensYaw(int x) { sensYaw = x/100.0f; }

-	static void setSensPitch(int x) { sensPitch = x/100.0f; }

-	static void setSensRoll(int x) { sensRoll = x/100.0f; }

-	static void setSensX(int x) { sensX = x/100.0f; }

-	static void setSensY(int x) { sensY = x/100.0f; }

-	static void setSensZ(int x) { sensZ = x/100.0f; }

+	static void setSensPitch(int x) { Pitch.sens = x/100.0f; }

+	static void setSensYaw(int x) { Yaw.sens = x/100.0f; }

+	static void setSensRoll(int x) { Roll.sens = x/100.0f; }

+	static void setSensX(int x) { X.sens = x/100.0f; }

+	static void setSensY(int x) { Y.sens = x/100.0f; }

+	static void setSensZ(int x) { Z.sens = x/100.0f; }

 

-	static void setInvertYaw(bool invert) { invertYaw = invert?-1.0f:+1.0f; }

-	static void setInvertPitch(bool invert) { invertPitch = invert?-1.0f:+1.0f; }

-	static void setInvertRoll(bool invert) { invertRoll = invert?-1.0f:+1.0f; }

-	static void setInvertX(bool invert) { invertX = invert?-1.0f:+1.0f; }

-	static void setInvertY(bool invert) { invertY = invert?-1.0f:+1.0f; }

-	static void setInvertZ(bool invert) { invertZ = invert?-1.0f:+1.0f; }

+	static void setInvertPitch(bool invert) { Pitch.invert = invert?-1.0f:+1.0f; }

+	static void setInvertYaw(bool invert) { Yaw.invert = invert?-1.0f:+1.0f; }

+	static void setInvertRoll(bool invert) { Roll.invert = invert?-1.0f:+1.0f; }

+	static void setInvertX(bool invert) { X.invert = invert?-1.0f:+1.0f; }

+	static void setInvertY(bool invert) { Y.invert = invert?-1.0f:+1.0f; }

+	static void setInvertZ(bool invert) { Z.invert = invert?-1.0f:+1.0f; }

 

 	static void setUseFilter(bool set) { useFilter = set; }

 

-	static void setRedYaw(int x) { redYaw = x/100.0f; }

-	static void setRedPitch(int x) { redPitch = x/100.0f; }

-	static void setRedRoll(int x) { redRoll = x/100.0f; }

-	static void setRedX(int x) { redX = x/100.0f; }

-	static void setRedY(int x) { redY = x/100.0f; }

-	static void setRedZ(int x) { redZ = x/100.0f; }

+	static void setRedYaw(int x) { Yaw.red = x/100.0f; }

+	static void setRedPitch(int x) { Pitch.red = x/100.0f; }

+	static void setRedRoll(int x) { Roll.red = x/100.0f; }

+	static void setRedX(int x) { X.red = x/100.0f; }

+	static void setRedY(int x) { Y.red = x/100.0f; }

+	static void setRedZ(int x) { Z.red = x/100.0f; }

 

 	static void setNeutralZone(int x) { rotNeutralZone = (x * 2.0f * 3.14159)/360.0f; }

 

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

 	float getSmoothFromList ( QList<float> *rawList );

 	float getCorrectedNewRaw ( float NewRaw, float rotNeutral );

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

 	float getDegreesFromRads ( float rads ) { return ((rads * 360.0f)/ (2.0f * 3.14159)); }

 

 	// For now, use one slider for all

 	void setSmoothing(int x) { 

-		intMaxYawItems = x;

-		intMaxPitchItems = x;

-		intMaxRollItems = x;

-		intMaxXItems = x;

-		intMaxYItems = x;

-		intMaxZItems = x;

+		Pitch.maxItems = x;

+		Yaw.maxItems = x;

+		Roll.maxItems = x;

+		X.maxItems = x;

+		Y.maxItems = x;

+		Z.maxItems = x;

 	}

 

 };

diff --git a/bin/FaceTrackNoIR.exe b/bin/FaceTrackNoIR.exe
index 9a5929e6..e6d33e2c 100644
--- a/bin/FaceTrackNoIR.exe
+++ b/bin/FaceTrackNoIR.exe
diff --git a/bin/Settings/ArmA.ini b/bin/Settings/ArmA.ini
index 0f3ea35d..dd72920f 100644
--- a/bin/Settings/ArmA.ini
+++ b/bin/Settings/ArmA.ini
@@ -1,24 +1,24 @@
 [Tracking]

-Smooth=11

+Smooth=25

 NeutralZone=5

 sensYaw=100

 sensPitch=100

 sensRoll=100

-sensX=70

-sensY=70

-sensZ=40

+sensX=100

+sensY=100

+sensZ=100

 invertYaw=false

 invertPitch=false

 invertRoll=true

 invertX=false

 invertY=false

 invertZ=false

-redYaw=70

-redPitch=70

-redRoll=70

-redX=70

-redY=70

-redZ=70

+redYaw=75

+redPitch=75

+redRoll=75

+redX=75

+redY=75

+redZ=75

 useEWMA=true

 

 [GameProtocol]

diff --git a/bin/Settings/FlightGear.ini b/bin/Settings/FlightGear.ini
index d7b64aff..d9cd14f9 100644
--- a/bin/Settings/FlightGear.ini
+++ b/bin/Settings/FlightGear.ini
@@ -7,6 +7,19 @@ sensRoll=300
 sensX=189

 sensY=185

 sensZ=189

+invertYaw=false

+invertPitch=false

+invertRoll=false

+invertX=false

+invertY=false

+invertZ=false

+useEWMA=false

+redYaw=70

+redPitch=70

+redRoll=70

+redX=70

+redY=70

+redZ=70

 

 [GameProtocol]

 Selection=1