diff options
| author | Stanislaw Halik <sthalik@misaki.pl> | 2014-10-25 20:48:27 +0200 |
|---|---|---|
| committer | Stanislaw Halik <sthalik@misaki.pl> | 2014-10-25 20:58:26 +0200 |
| commit | d1f1067007524f68e3e6dec074d21e1884722507 (patch) | |
| tree | e59ae44ae5aef87dd1bdb7e40ebade4dd3f9e2cf /opentrack/tracker.cpp | |
| parent | 52e0c206f862fc77a3aac270e8a3fda27d82609c (diff) | |
Basis change got lost, replace it
Issue: #63
Diffstat (limited to 'opentrack/tracker.cpp')
| -rw-r--r-- | opentrack/tracker.cpp | 3 |
1 files changed, 2 insertions, 1 deletions
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp index 24561be3..2a93898c 100644 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -138,6 +138,7 @@ void Tracker::logic() if (centerp) { centerp = false; + cv::Matx31d tmp; r_b = euler_to_rmat(&filtered_pose[Yaw]); for (int i = 0; i < 3; i++) t_b[i] = filtered_pose(i); @@ -147,7 +148,7 @@ void Tracker::logic() { const cv::Matx33d rmat = euler_to_rmat(&filtered_pose[Yaw]); - const cv::Matx33d m_ = rmat * r_b.t(); + const cv::Matx33d m_ = (r_b * rmat * r_b.t()) * r_b.t(); const auto euler = rmat_to_euler(m_); for (int i = 0; i < 3; i++) { |