summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2014-11-22 22:18:09 +0100
committerStanislaw Halik <sthalik@misaki.pl>2014-11-22 22:18:09 +0100
commitafb2ffaf47e8ce8ce5ee6bc1335c3a2c88cb3535 (patch)
tree19fb6b5b7906710557db6fb172e769209bd0baaa
parentddfa1005f16b439bb2dd933bf9ee49b5870eb8c7 (diff)
pt: restore original ypr code
-rw-r--r--ftnoir_tracker_pt/ftnoir_tracker_pt.cpp26
1 files changed, 14 insertions, 12 deletions
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index a6732ced..0429d6ed 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -168,22 +168,24 @@ void Tracker::data(THeadPoseData *data)
Matx33f R = X_GH.R;
Vec3f t = X_GH.t;
- Matx33f R_EG( 0, 0,-1,
- -1, 0, 0,
- 0, 1, 0);
+ // translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame
+ // -z -> x, y -> z, x -> -y
+ 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);
// extract rotation angles
- data[Yaw] = -rad2deg * atan2( R(1,0), R(0,0));
- data[Pitch] = -rad2deg * atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
- data[Roll] = rad2deg * atan2( R(2,1), R(2,2));
+ float alpha, beta, gamma;
+ beta = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
+ alpha = atan2( R(1,0), R(0,0));
+ gamma = atan2( R(2,1), R(2,2));
+ QMutexLocker lock(&mutex);
+ // extract rotation angles
+ data[Yaw] = rad2deg * alpha;
+ data[Pitch] = -rad2deg * beta;
+ data[Roll] = rad2deg * gamma;
// get translation(s)
data[TX] = t[0] / 10.0; // convert to cm
data[TY] = t[1] / 10.0;