diff options
| author | Stanislaw Halik <sthalik@misaki.pl> | 2013-07-28 14:08:42 +0200 |
|---|---|---|
| committer | Stanislaw Halik <sthalik@misaki.pl> | 2013-07-28 14:08:42 +0200 |
| commit | f7ebbcc2f6fdc3f835b323d7515a5291daa90481 (patch) | |
| tree | b0afb6bd7937c5f39361cf90e2b5c67f9cfa005c /ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | |
| parent | 0c3c6f34a0b94e1acbf1044efd2d1e5432a7caf3 (diff) | |
Apply translation compensation for yaw/pitch
Diffstat (limited to 'ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp')
| -rw-r--r-- | ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 5 |
1 files changed, 3 insertions, 2 deletions
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp index 171b8df3..200010e8 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp @@ -244,6 +244,9 @@ void Tracker::run() pose[Yaw] = foo[1]; pose[Pitch] = -foo[0]; pose[Roll] = foo[2]; + + pose[Yaw] -= atan(pose[TX] / pose[TZ]) * 180 / HT_PI; + pose[Pitch] -= atan(pose[TY] / pose[TZ]) * 180 / HT_PI; } frame = color.clone(); @@ -252,8 +255,6 @@ void Tracker::run() bool Tracker::GiveHeadPoseData(double *data) { - bool ret = false; - QMutexLocker lck(&mtx); if (frame.rows > 0) |