From 136f28907c55547eb3faada1cbe8250a259b4288 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Wed, 29 Oct 2014 15:19:25 +0100
Subject: centering formula got wrong again

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

diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
index 3d5f6cbd..bcdfcfe8 100644
--- a/opentrack/tracker.cpp
+++ b/opentrack/tracker.cpp
@@ -135,7 +135,8 @@ void Tracker::logic()
 
     {
         const cv::Matx33d rmat = euler_to_rmat(&filtered_pose[Yaw]);
-        const cv::Matx33d m_ = r_b.t() * rmat;
+        const cv::Matx33d m_c = rmat * r_b.t();
+        const cv::Matx33d m_ = r_b * m_c * r_b.t();
         const auto euler = rmat_to_euler(m_);
         for (int i = 0; i < 3; i++)
         {
-- 
cgit v1.2.3