diff options
27 files changed, 565 insertions, 116 deletions
diff --git a/CMakeLists.txt b/CMakeLists.txt index 8bf2b09e..f2b3805c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -133,6 +133,14 @@ if(NOT SDK_FACEAPI_ONLY) QT4_WRAP_UI(opentrack-filter-accela-uih ${opentrack-filter-accela-ui}) QT4_ADD_RESOURCES(opentrack-filter-accela-rcc ${opentrack-filter-accela-rc}) + file(GLOB opentrack-filter-kalman-c "ftnoir_filter_kalman/*.cpp") + file(GLOB opentrack-filter-kalman-h "ftnoir_filter_kalman/*.h") + QT4_WRAP_CPP(opentrack-filter-kalman-moc ${opentrack-filter-kalman-h}) + file(GLOB opentrack-filter-kalman-ui "ftnoir_filter_kalman/*.ui") + file(GLOB opentrack-filter-kalman-rc "ftnoir_filter_kalman/*.qrc") + QT4_WRAP_UI(opentrack-filter-kalman-uih ${opentrack-filter-kalman-ui}) + QT4_ADD_RESOURCES(opentrack-filter-kalman-rcc ${opentrack-filter-kalman-rc}) + file(GLOB opentrack-filter-ewma-c "ftnoir_filter_ewma2/*.cpp") file(GLOB opentrack-filter-ewma-h "ftnoir_filter_ewma2/*.h") QT4_WRAP_CPP(opentrack-filter-ewma-moc ${opentrack-filter-ewma-h}) @@ -327,6 +335,11 @@ endif() add_library(opentrack-filter-accela SHARED ${opentrack-filter-accela-c} ${opentrack-filter-accela-moc} ${opentrack-filter-accela-uih} ${opentrack-filter-accela-rcc}) target_link_libraries(opentrack-filter-accela ${MY_QT_LIBS} opentrack-spline-widget) + if(OpenCV_FOUND) + add_library(opentrack-filter-kalman SHARED ${opentrack-filter-kalman-c} ${opentrack-filter-kalman-moc} ${opentrack-filter-kalman-uih} ${opentrack-filter-kalman-rcc}) + target_link_libraries(opentrack-filter-kalman ${MY_QT_LIBS} ${OpenCV_LIBS}) + endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_GNUCC) SET_TARGET_PROPERTIES(opentrack-filter-accela PROPERTIES LINK_FLAGS "-Wl,--version-script=${CMAKE_SOURCE_DIR}/facetracknoir/posix-version-script.txt") @@ -555,7 +568,7 @@ if(NOT SDK_FACEAPI_ONLY) endif() if(OpenCV_FOUND) - install(TARGETS opentrack-tracker-pt RUNTIME DESTINATION . LIBRARY DESTINATION .) + install(TARGETS opentrack-tracker-pt opentrack-filter-kalman RUNTIME DESTINATION . LIBRARY DESTINATION .) endif() install(TARGETS diff --git a/facetracknoir/facetracknoir.cpp b/facetracknoir/facetracknoir.cpp index 94a5257d..fafef908 100644 --- a/facetracknoir/facetracknoir.cpp +++ b/facetracknoir/facetracknoir.cpp @@ -681,9 +681,9 @@ void FaceTrackNoIR::startTracker( ) { // Setup the Tracker and send the settings.
// This is necessary, because the events are only triggered 'on change'
//
- tracker->setInvertAxis(RX, ui.chkInvertYaw->isChecked() );
+ tracker->setInvertAxis(Yaw, ui.chkInvertYaw->isChecked() );
tracker->setInvertAxis(TY, ui.chkInvertPitch->isChecked() );
- tracker->setInvertAxis(RZ, ui.chkInvertRoll->isChecked() );
+ tracker->setInvertAxis(Roll, ui.chkInvertRoll->isChecked() );
tracker->setInvertAxis(TX, ui.chkInvertX->isChecked() );
tracker->setInvertAxis(TY, ui.chkInvertY->isChecked() );
tracker->setInvertAxis(TZ, ui.chkInvertZ->isChecked() );
@@ -849,9 +849,9 @@ void FaceTrackNoIR::showHeadPose() { ui.lcdNumY->display(QString("%1").arg(newdata[TY], 0, 'f', 1));
ui.lcdNumZ->display(QString("%1").arg(newdata[TZ], 0, 'f', 1));
- ui.lcdNumRotX->display(QString("%1").arg(newdata[RX], 0, 'f', 1));
- ui.lcdNumRotY->display(QString("%1").arg(newdata[RY], 0, 'f', 1));
- ui.lcdNumRotZ->display(QString("%1").arg(newdata[RZ], 0, 'f', 1));
+ ui.lcdNumRotX->display(QString("%1").arg(newdata[Yaw], 0, 'f', 1));
+ ui.lcdNumRotY->display(QString("%1").arg(newdata[Pitch], 0, 'f', 1));
+ ui.lcdNumRotZ->display(QString("%1").arg(newdata[Roll], 0, 'f', 1));
ui.txtTracking->setVisible(tracker->getTrackingActive());
@@ -859,15 +859,15 @@ void FaceTrackNoIR::showHeadPose() { // Get the output-pose and also display it.
//
tracker->getOutputHeadPose(newdata);
- ui.pose_display->rotateBy(newdata[RX], newdata[RZ], newdata[RY]);
+ ui.pose_display->rotateBy(newdata[Yaw], newdata[Roll], newdata[Pitch]);
ui.lcdNumOutputPosX->display(QString("%1").arg(newdata[TX], 0, 'f', 1));
ui.lcdNumOutputPosY->display(QString("%1").arg(newdata[TY], 0, 'f', 1));
ui.lcdNumOutputPosZ->display(QString("%1").arg(newdata[TZ], 0, 'f', 1));
- ui.lcdNumOutputRotX->display(QString("%1").arg(newdata[RX], 0, 'f', 1));
- ui.lcdNumOutputRotY->display(QString("%1").arg(newdata[RY], 0, 'f', 1));
- ui.lcdNumOutputRotZ->display(QString("%1").arg(newdata[RZ], 0, 'f', 1));
+ ui.lcdNumOutputRotX->display(QString("%1").arg(newdata[Yaw], 0, 'f', 1));
+ ui.lcdNumOutputRotY->display(QString("%1").arg(newdata[Pitch], 0, 'f', 1));
+ ui.lcdNumOutputRotZ->display(QString("%1").arg(newdata[Roll], 0, 'f', 1));
//
// Update the curves in the curve-configurator. This shows the ball with the red lines.
diff --git a/facetracknoir/facetracknoir.h b/facetracknoir/facetracknoir.h index ece1d9b9..fd9c06a7 100644 --- a/facetracknoir/facetracknoir.h +++ b/facetracknoir/facetracknoir.h @@ -186,13 +186,13 @@ private: void setInvertAxis( Axis axis, int invert );
void setInvertYaw(int invert) {
- setInvertAxis(RX, invert);
+ setInvertAxis(Yaw, invert);
}
void setInvertPitch(int invert) {
- setInvertAxis(RY, invert);
+ setInvertAxis(Pitch, invert);
}
void setInvertRoll(int invert) {
- setInvertAxis(RZ, invert);
+ setInvertAxis(Roll, invert);
}
void setInvertX(int invert) {
setInvertAxis(TX, invert);
diff --git a/facetracknoir/tracker.cpp b/facetracknoir/tracker.cpp index 380d1748..25e09126 100644 --- a/facetracknoir/tracker.cpp +++ b/facetracknoir/tracker.cpp @@ -161,6 +161,9 @@ void Tracker::run() { if (Libraries->pSecondTracker)
Libraries->pSecondTracker->NotifyCenter();
+
+ if (Libraries->pFilter)
+ Libraries->pFilter->Initialize();
}
if (getTrackingActive()) {
diff --git a/facetracknoir/tracker.h b/facetracknoir/tracker.h index 89fb2650..5500993c 100644 --- a/facetracknoir/tracker.h +++ b/facetracknoir/tracker.h @@ -142,9 +142,9 @@ public: axes[TX] = new THeadPoseDOF("tx","tx_alt", 100, 100, 100, 100);
axes[TY] = new THeadPoseDOF("ty","ty_alt", 100, 100, 100, 100);
axes[TZ] = new THeadPoseDOF("tz","tz_alt", 100, 100, 100, 100);
- axes[RX] = new THeadPoseDOF("rx", "rx_alt", 180, 180, 180, 180);
- axes[RY] = new THeadPoseDOF("ry", "ry_alt", 90, 90, 90, 90);
- axes[RZ] = new THeadPoseDOF("rz", "rz_alt", 180, 180, 180, 180);
+ axes[Yaw] = new THeadPoseDOF("rx", "rx_alt", 180, 180, 180, 180);
+ axes[Pitch] = new THeadPoseDOF("ry", "ry_alt", 90, 90, 90, 90);
+ axes[Roll] = new THeadPoseDOF("rz", "rz_alt", 180, 180, 180, 180);
}
};
diff --git a/facetracknoir/tracker_types.cpp b/facetracknoir/tracker_types.cpp index eedd94f9..11adc985 100644 --- a/facetracknoir/tracker_types.cpp +++ b/facetracknoir/tracker_types.cpp @@ -7,15 +7,15 @@ T6DOF operator-(const T6DOF& A, const T6DOF& B)
{
- Rotation R_A(A.axes[RX]*D2R, A.axes[RY]*D2R, A.axes[RZ]*D2R);
- Rotation R_B(B.axes[RX]*D2R, B.axes[RY]*D2R, B.axes[RZ]*D2R);
+ Rotation R_A(A.axes[Yaw]*D2R, A.axes[Pitch]*D2R, A.axes[Roll]*D2R);
+ Rotation R_B(B.axes[Yaw]*D2R, B.axes[Pitch]*D2R, B.axes[Roll]*D2R);
Rotation R_C = R_A * R_B.inv();
T6DOF C;
- R_C.toEuler(C.axes[RX], C.axes[RY], C.axes[RZ]);
- C.axes[RX] *= R2D;
- C.axes[RY] *= R2D;
- C.axes[RZ] *= R2D;
+ R_C.toEuler(C.axes[Yaw], C.axes[Pitch], C.axes[Roll]);
+ C.axes[Yaw] *= R2D;
+ C.axes[Pitch] *= R2D;
+ C.axes[Roll] *= R2D;
C.axes[TX] = A.axes[TX] - B.axes[TX];
C.axes[TY] = A.axes[TY] - B.axes[TY];
@@ -26,15 +26,15 @@ T6DOF operator-(const T6DOF& A, const T6DOF& B) T6DOF operator+(const T6DOF& A, const T6DOF& B)
{
- Rotation R_A(A.axes[RX]*D2R, A.axes[RY]*D2R, A.axes[RZ]*D2R);
- Rotation R_B(B.axes[RX]*D2R, B.axes[RY]*D2R, B.axes[RZ]*D2R);
+ Rotation R_A(A.axes[Yaw]*D2R, A.axes[Pitch]*D2R, A.axes[Roll]*D2R);
+ Rotation R_B(B.axes[Yaw]*D2R, B.axes[Pitch]*D2R, B.axes[Roll]*D2R);
Rotation R_C = R_A * R_B;
T6DOF C;
- R_C.toEuler(C.axes[RX], C.axes[RY], C.axes[RZ]);
- C.axes[RX] *= R2D;
- C.axes[RY] *= R2D;
- C.axes[RZ] *= R2D;
+ R_C.toEuler(C.axes[Yaw], C.axes[Pitch], C.axes[Roll]);
+ C.axes[Yaw] *= R2D;
+ C.axes[Pitch] *= R2D;
+ C.axes[Roll] *= R2D;
C.axes[TX] = A.axes[TX] + B.axes[TX];
C.axes[TY] = A.axes[TY] + B.axes[TY];
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.h b/ftnoir_filter_accela/ftnoir_filter_accela.h index 7939b783..fa1e9bcd 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.h +++ b/ftnoir_filter_accela/ftnoir_filter_accela.h @@ -48,6 +48,7 @@ public: FTNoIR_Filter();
~FTNoIR_Filter();
void FilterHeadPoseData(double *current_camera_position, double *target_camera_position, double *new_camera_position, double *last_post_filter_values);
+ void Initialize() {}
private:
void loadSettings(); // Load the settings from the INI-file
@@ -93,8 +94,8 @@ private: private slots:
void doOK();
void doCancel();
- void settingChanged(bool) { settingsDirty = true; };
- void settingChanged(int) { settingsDirty = true; };
+ void settingChanged(bool) { settingsDirty = true; }
+ void settingChanged(int) { settingsDirty = true; }
};
//*******************************************************************************************************
diff --git a/ftnoir_filter_base/ftnoir_filter_base.h b/ftnoir_filter_base/ftnoir_filter_base.h index 0dcf640a..b38fc226 100644 --- a/ftnoir_filter_base/ftnoir_filter_base.h +++ b/ftnoir_filter_base/ftnoir_filter_base.h @@ -27,6 +27,7 @@ struct IFilter {
virtual ~IFilter() {}
virtual void FilterHeadPoseData(double *current_camera_position, double *target_camera_position, double *new_camera_position, double *last_post_filter) = 0;
+ virtual void Initialize() = 0;
};
// Factory function that creates instances of the Filter object.
diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h index 7fc9c77e..910fc7c1 100644 --- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h +++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h @@ -41,6 +41,7 @@ class FTNoIR_Filter : public IFilter public:
FTNoIR_Filter();
~FTNoIR_Filter();
+ void Initialize() {}
void FilterHeadPoseData(double *current_camera_position, double *target_camera_position, double *new_camera_position, double *last_post_filter);
diff --git a/ftnoir_filter_kalman/ftnoir_filter_kalman.h b/ftnoir_filter_kalman/ftnoir_filter_kalman.h new file mode 100644 index 00000000..59169612 --- /dev/null +++ b/ftnoir_filter_kalman/ftnoir_filter_kalman.h @@ -0,0 +1,87 @@ +#pragma once +/* Copyright (c) 2013 Stanisław Halik <sthalik@misaki.pl> + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ +#ifndef INCLUDED_FTN_FILTER_H +#define INCLUDED_FTN_FILTER_H + +#undef FTNOIR_TRACKER_BASE_LIB +#define FTNOIR_TRACKER_BASE_EXPORT Q_DECL_IMPORT + +#include "ftnoir_filter_base/ftnoir_filter_base.h" +#include "ui_ftnoir_kalman_filtercontrols.h" +#include "facetracknoir/global-settings.h" +#include <opencv2/opencv.hpp> +#include <vector> +#include <QString> +#include <QIcon> +#include <QWidget> +#include <QObject> + +class FTNOIR_FILTER_BASE_EXPORT FTNoIR_Filter : public IFilter +{ +public: + FTNoIR_Filter(); + virtual ~FTNoIR_Filter() { + } + void Initialize(); + void FilterHeadPoseData(double *current_camera_position, + double *target_camera_position, + double *new_camera_position, + double *last_post_filter_values); + cv::KalmanFilter kalman; + double prev_position[6]; +}; + +void kalman_load_settings(FTNoIR_Filter& self); +void kalman_save_settings(FTNoIR_Filter& self); + +class FTNOIR_FILTER_BASE_EXPORT FTNoIR_FilterDll : public Metadata +{ +public: + void getFullName(QString *strToBeFilled) { *strToBeFilled = QString("Kalman filter"); } + void getShortName(QString *strToBeFilled) { *strToBeFilled = QString("Kalman filter"); } + void getDescription(QString *strToBeFilled) { *strToBeFilled = QString("Kalman filter"); } + void getIcon(QIcon *icon){ *icon = QIcon(":/images/filter-16.png"); } +}; + +class FTNOIR_FILTER_BASE_EXPORT FilterControls: public QWidget, Ui::KalmanUICFilterControls, public IFilterDialog +{ + Q_OBJECT +public: + explicit FilterControls() : settingsDirty(false) { + ui.setupUi(this); + QSettings settings("Abbequerque Inc.", "FaceTrackNoIR"); // Registry settings (in HK_USER) + + QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString(); + QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file) + + iniFile.beginGroup("ftnoir-filter-kalman"); + iniFile.endGroup(); + connect(ui.btnOk, SIGNAL(clicked()), this, SLOT(doOK())); + connect(ui.btnCancel, SIGNAL(clicked()), this, SLOT(doCancel())); + show(); + } + virtual ~FilterControls() {} + void showEvent ( QShowEvent * event ) { + show(); + } + + void Initialize(QWidget *parent, IFilter* ptr) { + } + + bool settingsDirty; + Ui::KalmanUICFilterControls ui; + +public slots: + void doOK(); + void doCancel(); + void settingsChanged(double unused) { + settingsDirty = true; + } +}; + +#endif diff --git a/ftnoir_filter_kalman/ftnoir_kalman_filtercontrols.ui b/ftnoir_filter_kalman/ftnoir_kalman_filtercontrols.ui new file mode 100644 index 00000000..7b71712a --- /dev/null +++ b/ftnoir_filter_kalman/ftnoir_kalman_filtercontrols.ui @@ -0,0 +1,211 @@ +<?xml version="1.0" encoding="UTF-8"?> +<ui version="4.0"> + <class>KalmanUICFilterControls</class> + <widget class="QWidget" name="KalmanUICFilterControls"> + <property name="windowModality"> + <enum>Qt::ApplicationModal</enum> + </property> + <property name="geometry"> + <rect> + <x>0</x> + <y>0</y> + <width>334</width> + <height>100</height> + </rect> + </property> + <property name="sizePolicy"> + <sizepolicy hsizetype="Fixed" vsizetype="Fixed"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> + <property name="windowTitle"> + <string>Filter settings</string> + </property> + <property name="windowIcon"> + <iconset> + <normaloff> + images/facetracknoir.png</normaloff> + images/facetracknoir.png</iconset> + </property> + <property name="layoutDirection"> + <enum>Qt::LeftToRight</enum> + </property> + <property name="autoFillBackground"> + <bool>false</bool> + </property> + <property name="styleSheet"> + <string notr="true"/> + </property> + <widget class="QPushButton" name="btnOk"> + <property name="geometry"> + <rect> + <x>173</x> + <y>70</y> + <width>73</width> + <height>25</height> + </rect> + </property> + <property name="text"> + <string>OK</string> + </property> + </widget> + <widget class="QPushButton" name="btnCancel"> + <property name="geometry"> + <rect> + <x>250</x> + <y>70</y> + <width>73</width> + <height>25</height> + </rect> + </property> + <property name="text"> + <string>Cancel</string> + </property> + </widget> + <widget class="QLabel" name="label_2"> + <property name="geometry"> + <rect> + <x>9</x> + <y>30</y> + <width>169</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>process-noise-covariance</string> + </property> + </widget> + <widget class="QDoubleSpinBox" name="post"> + <property name="geometry"> + <rect> + <x>180</x> + <y>26</y> + <width>150</width> + <height>22</height> + </rect> + </property> + <property name="alignment"> + <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set> + </property> + <property name="decimals"> + <number>14</number> + </property> + <property name="maximum"> + <double>1.000000000000000</double> + </property> + <property name="singleStep"> + <double>0.000001000000000</double> + </property> + <property name="value"> + <double>0.500000000000000</double> + </property> + </widget> + <widget class="QDoubleSpinBox" name="pnoise"> + <property name="geometry"> + <rect> + <x>180</x> + <y>6</y> + <width>150</width> + <height>22</height> + </rect> + </property> + <property name="alignment"> + <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set> + </property> + <property name="decimals"> + <number>14</number> + </property> + <property name="maximum"> + <double>1.000000000000000</double> + </property> + <property name="singleStep"> + <double>0.000001000000000</double> + </property> + <property name="value"> + <double>0.500000000000000</double> + </property> + </widget> + <widget class="QLabel" name="label"> + <property name="geometry"> + <rect> + <x>9</x> + <y>10</y> + <width>165</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>post-error-matrix</string> + </property> + <property name="alignment"> + <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set> + </property> + <property name="margin"> + <number>0</number> + </property> + </widget> + <widget class="QLabel" name="label_3"> + <property name="geometry"> + <rect> + <x>9</x> + <y>55</y> + <width>109</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>accel-coefficient</string> + </property> + </widget> + <widget class="QDoubleSpinBox" name="accl"> + <property name="geometry"> + <rect> + <x>181</x> + <y>47</y> + <width>150</width> + <height>22</height> + </rect> + </property> + <property name="alignment"> + <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set> + </property> + <property name="decimals"> + <number>6</number> + </property> + <property name="maximum"> + <double>1000000000.000000000000000</double> + </property> + <property name="singleStep"> + <double>0.000001000000000</double> + </property> + <property name="value"> + <double>1.000000000000000</double> + </property> + </widget> + </widget> + <resources/> + <connections/> + <designerdata> + <property name="gridDeltaX"> + <number>10</number> + </property> + <property name="gridDeltaY"> + <number>10</number> + </property> + <property name="gridSnapX"> + <bool>false</bool> + </property> + <property name="gridSnapY"> + <bool>false</bool> + </property> + <property name="gridVisible"> + <bool>true</bool> + </property> + </designerdata> + <slots> + <slot>startEngineClicked()</slot> + <slot>stopEngineClicked()</slot> + <slot>cameraSettingsClicked()</slot> + </slots> +</ui> diff --git a/ftnoir_filter_kalman/images/filter-16-ac.png b/ftnoir_filter_kalman/images/filter-16-ac.png Binary files differnew file mode 100644 index 00000000..d263db2d --- /dev/null +++ b/ftnoir_filter_kalman/images/filter-16-ac.png diff --git a/ftnoir_filter_kalman/kalman-filter.qrc b/ftnoir_filter_kalman/kalman-filter.qrc new file mode 100644 index 00000000..9a7d75fa --- /dev/null +++ b/ftnoir_filter_kalman/kalman-filter.qrc @@ -0,0 +1,5 @@ +<RCC> + <qresource prefix="/"> + <file>images/filter-16-ac.png</file> + </qresource> +</RCC> diff --git a/ftnoir_filter_kalman/kalman.cpp b/ftnoir_filter_kalman/kalman.cpp new file mode 100644 index 00000000..51af35e1 --- /dev/null +++ b/ftnoir_filter_kalman/kalman.cpp @@ -0,0 +1,156 @@ +/* Copyright (c) 2013 Stanisław Halik <sthalik@misaki.pl> + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ +#include "ftnoir_filter_kalman.h" +#include "facetracknoir/global-settings.h" +#include <QDebug> +#include <math.h> + +void kalman_load_settings(FTNoIR_Filter& self) { + QSettings settings("Abbequerque Inc.", "FaceTrackNoIR"); // Registry settings (in HK_USER) + + QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString(); + QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file) + + iniFile.beginGroup("ftnoir-filter-kalman"); + iniFile.endGroup(); +} + +void kalman_save_settings(FilterControls& self) { + QSettings settings("Abbequerque Inc.", "FaceTrackNoIR"); // Registry settings (in HK_USER) + + QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString(); + QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file) + + iniFile.beginGroup("ftnoir-filter-kalman"); + iniFile.endGroup(); +} + +FTNoIR_Filter::FTNoIR_Filter() { + kalman_load_settings(*this); + Initialize(); +} + +// the following was written by Donovan Baarda <abo@minkirri.apana.org.au> +// https://sourceforge.net/p/facetracknoir/discussion/1150909/thread/418615e1/?limit=25#af75/084b +void FTNoIR_Filter::Initialize() { + const double accel_variance = 1e-4; + const double noise_variance = 1e1; + kalman.init(12, 6, 0, CV_64F); + kalman.transitionMatrix = (cv::Mat_<double>(12, 12) << + 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1); + double a = 0.25 * accel_variance; + double b = 0.5 * accel_variance; + double c = 1.0 * accel_variance; + kalman.processNoiseCov = (cv::Mat_<double>(12, 12) << + a, 0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, + 0, a, 0, 0, 0, 0, 0, b, 0, 0, 0, 0, + 0, 0, a, 0, 0, 0, 0, 0, b, 0, 0, 0, + 0, 0, 0, a, 0, 0, 0, 0, 0, b, 0, 0, + 0, 0, 0, 0, a, 0, 0, 0, 0, 0, b, 0, + 0, 0, 0, 0, 0, a, 0, 0, 0, 0, 0, b, + b, 0, 0, 0, 0, 0, c, 0, 0, 0, 0, 0, + 0, b, 0, 0, 0, 0, 0, c, 0, 0, 0, 0, + 0, 0, b, 0, 0, 0, 0, 0, c, 0, 0, 0, + 0, 0, 0, b, 0, 0, 0, 0, 0, c, 0, 0, + 0, 0, 0, 0, b, 0, 0, 0, 0, 0, c, 0, + 0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, c); + cv::setIdentity(kalman.measurementMatrix); + cv::setIdentity(kalman.measurementNoiseCov, cv::Scalar::all(noise_variance)); + cv::setIdentity(kalman.errorCovPost, cv::Scalar::all(accel_variance * 1e4)); + for (int i = 0; i < 6; i++) + prev_position[i] = 0; +} + +void FTNoIR_Filter::FilterHeadPoseData(double *current_camera_position, + double *target_camera_position, + double *new_camera_position, + double *last_post_filter_values) +{ + bool new_target = false; + + for (int i = 0; i < 6; i++) + if (prev_position[i] != target_camera_position[i]) + { + new_target = true; + break; + } + + cv::Mat output = kalman.predict(); + + if (new_target) { + cv::Mat measurement(6, 1, CV_64F); + for (int i = 0; i < 3; i++) { + measurement.at<double>(i) = target_camera_position[i+3]; + measurement.at<double>(i+3) = target_camera_position[i]; + } + kalman.correct(measurement); + } + + for (int i = 0; i < 3; i++) { + new_camera_position[i] = output.at<double>(i+3); + new_camera_position[i+3] = output.at<double>(i); + prev_position[i] = target_camera_position[i]; + prev_position[i+3] = target_camera_position[i+3]; + } +} + +void FilterControls::doOK() { + kalman_save_settings(*this); + close(); +} + +void FilterControls::doCancel() { + if (settingsDirty) { + int ret = QMessageBox::question ( this, "Settings have changed", "Do you want to save the settings?", QMessageBox::Save | QMessageBox::Discard | QMessageBox::Cancel, QMessageBox::Discard ); + + qDebug() << "doCancel says: answer =" << ret; + + switch (ret) { + case QMessageBox::Save: + kalman_save_settings(*this); + this->close(); + break; + case QMessageBox::Discard: + this->close(); + break; + case QMessageBox::Cancel: + // Cancel was clicked + break; + default: + // should never be reached + break; + } + } + else { + this->close(); + } +} + +extern "C" FTNOIR_FILTER_BASE_EXPORT Metadata* CALLING_CONVENTION GetMetadata() +{ + return new FTNoIR_FilterDll; +} + +extern "C" FTNOIR_FILTER_BASE_EXPORT IFilter* CALLING_CONVENTION GetConstructor() +{ + return new FTNoIR_Filter; +} + +extern "C" FTNOIR_FILTER_BASE_EXPORT IFilterDialog* CALLING_CONVENTION GetDialog() { + return new FilterControls; +} diff --git a/ftnoir_protocol_fg/ftnoir_protocol_fg.cpp b/ftnoir_protocol_fg/ftnoir_protocol_fg.cpp index bbeac489..7b43e306 100644 --- a/ftnoir_protocol_fg/ftnoir_protocol_fg.cpp +++ b/ftnoir_protocol_fg/ftnoir_protocol_fg.cpp @@ -98,11 +98,11 @@ quint16 senderPort; // Copy the Raw measurements directly to the client.
//
FlightData.x = headpose[TX];
- FlightData.y = headpose[RY];
+ FlightData.y = headpose[Pitch];
FlightData.z = headpose[TZ];
FlightData.p = headpose[TY];
- FlightData.h = headpose[RX];
- FlightData.r = headpose[RZ];
+ FlightData.h = headpose[Yaw];
+ FlightData.r = headpose[Roll];
FlightData.status = fg_cmd;
//
diff --git a/ftnoir_protocol_fsuipc/ftnoir_protocol_fsuipc.cpp b/ftnoir_protocol_fsuipc/ftnoir_protocol_fsuipc.cpp index 418003c0..e457d363 100644 --- a/ftnoir_protocol_fsuipc/ftnoir_protocol_fsuipc.cpp +++ b/ftnoir_protocol_fsuipc/ftnoir_protocol_fsuipc.cpp @@ -110,9 +110,9 @@ float virtRotZ; // qDebug() << "FSUIPCServer::run() says: started!";
- virtRotX = -headpose[RY]; // degrees
- virtRotY = headpose[RX];
- virtRotZ = headpose[RZ];
+ virtRotX = -headpose[Pitch]; // degrees
+ virtRotY = headpose[Yaw];
+ virtRotZ = headpose[Roll];
virtPosX = 0.0f; // cm, X and Y are not working for FS2002/2004!
virtPosY = 0.0f;
diff --git a/ftnoir_protocol_ft/ftnoir_protocol_ft.cpp b/ftnoir_protocol_ft/ftnoir_protocol_ft.cpp index 3108f0d7..1ee8b49e 100644 --- a/ftnoir_protocol_ft/ftnoir_protocol_ft.cpp +++ b/ftnoir_protocol_ft/ftnoir_protocol_ft.cpp @@ -129,16 +129,16 @@ float headRotZ; //
// Scale the Raw measurements to the client measurements.
//
- headRotX = getRadsFromDegrees(headpose[RY]);
- headRotY = getRadsFromDegrees(headpose[RX]);
- headRotZ = getRadsFromDegrees(headpose[RZ]);
+ headRotX = getRadsFromDegrees(headpose[Pitch]);
+ headRotY = getRadsFromDegrees(headpose[Yaw]);
+ headRotZ = getRadsFromDegrees(headpose[Roll]);
headPosX = headpose[TX] * 10;
headPosY = headpose[TY] * 10;
headPosZ = headpose[TZ] * 10;
- virtRotX = getRadsFromDegrees(headpose[RY]);
- virtRotY = getRadsFromDegrees(headpose[RX]);
- virtRotZ = getRadsFromDegrees(headpose[RZ]);
+ virtRotX = getRadsFromDegrees(headpose[Pitch]);
+ virtRotY = getRadsFromDegrees(headpose[Yaw]);
+ virtRotZ = getRadsFromDegrees(headpose[Roll]);
virtPosX = headpose[TX] * 10;
virtPosY = headpose[TY] * 10;
virtPosZ = headpose[TZ] * 10;
diff --git a/ftnoir_protocol_mouse/ftnoir_protocol_mouse.h b/ftnoir_protocol_mouse/ftnoir_protocol_mouse.h index 14719540..fd0058ea 100644 --- a/ftnoir_protocol_mouse/ftnoir_protocol_mouse.h +++ b/ftnoir_protocol_mouse/ftnoir_protocol_mouse.h @@ -46,9 +46,9 @@ #define MOUSE_AXIS_MAX 65535
enum FTN_AngleName {
- FTN_YAW = RX,
- FTN_PITCH = RY,
- FTN_ROLL = RZ,
+ FTN_YAW = Yaw,
+ FTN_PITCH = Pitch,
+ FTN_ROLL = Roll,
FTN_X = TX,
FTN_Y = TY,
FTN_Z = TZ
diff --git a/ftnoir_protocol_sc/ftnoir_protocol_sc.cpp b/ftnoir_protocol_sc/ftnoir_protocol_sc.cpp index ba4c9012..3cc25be4 100644 --- a/ftnoir_protocol_sc/ftnoir_protocol_sc.cpp +++ b/ftnoir_protocol_sc/ftnoir_protocol_sc.cpp @@ -90,9 +90,9 @@ void FTNoIR_Protocol::sendHeadposeToGame( double *headpose, double *rawheadpose PDWORD_PTR MsgResult = 0;
- virtSCRotX = -headpose[RY]; // degrees
- virtSCRotY = -headpose[RX];
- virtSCRotZ = headpose[RZ];
+ virtSCRotX = -headpose[Pitch]; // degrees
+ virtSCRotY = -headpose[Yaw];
+ virtSCRotZ = headpose[Roll];
virtSCPosX = headpose[TX]/100.f; // cm to meters
virtSCPosY = headpose[TY]/100.f;
diff --git a/ftnoir_protocol_vjoy/ftnoir_protocol_vjoy.cpp b/ftnoir_protocol_vjoy/ftnoir_protocol_vjoy.cpp index f4495e10..50a7bf97 100644 --- a/ftnoir_protocol_vjoy/ftnoir_protocol_vjoy.cpp +++ b/ftnoir_protocol_vjoy/ftnoir_protocol_vjoy.cpp @@ -17,9 +17,9 @@ FTNoIR_Protocol::~FTNoIR_Protocol() void FTNoIR_Protocol::sendHeadposeToGame( double *headpose, double *rawheadpose ) { JOYSTICK_STATE state[2] = { 0 }; - state[0].XAxis = std::min<int>(VJOY_AXIS_MAX, std::max<int>(VJOY_AXIS_MIN, headpose[RX] * VJOY_AXIS_MAX / 180.0)); - state[0].YAxis = std::min<int>(VJOY_AXIS_MAX, std::max<int>(VJOY_AXIS_MIN, headpose[RY] * VJOY_AXIS_MAX / 180.0)); - state[0].ZAxis = std::min<int>(VJOY_AXIS_MAX, std::max<int>(VJOY_AXIS_MIN, headpose[RZ] * VJOY_AXIS_MAX / 180.0)); + state[0].XAxis = std::min<int>(VJOY_AXIS_MAX, std::max<int>(VJOY_AXIS_MIN, headpose[Yaw] * VJOY_AXIS_MAX / 180.0)); + state[0].YAxis = std::min<int>(VJOY_AXIS_MAX, std::max<int>(VJOY_AXIS_MIN, headpose[Pitch] * VJOY_AXIS_MAX / 180.0)); + state[0].ZAxis = std::min<int>(VJOY_AXIS_MAX, std::max<int>(VJOY_AXIS_MIN, headpose[Roll] * VJOY_AXIS_MAX / 180.0)); state[0].XRotation = std::min<int>(VJOY_AXIS_MAX, std::max<int>(VJOY_AXIS_MIN, headpose[TX] * VJOY_AXIS_MAX / 100.0)); state[0].YRotation = std::min<int>(VJOY_AXIS_MAX, std::max<int>(VJOY_AXIS_MIN, headpose[TY] * VJOY_AXIS_MAX / 100.0)); state[0].ZRotation = std::min<int>(VJOY_AXIS_MAX, std::max<int>(VJOY_AXIS_MIN, headpose[TZ] * VJOY_AXIS_MAX / 100.0)); diff --git a/ftnoir_tracker_base/ftnoir_tracker_types.h b/ftnoir_tracker_base/ftnoir_tracker_types.h index d2981701..583d508b 100644 --- a/ftnoir_tracker_base/ftnoir_tracker_types.h +++ b/ftnoir_tracker_base/ftnoir_tracker_types.h @@ -31,7 +31,7 @@ #define FTNOIR_TRACKER_TYPES_H
enum Axis {
- TX = 0, TY, TZ, RX, RY, RZ
+ TX = 0, TY, TZ, Yaw, Pitch, Roll
};
#endif // FTNOIR_TRACKER_TYPES_H
diff --git a/ftnoir_tracker_hillcrest/ftnoir_tracker_hillcrest.cpp b/ftnoir_tracker_hillcrest/ftnoir_tracker_hillcrest.cpp index f2dd1fd7..e6c3bbf2 100644 --- a/ftnoir_tracker_hillcrest/ftnoir_tracker_hillcrest.cpp +++ b/ftnoir_tracker_hillcrest/ftnoir_tracker_hillcrest.cpp @@ -213,11 +213,11 @@ bool FTNoIR_Tracker::GiveHeadPoseData(double *data) freespace_perform(); body = cachedBodyFrame; if (bEnableYaw) - data[RX] = body.angularVelZ * TO_DEG; + data[Yaw] = body.angularVelZ * TO_DEG; if (bEnablePitch) - data[RY] = body.angularVelY * TO_DEG; + data[Pitch] = body.angularVelY * TO_DEG; if (bEnableRoll) - data[RZ] = body.angularVelX * TO_DEG; + data[Roll] = body.angularVelX * TO_DEG; } return device != -1; } diff --git a/ftnoir_tracker_ht/ftnoir_tracker_ht.cpp b/ftnoir_tracker_ht/ftnoir_tracker_ht.cpp index 63c1eadd..29738dd2 100644 --- a/ftnoir_tracker_ht/ftnoir_tracker_ht.cpp +++ b/ftnoir_tracker_ht/ftnoir_tracker_ht.cpp @@ -109,7 +109,7 @@ static void load_settings(ht_config_t* config, Tracker* tracker) config->pyrlk_pyramids = 3; config->pyrlk_win_size_w = config->pyrlk_win_size_h = 21; config->max_keypoints = 200; - config->keypoint_distance = 5; + config->keypoint_distance = 4.5; //config->force_width = 640; //config->force_height = 480; config->force_fps = iniFile.value("fps", 0).toInt(); @@ -215,12 +215,12 @@ bool Tracker::GiveHeadPoseData(double *data) } if (shm->result.filled) { if (enableRX) - data[RX] = shm->result.rotx; + data[Yaw] = shm->result.rotx; if (enableRY) { - data[RY] = shm->result.roty; + data[Pitch] = shm->result.roty; } if (enableRZ) { - data[RZ] = shm->result.rotz; + data[Roll] = shm->result.rotz; } if (enableTX) data[TX] = shm->result.tx; diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp index ddf91c59..99b2f680 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp @@ -240,13 +240,13 @@ bool Tracker::GiveHeadPoseData(double *data) gamma = atan2( R(2,1), R(2,2));
if (bEnableYaw) {
- data[RX] = rad2deg * alpha;
+ data[Yaw] = rad2deg * alpha;
}
if (bEnablePitch) {
- data[RY] = rad2deg * beta;
+ data[Pitch] = rad2deg * beta;
}
if (bEnableRoll) {
- data[RZ] = rad2deg * gamma;
+ data[Roll] = rad2deg * gamma;
}
}
return true;
diff --git a/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp b/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp index 13336313..3ef68260 100644 --- a/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp +++ b/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp @@ -1,23 +1,11 @@ /* Copyright: "i couldn't care less what anyone does with the 5 lines of code i wrote" - mm0zct */ #include "ftnoir_tracker_rift.h" #include "facetracknoir/global-settings.h" - #include "OVR.h" +#include "OVR.h" #include <cstdio> -#define SIXENSE_STATIC_LIB -#define SIXENSE_UTILS_STATIC_LIB -#include <sixense.h> -#include <sixense_math.hpp> -#ifdef WIN32 -#include <sixense_utils/mouse_pointer.hpp> -#endif -#include <sixense_utils/derivatives.hpp> -#include <sixense_utils/button_states.hpp> -#include <sixense_utils/event_triggers.hpp> -#include <sixense_utils/controller_manager/controller_manager.hpp> using namespace OVR; - Rift_Tracker::Rift_Tracker() { pSensor.Clear(); @@ -38,8 +26,6 @@ Rift_Tracker::Rift_Tracker() Rift_Tracker::~Rift_Tracker() { - - sixenseExit(); pSensor.Clear(); pHMD.Clear(); pManager.Clear(); @@ -48,7 +34,7 @@ Rift_Tracker::~Rift_Tracker() /* void controller_manager_setup_callback( sixenseUtils::ControllerManager::setup_step step ) { - + QMessageBox::warning(0,"OpenTrack Info", "controller manager callback",QMessageBox::Ok,QMessageBox::NoButton); if( sixenseUtils::getTheControllerManager()->isMenuVisible() ) { // Ask the controller manager what the next instruction string should be. @@ -61,18 +47,18 @@ void controller_manager_setup_callback( sixenseUtils::ControllerManager::setup_s void Rift_Tracker::StartTracker(QFrame* videoFrame) { - //QMessageBox::warning(0,"FaceTrackNoIR Notification", "Tracking loading settings...",QMessageBox::Ok,QMessageBox::NoButton); + //QMessageBox::warning(0,"FaceTrackNoIR Notification", "Tracking loading settings...",QMessageBox::Ok,QMessageBox::NoButton); loadSettings(); // // Startup the Oculus SDK device handling, use the first Rift sensor we find. // - System::Init(Log::ConfigureDefaultLog(LogMask_All)); - pManager = *DeviceManager::Create(); + System::Init(Log::ConfigureDefaultLog(LogMask_All)); + pManager = *DeviceManager::Create(); DeviceEnumerator<HMDDevice>& enumerator = pManager->EnumerateDevices<HMDDevice>(); if (enumerator.IsAvailable()) { pHMD = *enumerator.CreateDevice(); - + pSensor = *pHMD->GetSensor(); if (pSensor){ @@ -80,28 +66,17 @@ void Rift_Tracker::StartTracker(QFrame* videoFrame) }else{ QMessageBox::warning(0,"FaceTrackNoIR Error", "Unable to find Rift tracker",QMessageBox::Ok,QMessageBox::NoButton); } - isCalibrated = false; - MagCal.BeginAutoCalibration(SFusion); - SFusion.SetMagReference(SFusion.GetOrientation()); - + isCalibrated = false; + MagCal.BeginAutoCalibration(SFusion); + SFusion.SetMagReference(SFusion.GetOrientation()); } - // Init sixense - //QMessageBox::warning(0,"OpenTrack Info", "sixense init",QMessageBox::Ok,QMessageBox::NoButton); - sixenseInit(); - //QMessageBox::warning(0,"OpenTrack Info", "sixense init complete, setting controller manager",QMessageBox::Ok,QMessageBox::NoButton); - // Init the controller manager. This makes sure the controllers are present, assigned to left and right hands, and that - // the hemisphere calibration is complete. - //sixenseUtils::getTheControllerManager()->setGameType( sixenseUtils::ControllerManager::ONE_PLAYER_TWO_CONTROLLER ); - //sixenseUtils::getTheControllerManager()->registerSetupCallback( controller_manager_setup_callback ); - //QMessageBox::warning(0,"OpenTrack Info", "controller manager callback registered",QMessageBox::Ok,QMessageBox::NoButton); - return; } bool Rift_Tracker::GiveHeadPoseData(double *data) { if (pHMD.GetPtr() != NULL) { - +#if 0 if (SFusion.IsMagReady() && !isCalibrated ){ SFusion.SetYawCorrectionEnabled(true); QMessageBox::warning(0,"OpenTrack Info", "Calibrated magnetic sensor",QMessageBox::Ok,QMessageBox::NoButton); @@ -111,6 +86,7 @@ bool Rift_Tracker::GiveHeadPoseData(double *data) QMessageBox::warning(0,"OpenTrack Info", "Lost magnetic calibration",QMessageBox::Ok,QMessageBox::NoButton); } } +#endif // Magnetometer calibration procedure MagCal.UpdateAutoCalibration(SFusion); @@ -120,20 +96,15 @@ bool Rift_Tracker::GiveHeadPoseData(double *data) float roll = 0.0f; //hmdOrient.GetEulerAngles< Axis_X, Axis_Y, Axis_Z>(&pitch, &yaw, &roll); //hmdOrient.GetEulerAngles< Axis_X, Axis_Z, Axis_Y>(&pitch, &roll, &yaw); - hmdOrient.GetEulerAngles< Axis_Y, Axis_X, Axis_Z>(&yaw, &pitch, &roll); + hmdOrient.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&yaw, &pitch , &roll); //hmdOrient.GetEulerAngles< Axis_Y, Axis_Z, Axis_X>(&yaw, &roll, &pitch); //hmdOrient.GetEulerAngles< Axis_Z, Axis_X, Axis_Y>(&roll, &pitch, &yaw); //hmdOrient.GetEulerAngles< Axis_Z, Axis_Y, Axis_X>(&roll, &yaw, &pitch); - newHeadPose[RY] =pitch; - newHeadPose[RZ] = roll; - newHeadPose[RX] = yaw; - + newHeadPose[Pitch] = pitch; + newHeadPose[Roll] = roll; + newHeadPose[Yaw] = yaw; - sixenseSetActiveBase(0); - sixenseAllControllerData acd; - sixenseGetAllNewestData( &acd ); - //sixenseUtils::getTheControllerManager()->update( &acd ); -#if 1 +#if 0 sixenseControllerData cd; newHeadPose[TX] = acd.controllers[0].pos[0]/50.0f; @@ -151,13 +122,13 @@ bool Rift_Tracker::GiveHeadPoseData(double *data) //} #endif if (bEnableYaw) { - data[RX] = newHeadPose[RX] * 57.295781f; + data[Yaw] = newHeadPose[Yaw] * 57.295781f; } if (bEnablePitch) { - data[RY] = newHeadPose[RY] * 57.295781f; + data[Pitch] = newHeadPose[Pitch] * 57.295781f; } if (bEnableRoll) { - data[RZ] = newHeadPose[RZ] * 57.295781f; + data[Roll] = newHeadPose[Roll] * 57.295781f; } } return pHMD.GetPtr() != NULL; diff --git a/ftnoir_tracker_sm/ftnoir_tracker_faceapi.cpp b/ftnoir_tracker_sm/ftnoir_tracker_faceapi.cpp index b50f4916..74e1d3d2 100644 --- a/ftnoir_tracker_sm/ftnoir_tracker_faceapi.cpp +++ b/ftnoir_tracker_sm/ftnoir_tracker_faceapi.cpp @@ -112,13 +112,13 @@ bool FTNoIR_Tracker::GiveHeadPoseData(double *data) data[TZ] = pMemData->data.new_pose.head_pos.z * 100.0f;
}
if (bEnableYaw) {
- data[RX] = pMemData->data.new_pose.head_rot.y_rads * 57.295781f; // From rads to degrees
+ data[Yaw] = pMemData->data.new_pose.head_rot.y_rads * 57.295781f; // From rads to degrees
}
if (bEnablePitch) {
- data[RY] = pMemData->data.new_pose.head_rot.x_rads * 57.295781f;
+ data[Pitch] = pMemData->data.new_pose.head_rot.x_rads * 57.295781f;
}
if (bEnableRoll) {
- data[RZ] = pMemData->data.new_pose.head_rot.z_rads * 57.295781f;
+ data[Roll] = pMemData->data.new_pose.head_rot.z_rads * 57.295781f;
}
//
diff --git a/ftnoir_tracker_udp/ftnoir_tracker_udp.cpp b/ftnoir_tracker_udp/ftnoir_tracker_udp.cpp index e62a54cc..83f518fa 100644 --- a/ftnoir_tracker_udp/ftnoir_tracker_udp.cpp +++ b/ftnoir_tracker_udp/ftnoir_tracker_udp.cpp @@ -121,13 +121,13 @@ bool FTNoIR_Tracker::GiveHeadPoseData(double *data) data[TZ] = newHeadPose[TZ];
}
if (bEnableYaw) {
- data[RX] = newHeadPose[RX];
+ data[Yaw] = newHeadPose[Yaw];
}
if (bEnablePitch) {
- data[RY] = newHeadPose[RY];
+ data[Pitch] = newHeadPose[Pitch];
}
if (bEnableRoll) {
- data[RZ] = newHeadPose[RZ];
+ data[Roll] = newHeadPose[Roll];
}
mutex.unlock();
return true;
|
