tracker/pt: add unperspective for yaw

It's commented out for the time-being.

1 files changed, 15 insertions, 3 deletions

diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp
index 2a13e3c6..ef6058db 100644
--- a/tracker-pt/ftnoir_tracker_pt.cpp
+++ b/tracker-pt/ftnoir_tracker_pt.cpp
@@ -235,6 +235,12 @@ void Tracker_PT::data(double *data)
 
         using std::atan2;
         using std::sqrt;
+        using std::atan;
+        using std::fabs;
+        using std::copysign;
+
+        // get translation(s)
+        const vec3& t = X_GH.t;
 
         // extract rotation angles
         {
@@ -243,13 +249,19 @@ void Tracker_PT::data(double *data)
             alpha = atan2( R(1,0), R(0,0));
             gamma = atan2( R(2,1), R(2,2));
 
+#if 0
+            if (t[2] > 1e-4)
+            {
+                alpha += copysign(atan(t[0] / t[2]), t[0]);
+                // pitch is skewed anyway due to only one focal length value
+                //beta -= copysign(atan(t[1] / t[2]), t[1]);
+            }
+#endif
+
             data[Yaw] = rad2deg * alpha;
             data[Pitch] = -rad2deg * beta;
             data[Roll] = rad2deg * gamma;
         }
-        // get translation(s)
-
-        const vec3& t = X_GH.t;
 
         // convert to cm
         data[TX] = t[0] / 10;