From 6c6e6c6aa063796b503f699e710e91b91f93c4d8 Mon Sep 17 00:00:00 2001 From: Wim Vriend Date: Fri, 11 Jun 2010 12:01:15 +0000 Subject: Tryout with lowPassFilter after smoothing... git-svn-id: svn+ssh://svn.code.sf.net/p/facetracknoir/code@8 19e81ba0-9b1a-49c3-bd6c-561e1906d5fb --- FaceTrackNoIR/FTServer.cpp | 2 +- FaceTrackNoIR/FaceTrackNoIR.cpp | 3 + FaceTrackNoIR/FaceTrackNoIR.vcproj | 58 ++++++ .../FaceTrackNoIR.vcproj.VRIEND200810.Wim.user | 2 +- FaceTrackNoIR/GeneratedFiles/qrc_FaceTrackNoIR.cpp | 2 +- FaceTrackNoIR/GeneratedFiles/ui_FaceTrackNoIR.h | 82 +++++---- FaceTrackNoIR/Release/BuildLog.htm | Bin 10824 -> 10992 bytes FaceTrackNoIR/Release/mt.dep | 2 +- FaceTrackNoIR/Release/vc90.idb | Bin 2558976 -> 2624512 bytes FaceTrackNoIR/tracker.cpp | 196 ++++++++++++++------- FaceTrackNoIR/tracker.h | 2 + bin/FaceTrackNoIR.exe | Bin 457216 -> 458752 bytes bin/Settings/ArmA.ini | 14 +- bin/Settings/FlightGear.ini | 4 +- 14 files changed, 258 insertions(+), 109 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 @@ -209,6 +209,10 @@ RelativePath=".\main.cpp" > + + @@ -331,6 +335,36 @@ RelativePath=".\FTTypes.h" > + + + + + + + + + + @@ -585,6 +619,18 @@ /> + + + + + @@ -651,6 +697,18 @@ /> + + + + + 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 @@ 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 Binary files a/FaceTrackNoIR/Release/BuildLog.htm and b/FaceTrackNoIR/Release/BuildLog.htm differ 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 Binary files a/FaceTrackNoIR/Release/vc90.idb and b/FaceTrackNoIR/Release/vc90.idb differ 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 *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 **/ diff --git a/bin/FaceTrackNoIR.exe b/bin/FaceTrackNoIR.exe index b9f293af..463f086e 100644 Binary files a/bin/FaceTrackNoIR.exe and b/bin/FaceTrackNoIR.exe differ diff --git a/bin/Settings/ArmA.ini b/bin/Settings/ArmA.ini index b3c1bf42..cbcf7643 100644 --- a/bin/Settings/ArmA.ini +++ b/bin/Settings/ArmA.ini @@ -1,12 +1,12 @@ [Tracking] -Smooth=12 +Smooth=15 NeutralZone=5 -sensYaw=110 -sensPitch=110 -sensRoll=110 -sensX=100 -sensY=100 -sensZ=100 +sensYaw=120 +sensPitch=50 +sensRoll=120 +sensX=0 +sensY=0 +sensZ=0 invertYaw=false invertPitch=false invertRoll=true diff --git a/bin/Settings/FlightGear.ini b/bin/Settings/FlightGear.ini index d9cd14f9..fe506c45 100644 --- a/bin/Settings/FlightGear.ini +++ b/bin/Settings/FlightGear.ini @@ -1,7 +1,7 @@ [Tracking] -Smooth=10 +Smooth=5 NeutralZone=5 -sensYaw=300 +sensYaw=400 sensPitch=300 sensRoll=300 sensX=189 -- cgit v1.2.3