From f42d2321973dd6949700cb4d272a13da3c1fc2a4 Mon Sep 17 00:00:00 2001 From: Wim Vriend Date: Wed, 2 Jun 2010 18:49:45 +0000 Subject: git-svn-id: svn+ssh://svn.code.sf.net/p/facetracknoir/code@5 19e81ba0-9b1a-49c3-bd6c-561e1906d5fb --- FaceTrackNoIR/tracker.cpp | 146 ++++++++++++++++++++++++++++++---------------- 1 file changed, 95 insertions(+), 51 deletions(-) (limited to 'FaceTrackNoIR/tracker.cpp') diff --git a/FaceTrackNoIR/tracker.cpp b/FaceTrackNoIR/tracker.cpp index c22961bd..377f605d 100644 --- a/FaceTrackNoIR/tracker.cpp +++ b/FaceTrackNoIR/tracker.cpp @@ -55,6 +55,7 @@ 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; @@ -73,12 +74,14 @@ float Tracker::invertX = 1.0f; float Tracker::invertY = 1.0f; float Tracker::invertZ = 1.0f; -float Tracker::thresYaw = 1.0f; -float Tracker::thresPitch = 1.0f; -float Tracker::thresRoll = 1.0f; -float Tracker::thresX = 1.0f; -float Tracker::thresY = 1.0f; -float Tracker::thresZ = 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) @@ -200,6 +203,12 @@ void Tracker::run() { float newY = 0.0f; float newZ = 0.0f; + //QFile data("output.txt"); + //if (data.open(QFile::WriteOnly | QFile::Truncate)) { + // QTextStream out(&data); + // out << "Polling results"; + //} + // // Setup the DirectInput for keyboard strokes // @@ -274,7 +283,7 @@ void Tracker::run() { } //if the confidence is good enough the headpose will be updated **/ - if (Tracker::confid) { + if (Tracker::confid && Tracker::newdata) { // // Most games need an offset to the initial position and NOT the @@ -295,48 +304,67 @@ void Tracker::run() { headRotZLine->setText(QString("%1").arg(Tracker::getHeadRotZ()*100, 0, 'f', 1)); //// listener.setTrackedPosition(QPoint(Tracker::getHeadPosX()-50, Tracker::getHeadPosY()-37.5)); - server_FT->setHeadRotX( Tracker::headRotX ); // rads (?) + // + // 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->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); // - // Add the raw values to the QList, so they can be smoothed. - // The raw value that enters the QList is first (evt.) inverted and corrected for Neutral Zone. + // Calculate the new values, applying a low-pass filter. + // Add the values to their respective QList, for further smoothing // - newPitch = Tracker::headRotX; - if ( (fabs (newPitch - prevPitch) ) < Tracker::thresPitch ) { // delta smaller than threshold? - newPitch = prevPitch; + if (Tracker::useFilter) { + newPitch = lowPassFilter ( Tracker::headRotX, &prevPitch, 0.020f, Tracker::redPitch ); } else { - prevPitch = newPitch; + newPitch = Tracker::headRotX; } - addRaw2List ( &rawPitchList, intMaxPitchItems, getCorrectedNewRaw ( Tracker::invertPitch * newPitch , Tracker::rotNeutralZone ) ); - - newYaw = Tracker::headRotY; - if ( (fabs (newYaw - prevYaw) ) < Tracker::thresYaw ) { // delta smaller than threshold? - newYaw = prevYaw; + 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 { - prevYaw = newYaw; + newYaw = Tracker::headRotY; } - addRaw2List ( &rawYawList, intMaxYawItems, getCorrectedNewRaw ( Tracker::invertYaw * newYaw, Tracker::rotNeutralZone ) ); - - newRoll = Tracker::headRotZ; - if ( (fabs (newRoll - prevRoll) ) < Tracker::thresRoll ) { // delta smaller than threshold? - newRoll = prevRoll; + 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 { - prevRoll = newRoll; + newZ = Tracker::headPosZ; } - addRaw2List ( &rawRollList, intMaxRollItems, getCorrectedNewRaw ( Tracker::invertRoll * newRoll , Tracker::rotNeutralZone ) ); + addRaw2List ( &rawZList, intMaxZItems, newZ ); - addRaw2List ( &rawXList, intMaxXItems, Tracker::invertX * Tracker::headPosX * 1000.0f ); - addRaw2List ( &rawYList, intMaxYItems, Tracker::invertY * Tracker::headPosY * 1000.0f ); - addRaw2List ( &rawZList, intMaxZItems, ( Tracker::invertZ * Tracker::headPosZ - Tracker::initial_headPosZ ) * 1000.0f ); + Tracker::newdata = false; // Reset flag for ReceiveHeadPose } // @@ -356,19 +384,19 @@ void Tracker::run() { // // Also send the Virtual Pose to FT-server and FG-server // - server_FT->setVirtRotX ( Tracker::sensPitch * getSmoothFromList( &rawPitchList ) - offset_headRotX ); - server_FT->setVirtRotY ( Tracker::sensYaw * getSmoothFromList( &rawYawList ) - offset_headRotY ); - server_FT->setVirtRotZ ( Tracker::sensRoll * getSmoothFromList( &rawRollList ) - offset_headRotZ ); - server_FT->setVirtPosX ( Tracker::sensX * getSmoothFromList( &rawXList ) - offset_headPosX ); - server_FT->setVirtPosY ( Tracker::sensY * getSmoothFromList( &rawYList ) - offset_headPosY ); - server_FT->setVirtPosZ ( Tracker::sensZ * getSmoothFromList( &rawZList ) - offset_headPosZ ); - - server_FG->setVirtRotX ( getDegreesFromRads ( Tracker::sensPitch * getSmoothFromList( &rawPitchList ) - offset_headRotX ) ); - server_FG->setVirtRotY ( getDegreesFromRads ( Tracker::sensYaw * getSmoothFromList( &rawYawList ) - offset_headRotY ) ); - server_FG->setVirtRotZ ( getDegreesFromRads ( Tracker::sensRoll * getSmoothFromList( &rawRollList ) - offset_headRotZ ) ); - server_FG->setVirtPosX ( ( Tracker::sensX * getSmoothFromList( &rawXList ) - offset_headPosX ) / 1000.0f ); - server_FG->setVirtPosY ( ( Tracker::sensY * getSmoothFromList( &rawYList ) - offset_headPosY ) / 1000.0f ); - server_FG->setVirtPosZ ( ( Tracker::sensZ * getSmoothFromList( &rawZList ) - offset_headPosZ ) / 1000.0f ); + 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) ); } else { // @@ -381,16 +409,16 @@ void Tracker::run() { server_FT->setVirtPosY ( 0.0f ); server_FT->setVirtPosZ ( 0.0f ); - server_FG->setVirtRotX( 0.0f ); - server_FG->setVirtRotY( 0.0f ); - server_FG->setVirtRotZ( 0.0f ); + server_FG->setVirtRotX ( 0.0f ); + server_FG->setVirtRotY ( 0.0f ); + server_FG->setVirtRotZ ( 0.0f ); server_FG->setVirtPosX ( 0.0f ); server_FG->setVirtPosY ( 0.0f ); server_FG->setVirtPosZ ( 0.0f ); } //for lower cpu load - msleep(50); + msleep(20); yieldCurrentThread(); } } @@ -415,13 +443,12 @@ 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); - } else { Tracker::confid = false; - } + Tracker::newdata = true; // Set flag for run() // for lower cpu load - msleep(50); + msleep(40); yieldCurrentThread(); } @@ -520,3 +547,20 @@ float Tracker::getCorrectedNewRaw ( float NewRaw, float rotNeutral ) { } } + +// +// Implementation of an Exponentially Weighed 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) +// +float Tracker::lowPassFilter ( float newvalue, float *oldvalue, float dt, float coeff) { +float c = 0.0f; +float fil = 0.0f; + + c = dt / (coeff + dt); + fil = (newvalue * c) + (*oldvalue * (1 - c)); + *oldvalue = fil; + + return fil; +} -- cgit v1.2.3