From d1f1067007524f68e3e6dec074d21e1884722507 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Sat, 25 Oct 2014 20:48:27 +0200
Subject: Basis change got lost, replace it

Issue: #63
---
 opentrack/tracker.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

(limited to 'opentrack')

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++)
         {
-- 
cgit v1.2.3