summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
Diffstat
-rw-r--r--CMakeLists.txt3
-rw-r--r--facetracknoir/facetracknoir.cpp8
-rw-r--r--facetracknoir/facetracknoir.h3
-rw-r--r--facetracknoir/rotation.h54
-rw-r--r--facetracknoir/tracker_types.cpp2
-rw-r--r--ftnoir_tracker_base/ftnoir_tracker_types.h4
-rw-r--r--ftnoir_tracker_hydra/ftnoir_tracker_hydra.cpp35
-rw-r--r--ftnoir_tracker_rift/ftnoir_rift_clientcontrols.ui165
-rw-r--r--ftnoir_tracker_rift/ftnoir_tracker_rift.cpp20
-rw-r--r--ftnoir_tracker_rift/ftnoir_tracker_rift.h5
10 files changed, 162 insertions, 137 deletions
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e7a3b9de..79d7903c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -453,7 +453,8 @@ endif()
endif()
if(SDK_RIFT)
- include_directories("${SDK_RIFT}/include")
+ include_directories("${SDK_RIFT}/include")
+ include_directories("${SDK_RIFT}/Src")
add_library(opentrack-tracker-rift SHARED ${opentrack-tracker-rift-c} ${opentrack-tracker-rift-moc} ${opentrack-tracker-rift-uih} ${opentrack-tracker-rift-rcc})
target_link_libraries(opentrack-tracker-rift ${MY_QT_LIBS})
if(WIN32)
diff --git a/facetracknoir/facetracknoir.cpp b/facetracknoir/facetracknoir.cpp
index fafef908..623b16b2 100644
--- a/facetracknoir/facetracknoir.cpp
+++ b/facetracknoir/facetracknoir.cpp
@@ -682,9 +682,8 @@ void FaceTrackNoIR::startTracker( ) {
// This is necessary, because the events are only triggered 'on change'
//
tracker->setInvertAxis(Yaw, ui.chkInvertYaw->isChecked() );
- tracker->setInvertAxis(TY, ui.chkInvertPitch->isChecked() );
- tracker->setInvertAxis(Roll, ui.chkInvertRoll->isChecked() );
- tracker->setInvertAxis(TX, ui.chkInvertX->isChecked() );
+ tracker->setInvertAxis(Pitch, ui.chkInvertPitch->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,6 +848,7 @@ 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[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));
@@ -859,12 +859,14 @@ void FaceTrackNoIR::showHeadPose() {
// Get the output-pose and also display it.
//
tracker->getOutputHeadPose(newdata);
+
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[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));
diff --git a/facetracknoir/facetracknoir.h b/facetracknoir/facetracknoir.h
index fd9c06a7..a6d99165 100644
--- a/facetracknoir/facetracknoir.h
+++ b/facetracknoir/facetracknoir.h
@@ -186,8 +186,7 @@ private:
void setInvertAxis( Axis axis, int invert );
void setInvertYaw(int invert) {
- setInvertAxis(Yaw, invert);
- }
+ setInvertAxis(Yaw, invert); }
void setInvertPitch(int invert) {
setInvertAxis(Pitch, invert);
}
diff --git a/facetracknoir/rotation.h b/facetracknoir/rotation.h
index a1500969..1e472cf2 100644
--- a/facetracknoir/rotation.h
+++ b/facetracknoir/rotation.h
@@ -12,12 +12,12 @@
class Rotation {
public:
- Rotation() : a(1.0),b(0.0),c(0.0),d(0.0) {}
+ Rotation() : w(1.0),x(0.0),y(0.0),z(0.0) {}
Rotation(double yaw, double pitch, double roll) { fromEuler(yaw, pitch, roll); }
- Rotation(double a, double b, double c, double d) : a(a),b(b),c(c),d(d) {}
+ Rotation(double x, double y, double z, double w) : x(x),y(y),z(z),w(w) {}
Rotation inv(){ // inverse
- return Rotation(a,-b,-c,-d);
+ return Rotation(-x,-y,-z, w);
}
@@ -25,46 +25,48 @@ public:
// see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
void fromEuler(double yaw, double pitch, double roll)
{
- double sin_phi = sin(roll/2.0);
- double cos_phi = cos(roll/2.0);
- double sin_the = sin(pitch/2.0);
- double cos_the = cos(pitch/2.0);
- double sin_psi = sin(yaw/2.0);
- double cos_psi = cos(yaw/2.0);
-
- a = cos_phi*cos_the*cos_psi + sin_phi*sin_the*sin_psi;
- b = sin_phi*cos_the*cos_psi - cos_phi*sin_the*sin_psi;
- c = cos_phi*sin_the*cos_psi + sin_phi*cos_the*sin_psi;
- d = cos_phi*cos_the*sin_psi - sin_phi*sin_the*cos_psi;
+ // Assuming the angles are in radians.
+ double c1 = cos(yaw);
+ double s1 = sin(yaw);
+ double c2 = cos(roll);
+ double s2 = sin(roll);
+ double c3 = cos(pitch);
+ double s3 = sin(pitch);
+ w = sqrt(1.0 + c1 * c2 + c1*c3 - s1 * s2 * s3 + c2*c3) / 2.0;
+ double w4 = (4.0 * w);
+ x = (c2 * s3 + c1 * s3 + s1 * s2 * c3) / w4 ;
+ y = (s1 * c2 + s1 * c3 + c1 * s2 * s3) / w4 ;
+ z = (-s1 * s3 + c1 * s2 * c3 +s2) / w4 ;
}
void toEuler(double& yaw, double& pitch, double& roll)
{
- roll = atan2(2.0*(a*b + c*d), 1.0 - 2.0*(b*b + c*c));
- pitch = asin(2.0*(a*c - b*d));
- yaw = atan2(2.0*(a*d + b*c), 1.0 - 2.0*(c*c + d*d));
+
+ yaw = atan2(2.0*(y*w - x*z), 1.0 - 2.0*(y*y + z*z));
+ roll = asin(2.0*(x*y + z*w));
+ pitch = atan2(2.0*(x*w - y*z), 1.0 - 2.0*(x*x + z*z));
}
/* const Rotation operator*(const Rotation& A, const Rotation& B)
{
- return Rotation(A.a*B.a - A.b*B.b - A.c*B.c - A.d*B.d, // quaternion multiplication
- A.a*B.b + A.b*B.a + A.c*B.d - A.d*B.c,
- A.a*B.c - A.b*B.d + A.c*B.a + A.d*B.b,
- A.a*B.d + A.b*B.c - A.c*B.b + A.d*B.a);
+ return Rotation(A.w*B.w - A.x*B.x - A.y*B.y - A.z*B.z, // quaternion multiplication
+ A.w*B.x + A.x*B.w + A.y*B.z - A.z*B.y,
+ A.w*B.y - A.x*B.z + A.y*B.w + A.z*B.x,
+ A.w*B.z + A.x*B.y - A.y*B.x + A.z*B.w);
}*/
const Rotation operator*(const Rotation& B)
{
const Rotation& A = *this;
- return Rotation(A.a*B.a - A.b*B.b - A.c*B.c - A.d*B.d, // quaternion multiplication
- A.a*B.b + A.b*B.a + A.c*B.d - A.d*B.c,
- A.a*B.c - A.b*B.d + A.c*B.a + A.d*B.b,
- A.a*B.d + A.b*B.c - A.c*B.b + A.d*B.a);
+ return Rotation(A.w*B.w - A.x*B.x - A.y*B.y - A.z*B.z, // quaternion multiplication
+ A.w*B.x + A.x*B.w + A.y*B.z - A.z*B.y,
+ A.w*B.y - A.x*B.z + A.y*B.w + A.z*B.x,
+ A.w*B.z + A.x*B.y - A.y*B.x + A.z*B.w);
}
protected:
- double a,b,c,d; // quaternion coefficients
+ double w,x,y,z; // quaternion coefficients
};
diff --git a/facetracknoir/tracker_types.cpp b/facetracknoir/tracker_types.cpp
index 11adc985..89a06d7e 100644
--- a/facetracknoir/tracker_types.cpp
+++ b/facetracknoir/tracker_types.cpp
@@ -13,10 +13,10 @@ T6DOF operator-(const T6DOF& A, const T6DOF& B)
T6DOF C;
R_C.toEuler(C.axes[Yaw], C.axes[Pitch], C.axes[Roll]);
+ 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];
C.axes[TZ] = A.axes[TZ] - B.axes[TZ];
diff --git a/ftnoir_tracker_base/ftnoir_tracker_types.h b/ftnoir_tracker_base/ftnoir_tracker_types.h
index 583d508b..8bf12990 100644
--- a/ftnoir_tracker_base/ftnoir_tracker_types.h
+++ b/ftnoir_tracker_base/ftnoir_tracker_types.h
@@ -30,8 +30,8 @@
#ifndef FTNOIR_TRACKER_TYPES_H
#define FTNOIR_TRACKER_TYPES_H
+
enum Axis {
- TX = 0, TY, TZ, Yaw, Pitch, Roll
-};
+ TX = 0, TY, TZ, Yaw, Pitch, Roll};
#endif // FTNOIR_TRACKER_TYPES_H
diff --git a/ftnoir_tracker_hydra/ftnoir_tracker_hydra.cpp b/ftnoir_tracker_hydra/ftnoir_tracker_hydra.cpp
index 24ffedcd..3789c7bf 100644
--- a/ftnoir_tracker_hydra/ftnoir_tracker_hydra.cpp
+++ b/ftnoir_tracker_hydra/ftnoir_tracker_hydra.cpp
@@ -74,41 +74,42 @@ bool Hydra_Tracker::GiveHeadPoseData(double *data)
//sixenseUtils::getTheControllerManager()->update( &acd );
//sixenseControllerData cd;
- Rotation quat = Rotation(acd.controllers[0].rot_quat[0],acd.controllers[0].rot_quat[1],acd.controllers[0].rot_quat[2],acd.controllers[0].rot_quat[3]);
+ //Rotation quat = Rotation(acd.controllers[0].rot_quat[1],acd.controllers[0].rot_quat[2],acd.controllers[0].rot_quat[3],acd.controllers[0].rot_quat[0]);
+ sixenseMath::Matrix4 mat = sixenseMath::Matrix4(acd.controllers[0].rot_mat);// sixenseMath::Quat(acd.controllers[0].rot_quat[1],acd.controllers[0].rot_quat[2],acd.controllers[0].rot_quat[3],acd.controllers[0].rot_quat[0]);
+
double yaw = 0.0f;
double pitch = 0.0f;
double roll = 0.0f;
-
- quat.toEuler(yaw, pitch, roll);
-
- newHeadPose[RY] = pitch;
- newHeadPose[RZ] = roll;
- newHeadPose[RX] = yaw;
-
+ float ypr[3];
+
+ mat.getEulerAngles().fill(ypr);
+ newHeadPose[Yaw] = ypr[0];
+ newHeadPose[Pitch] = ypr[1];
+ newHeadPose[Roll] = ypr[2];
newHeadPose[TX] = acd.controllers[0].pos[0]/50.0f;
newHeadPose[TY] = acd.controllers[0].pos[1]/50.0f;
newHeadPose[TZ] = acd.controllers[0].pos[2]/50.0f;
- //if (bEnableX) {
+ if (bEnableX) {
data[TX] = newHeadPose[TX];
- //}
- //if (bEnableY) {
+ }
+ if (bEnableY) {
data[TY] = newHeadPose[TY];
- //}
- //if (bEnableY) {
+ }
+ if (bEnableY) {
data[TZ] = newHeadPose[TZ];
- //}
+ }
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 true;
diff --git a/ftnoir_tracker_rift/ftnoir_rift_clientcontrols.ui b/ftnoir_tracker_rift/ftnoir_rift_clientcontrols.ui
index 789d2d25..eaf5b9d6 100644
--- a/ftnoir_tracker_rift/ftnoir_rift_clientcontrols.ui
+++ b/ftnoir_tracker_rift/ftnoir_rift_clientcontrols.ui
@@ -1,7 +1,7 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>UIRiftControls</class>
- <widget class="QWidget" name="UIRiftControls">
+ <widget class="QWidget" name="UIHydraControls">
<property name="geometry">
<rect>
<x>0</x>
@@ -45,78 +45,97 @@
</rect>
</property>
<layout class="QGridLayout" name="gridLayout_2">
- <item row="0" column="0">
- <widget class="QLabel" name="label_6">
- <property name="text">
- <string>Roll:</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_11">
- <property name="text">
- <string>Yaw:</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QCheckBox" name="chkEnablePitch">
- <property name="maximumSize">
- <size>
- <width>20</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="layoutDirection">
- <enum>Qt::LeftToRight</enum>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLabel" name="label_9">
- <property name="text">
- <string>Pitch:</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QCheckBox" name="chkEnableRoll">
- <property name="maximumSize">
- <size>
- <width>20</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="layoutDirection">
- <enum>Qt::LeftToRight</enum>
- </property>
- <property name="styleSheet">
- <string notr="true"/>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QCheckBox" name="chkEnableYaw">
- <property name="maximumSize">
- <size>
- <width>20</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="layoutDirection">
- <enum>Qt::LeftToRight</enum>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
+
+
+
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_6">
+ <property name="text">
+ <string>Pitch:</string>
+ </property>
+ </widget>
+ </item>
+
+ <item row="0" column="1">
+ <widget class="QCheckBox" name="chkEnablePitch">
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="layoutDirection">
+ <enum>Qt::LeftToRight</enum>
+ </property>
+ <property name="styleSheet">
+ <string notr="true"/>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_9">
+ <property name="text">
+ <string>Yaw:</string>
+ </property>
+ </widget>
+ </item>
+
+
+ <item row="1" column="1">
+ <widget class="QCheckBox" name="chkEnableYaw">
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="layoutDirection">
+ <enum>Qt::LeftToRight</enum>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+
+
+
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_11">
+ <property name="text">
+ <string>Roll:</string>
+ </property>
+ </widget>
+ </item>
+
+ <item row="2" column="1">
+ <widget class="QCheckBox" name="chkEnableRoll">
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="layoutDirection">
+ <enum>Qt::LeftToRight</enum>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+
+
+
+
+
+
+
+
</layout>
</widget>
</widget>
diff --git a/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp b/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp
index 367ab389..4e9dd7a3 100644
--- a/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp
+++ b/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp
@@ -66,7 +66,8 @@ void Rift_Tracker::StartTracker(QFrame* videoFrame)
}else{
QMessageBox::warning(0,"FaceTrackNoIR Error", "Unable to find Rift tracker",QMessageBox::Ok,QMessageBox::NoButton);
}
- //MagCal.BeginAutoCalibration(SFusion);
+ isCalibrated = false;
+ MagCal.BeginAutoCalibration(SFusion);
SFusion.SetMagReference(SFusion.GetOrientation());
}
}
@@ -79,32 +80,27 @@ bool Rift_Tracker::GiveHeadPoseData(double *data)
if (SFusion.IsMagReady() && !isCalibrated ){
SFusion.SetYawCorrectionEnabled(true);
QMessageBox::warning(0,"OpenTrack Info", "Calibrated magnetic sensor",QMessageBox::Ok,QMessageBox::NoButton);
+ isCalibrated = true;
}else{
if(isCalibrated){
isCalibrated = false;
QMessageBox::warning(0,"OpenTrack Info", "Lost magnetic calibration",QMessageBox::Ok,QMessageBox::NoButton);
}
}
-#endif
// Magnetometer calibration procedure
- //MagCal.UpdateAutoCalibration(SFusion);
+ MagCal.UpdateAutoCalibration(SFusion);
+#endif
Quatf hmdOrient = SFusion.GetOrientation();
float yaw = 0.0f;
float pitch = 0.0f;
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_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[Pitch] = pitch;
- newHeadPose[Roll] = roll;
- newHeadPose[Yaw] = yaw;
+ newHeadPose[Yaw] = yaw;
+ newHeadPose[Pitch] =pitch;
+ newHeadPose[Roll] = roll;
#if 0
- sixenseControllerData cd;
newHeadPose[TX] = acd.controllers[0].pos[0]/50.0f;
newHeadPose[TY] = acd.controllers[0].pos[1]/50.0f;
diff --git a/ftnoir_tracker_rift/ftnoir_tracker_rift.h b/ftnoir_tracker_rift/ftnoir_tracker_rift.h
index 0f3bc099..7bacb91c 100644
--- a/ftnoir_tracker_rift/ftnoir_tracker_rift.h
+++ b/ftnoir_tracker_rift/ftnoir_tracker_rift.h
@@ -6,6 +6,7 @@
#include <math.h>
#include "facetracknoir/global-settings.h"
#include "OVR.h"
+#include "Util/Util_MagCalibration.h"
class Rift_Tracker : public ITracker
{
public:
@@ -26,6 +27,10 @@ private:
OVR::Ptr<OVR::HMDDevice> pHMD;
OVR::Ptr<OVR::SensorDevice> pSensor;
OVR::SensorFusion SFusion;
+ // Magnetometer calibration and yaw correction
+ OVR::Util::MagCalibration MagCal;
+ bool isCalibrated;
+
double newHeadPose[6]; // Structure with new headpose
bool bEnableRoll;
bool bEnablePitch;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-21 07:34:00 +0000