Rotation basis algebra for centering

Reported-by: @doveman months ago, many times

Issue: #63
@dbaarda please confirm that- or -whether- it makes any sense.

Issue: #86
It could be either worse or better than before. Please specify.
Sadly, no time to plug the videos as camera input to PT as of yet.

Signed-off-by: Stanislaw Halik <sthalik@misaki.pl>

4 files changed, 61 insertions, 46 deletions

diff --git a/opentrack/pose.hpp b/opentrack/pose.hpp
index 41e984f5..d0aadcd9 100644
--- a/opentrack/pose.hpp
+++ b/opentrack/pose.hpp
@@ -13,7 +13,7 @@ private:
 
     double axes[6];
 public:
-    Pose() : axes {0,0,0, 0,0,0 } {}
+    Pose() : axes {0,0,0, 0,0,0} {}
 
     inline operator double*() { return axes; }
     inline operator const double*() const { return axes; }
@@ -32,13 +32,4 @@ public:
         q.to_euler_degrees(ret(Yaw), ret(Pitch), ret(Roll));
         return ret;
     }
-
-    Pose operator&(const Pose& B) const
-    {
-        const Quat q = quat() * B.quat().inv();
-        Pose ret = fromQuat(q);
-        for (int i = TX; i < TX + 3; i++)
-            ret(i) = axes[i] - B.axes[i];
-        return ret;
-    }
 };
diff --git a/opentrack/quat.hpp b/opentrack/quat.hpp
index 6d777b28..aec87f4c 100644
--- a/opentrack/quat.hpp
+++ b/opentrack/quat.hpp
@@ -12,19 +12,19 @@ class Quat {
 private:
     static constexpr double pi = 3.141592653;
     static constexpr double r2d = 180./pi;
-    double a,b,c,d; // quaternion coefficients
+    double _0,_1,_2,_3; // quaternion coefficients
 public:
-    Quat() : a(1.),b(0.),c(0.),d(0.) {}
-    Quat(double yaw, double pitch, double roll) { from_euler_rads(yaw, pitch, roll); }
-    Quat(double a, double b, double c, double d) : a(a),b(b),c(c),d(d) {}
+    Quat() : _0(1.),_1(0.),_2(0.),_3(0.) {}
+    Quat(double yaw, double pitch, double roll) { *this = from_euler_rads(yaw, pitch, roll); }
+    Quat(double a, double b, double c, double d) : _0(a),_1(b),_2(c),_3(d) {}
 
-    Quat inv(){
-        return Quat(a,-b,-c, -d);
+    Quat inv() const {
+        return Quat(_0,-_1,-_2, -_3);
     }
 
     // conversions
     // see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
-    void from_euler_rads(double yaw, double pitch, double roll)
+    static Quat from_euler_rads(double yaw, double pitch, double roll)
     {
 
         const double sin_phi = sin(roll/2.);
@@ -34,17 +34,21 @@ public:
         const double sin_psi = sin(yaw/2.);
         const double cos_psi = cos(yaw/2.);
 
-        a = cos_phi*cos_the*cos_psi + sin_phi*sin_the*sin_psi;
-        b = sin_phi*cos_the*cos_psi - cos_phi*sin_the*sin_psi;
-        c = cos_phi*sin_the*cos_psi + sin_phi*cos_the*sin_psi;
-        d = cos_phi*cos_the*sin_psi - sin_phi*sin_the*cos_psi;
+        Quat q;
+
+        q._0 = cos_phi*cos_the*cos_psi + sin_phi*sin_the*sin_psi;
+        q._1 = sin_phi*cos_the*cos_psi - cos_phi*sin_the*sin_psi;
+        q._2 = cos_phi*sin_the*cos_psi + sin_phi*cos_the*sin_psi;
+        q._3 = cos_phi*cos_the*sin_psi - sin_phi*sin_the*cos_psi;
+
+        return q;
     }
 
     void to_euler_rads(double& yaw, double& pitch, double& roll) const
     {
-        roll = atan2(2.*(a*b + c*d), 1. - 2.*(b*b + c*c));
-        pitch = asin(2.*(a*c - b*d));
-        yaw =  atan2(2.*(a*d + b*c), 1. - 2.*(c*c + d*d));
+        roll = atan2(2.*(_0*_1 + _2*_3), 1. - 2.*(_1*_1 + _2*_2));
+        pitch = asin(2.*(_0*_2 - _1*_3));
+        yaw =  atan2(2.*(_0*_3 + _1*_2), 1. - 2.*(_2*_2 + _3*_3));
     }
 
     void to_euler_degrees(double& yaw, double& pitch, double& roll) const
@@ -58,9 +62,15 @@ public:
     const Quat operator*(const Quat& B) const
     {
         const Quat& A = *this;
-        return Quat(A.a*B.a - A.b*B.b - A.c*B.c - A.d*B.d,	// quaternion multiplication
-                    A.a*B.b + A.b*B.a + A.c*B.d - A.d*B.c,
-                    A.a*B.c - A.b*B.d + A.c*B.a + A.d*B.b,
-                    A.a*B.d + A.b*B.c - A.c*B.b + A.d*B.a);
-    }
+        const Quat ret(A._0*B._0 - A._1*B._1 - A._2*B._2 - A._3*B._3,
+                       A._0*B._1 + A._1*B._0 + A._2*B._3 - A._3*B._2,
+                       A._0*B._2 - A._1*B._3 + A._2*B._0 + A._3*B._1,
+                       A._0*B._3 + A._1*B._2 - A._2*B._1 + A._3*B._0);
+        const double mag_2 = ret._0 * ret._0 +
+                             ret._1 * ret._1 +
+                             ret._2 * ret._2 +
+                             ret._3 * ret._3;
+        const double inv_mag = 1./sqrt(mag_2);
+        return Quat(1., ret._1 * inv_mag, ret._2 * inv_mag, ret._3 * inv_mag);
+   }
 };
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
index b40399e3..95e81c1a 100644
--- a/opentrack/tracker.cpp
+++ b/opentrack/tracker.cpp
@@ -28,7 +28,8 @@ Tracker::Tracker(main_settings& s, Mappings &m, SelectedLibraries &libs) :
     centerp(false),
     enabledp(true),
     should_quit(false),
-    libs(libs)
+    libs(libs),
+    t_b {0,0,0}
 {
 }
 
@@ -73,7 +74,7 @@ static cv::Matx33d euler_to_rmat(const double* input)
         cosH * cosP,
     };
 
-    return cv::Matx33d(foo);    
+    return cv::Matx33d(foo);
 }
 
 void Tracker::t_compensate(const double* input, double* output, bool rz)
@@ -91,7 +92,7 @@ void Tracker::t_compensate(const double* input, double* output, bool rz)
 void Tracker::logic()
 {
     libs.pTracker->data(newpose);
-    
+
     Pose final_raw_;
 
     if (enabledp)
@@ -110,29 +111,38 @@ void Tracker::logic()
         }
         final_raw = final_raw_;
     }
-    
+
     Pose filtered_pose;
-    
+
     if (libs.pFilter)
         libs.pFilter->filter(final_raw, filtered_pose);
     else
         filtered_pose = final_raw;
-    
+
     if (centerp)
     {
         centerp = false;
-        raw_center = filtered_pose;
+        r_b = filtered_pose.quat();
+        for (int i = 0; i < 3; i++)
+            t_b[i] = filtered_pose(i);
+    }
+
+    Pose raw_centered;
+
+    {
+        const Quat q = filtered_pose.quat();
+        raw_centered = Pose::fromQuat(r_b.inv() * q);
+        for (int i = 0; i < 3; i++)
+            raw_centered(i) = filtered_pose(i) - t_b[i];
     }
-    
-    Pose raw_centered = filtered_pose & raw_center;
-    
+
     Pose mapped_pose_precomp;
-    
+
     for (int i = 0; i < 6; i++)
         mapped_pose_precomp(i) = map(raw_centered(i), m(i));
-    
+
     Pose mapped_pose;
-    
+
     mapped_pose = mapped_pose_precomp;
     if (s.tcomp_p)
         t_compensate(mapped_pose_precomp, mapped_pose, s.tcomp_tz);
diff --git a/opentrack/tracker.h b/opentrack/tracker.h
index 462f4e50..4282bc4e 100644
--- a/opentrack/tracker.h
+++ b/opentrack/tracker.h
@@ -21,18 +21,22 @@ private:
     QMutex mtx;
     main_settings& s;
     Mappings& m;
-    
+
     Timer t;
-    Pose output_pose, raw_6dof, raw_center, final_raw;
+    Pose output_pose, raw_6dof, final_raw;
+
     double newpose[6];
     std::atomic<bool> centerp;
     std::atomic<bool> enabledp;
     std::atomic<bool> should_quit;
     SelectedLibraries const& libs;
-    
+
+    Quat r_b;
+    double t_b[3];
+
     double map(double pos, Mapping& axis);
     void logic();
-    
+
     static void t_compensate(const double* input, double* output, bool rz);
     void run() override;
 public: