1 files changed, 14 insertions, 10 deletions
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index 3afb7fa7..1545f6b1 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -166,17 +166,21 @@ void Tracker::data(THeadPoseData *data)
     Matx33f R = X_GH.R;
     Vec3f   t = X_GH.t;
 
-    // extract rotation angles
-    float alpha, beta, gamma;
-    beta  = atan( -R(2,0) / sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
-    alpha = atan( R(1,0) / R(0,0));
-    gamma = atan( R(2,1) / R(2,2));
-
+    Matx33f R_EG( 0, 0,-1,
+                 -1, 0, 0,
+                  0, 1, 0);
+    R = R_EG * R * R_EG.t();
+
+    static constexpr float pi = 3.141592653;
+    float cy = cos(pi*.5);
+    float sy = sin(pi*.5);
+    Matx33f r_y(cy, 0, sy, 0, 1, 0, -sy, 0, cy);
+    R = r_y.t() * R;
     QMutexLocker lock(&mutex);
-
-    data[Yaw] =   rad2deg * alpha;
-    data[Roll] = - rad2deg * beta;	// FTNoIR expects a minus here
-    data[Pitch]  =   rad2deg * gamma;
+    // extract rotation angles
+    data[Pitch]   = rad2deg * atan( -R(0,2) / R(0,0));
+    data[Roll]    = rad2deg * asin( R(0,1));
+    data[Yaw]  = rad2deg * atan( R(2,1) / R(1,1));
 
     // get translation(s)
     data[TX] = t[0] / 10.0;	// convert to cm