new track logging: record poses in various stages of processing into a file

1 files changed, 21 insertions, 1 deletions

diff --git a/opentrack-logic/tracker.cpp b/opentrack-logic/tracker.cpp
index a1f206a1..8101932c 100644
--- a/opentrack-logic/tracker.cpp
+++ b/opentrack-logic/tracker.cpp
@@ -21,7 +21,7 @@
 #   include <windows.h>
 #endif
 
-Tracker::Tracker(Mappings &m, SelectedLibraries &libs) :
+Tracker::Tracker(Mappings &m, SelectedLibraries &libs, TrackLogger &logger) :
     m(m),
     newpose {0,0,0, 0,0,0},
     centerp(s.center_at_startup),
@@ -29,6 +29,7 @@ Tracker::Tracker(Mappings &m, SelectedLibraries &libs) :
     zero_(false),
     should_quit(false),
     libs(libs),
+    logger(logger),
     r_b(get_camera_offset_matrix().t()),
     t_b {0,0,0}
 {
@@ -134,6 +135,8 @@ void Tracker::logic()
         raw(i) = newpose[i];
     }
 
+    logger.write_pose(raw); // raw tracker input
+
     if (is_nan(raw))
         raw = last_raw;
 
@@ -207,10 +210,13 @@ void Tracker::logic()
         }
     }
 
+    logger.write_pose(value); // after various transformations to account for camera position
+
     // whenever something can corrupt its internal state due to nan/inf, elide the call
     if (is_nan(value))
     {
         nan = true;
+        logger.write_pose(value); // for consistency with filtered value
     }
     else
     {
@@ -218,8 +224,11 @@ void Tracker::logic()
             Pose tmp = value;
 
             if (libs.pFilter)
+            {
                 libs.pFilter->filter(tmp, value);
+            }
         }
+        logger.write_pose(value); // filtered value if filter present
 
         // CAVEAT rotation only, due to tcomp
         for (int i = 3; i < 6; i++)
@@ -256,6 +265,8 @@ void Tracker::logic()
     for (int i = 0; i < 3; i++)
         value(i) = map(value(i), m(i));
 
+    logger.write_pose(value); // after mapping
+
     if (nan)
     {
         value = last_mapped;
@@ -265,6 +276,8 @@ void Tracker::logic()
             (void) map(value(i), m(i));
     }
 
+    logger.next_line();
+
     libs.pProtocol->pose(value);
 
     last_mapped = value;
@@ -283,8 +296,15 @@ void Tracker::run()
     (void) timeBeginPeriod(1);
 #endif
 
+    logger.write("//dt;raw;before filter;after filter;after mapping; every pose has channels TX, TY, TZ, Yaw, Pitch, Roll");
+    logger.next_line();
+
     while (!should_quit)
     {
+        {
+            double dt = t.elapsed_seconds();
+            logger.write(&dt, 1);
+        }
         t.start();
 
         double tmp[6] {0,0,0, 0,0,0};