summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2014-11-09 15:51:15 +0100
committerStanislaw Halik <sthalik@misaki.pl>2014-11-10 08:10:08 +0100
commit8ca41f5afb05e702b9d0e715825c49679a6e7d5f (patch)
tree80ca47cd26f1f9bbba2c7b29c26fc123c863355f
parent4218571bfcf09a0fa67ecd0919dba69e442391d4 (diff)
restore PT axis order swap
Reported-by: @FlyingCircus- Issue: #63
-rw-r--r--ftnoir_tracker_pt/ftnoir_tracker_pt.cpp24
-rw-r--r--ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp2
2 files changed, 15 insertions, 11 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
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
index ad9ef7c0..d4cf68ec 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
@@ -83,7 +83,7 @@ TrackerDialog::TrackerDialog()
void TrackerDialog::set_model_clip()
{
s.m01_x = 0;
- s.m01_y = static_cast<double>(s.clip_ty);
+ s.m01_y = -static_cast<double>(s.clip_ty);
s.m01_z = -static_cast<double>(s.clip_tz);
s.m02_x = 0;
s.m02_y = static_cast<double>(s.clip_by);