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

8 files changed, 122 insertions, 6 deletions

diff --git a/opentrack-logic/main-settings.hpp b/opentrack-logic/main-settings.hpp
index a7dcd11c..81eb99cf 100644
--- a/opentrack-logic/main-settings.hpp
+++ b/opentrack-logic/main-settings.hpp
@@ -73,6 +73,8 @@ struct main_settings
     key_opts key_start_tracking, key_stop_tracking, key_toggle_tracking, key_restart_tracking;
     key_opts key_center, key_toggle, key_zero;
     key_opts key_toggle_press, key_zero_press;
+    value<bool> tracklogging_enabled;
+    value<QString> tracklogging_filename;
     main_settings() :
         b(bundle("opentrack-ui")),
         a_x(b, "x", TX),
@@ -98,7 +100,9 @@ struct main_settings
         key_toggle(b, "toggle"),
         key_zero(b, "zero"),
         key_toggle_press(b, "toggle-press"),
-        key_zero_press(b, "zero-press")
+        key_zero_press(b, "zero-press"),
+        tracklogging_enabled(b, "tracklogging-enabled", false),
+        tracklogging_filename(b, "tracklogging-filename", QString())
     {
     }
 };
diff --git a/opentrack-logic/state.hpp b/opentrack-logic/state.hpp
index 1c608f7a..bdab9afe 100644
--- a/opentrack-logic/state.hpp
+++ b/opentrack-logic/state.hpp
@@ -14,6 +14,7 @@ using namespace options;
 #include "main-settings.hpp"
 #include "mappings.hpp"
 #include "selected-libraries.hpp"
+#include "tracklogger.hpp"
 #include "work.hpp"
 #include <vector>
 #include <QString>
@@ -29,4 +30,5 @@ struct State
     main_settings s;
     Mappings pose;
     mem<Work> work;
+    mem<TrackLogger> logger;
 };
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};
diff --git a/opentrack-logic/tracker.h b/opentrack-logic/tracker.h
index fae8bd9e..b4d39a44 100644
--- a/opentrack-logic/tracker.h
+++ b/opentrack-logic/tracker.h
@@ -20,6 +20,7 @@
 #include "spline-widget/functionconfig.h"
 #include "main-settings.hpp"
 #include "opentrack-compat/options.hpp"
+#include "tracklogger.hpp"
 
 #include <QMutex>
 #include <QThread>
@@ -62,6 +63,10 @@ private:
     volatile bool zero_;
     volatile bool should_quit;
     SelectedLibraries const& libs;
+    // The owner of the reference is the main window. 
+    // This design might be usefull if we decide later on to swap out 
+    // the logger while the tracker is running.
+    TrackLogger &logger;  
 
     using rmat = euler::rmat;
     using euler_t = euler::euler_t;
@@ -78,7 +83,7 @@ private:
     static constexpr double r2d = 180. / OPENTRACK_PI;
     static constexpr double d2r = OPENTRACK_PI / 180.;
 public:
-    Tracker(Mappings& m, SelectedLibraries& libs);
+    Tracker(Mappings& m, SelectedLibraries& libs, TrackLogger &logger);
     ~Tracker();
 
     rmat get_camera_offset_matrix();
diff --git a/opentrack-logic/tracklogger.cpp b/opentrack-logic/tracklogger.cpp
new file mode 100644
index 00000000..5e4cf4f9
--- /dev/null
+++ b/opentrack-logic/tracklogger.cpp
@@ -0,0 +1,28 @@
+#include "tracklogger.hpp"
+#include "tracker.h"
+ 
+
+void TrackLoggerCSV::write(const char *s)
+{
+    out << s;
+}
+
+
+void TrackLoggerCSV::write(const double *p, int n)
+{
+    if (!first_col)
+        out.put(';');
+    first_col = false;
+    for (int i = 0; i < n-1; ++i)
+    {
+        out << p[i];
+        out.put(';');
+    }
+    out << p[n-1];
+}
+
+void TrackLoggerCSV::next_line()
+{
+    out << std::endl;
+    first_col = true;
+}
\ No newline at end of file
diff --git a/opentrack-logic/tracklogger.hpp b/opentrack-logic/tracklogger.hpp
new file mode 100644
index 00000000..feed7fa2
--- /dev/null
+++ b/opentrack-logic/tracklogger.hpp
@@ -0,0 +1,57 @@
+#pragma once
+#include "main-settings.hpp"
+#include "opentrack-compat/options.hpp"
+
+#include <fstream>
+#include <QString>
+#include <QMessageBox>
+#include <QWidget>
+
+class OPENTRACK_LOGIC_EXPORT TrackLogger
+{
+public:
+    TrackLogger()
+    {
+    }
+
+    static mem<TrackLogger> make() { return std::make_shared<TrackLogger>(); }
+
+    virtual void write(const char *)
+    {
+    }
+
+    virtual void write(const double *, int n)
+    {
+    }
+
+    virtual void next_line()
+    {
+    }
+
+    void write_pose(const double *p)
+    {
+        write(p, 6);
+    }
+};
+
+
+class OPENTRACK_LOGIC_EXPORT TrackLoggerCSV : public TrackLogger
+{
+    std::ofstream out;
+    bool first_col;
+public:
+    TrackLoggerCSV(const QString &filename)  :  TrackLogger(),
+        first_col(true)
+    {
+        out.open(filename.toStdString());
+        if (!out.is_open())
+            throw std::ios_base::failure("unable to open file");
+    }
+
+    static mem<TrackLogger> make(const main_settings &s) { return std::static_pointer_cast<TrackLogger>(std::make_shared<TrackLoggerCSV>(s.tracklogging_filename)); }
+
+    virtual void write(const char *s);
+    virtual void write(const double *p, int n);
+    virtual void next_line();
+};
+
diff --git a/opentrack-logic/work.cpp b/opentrack-logic/work.cpp
index 820112bf..e5b08c18 100644
--- a/opentrack-logic/work.cpp
+++ b/opentrack-logic/work.cpp
@@ -1,9 +1,9 @@
 #include "work.hpp"
 
 
-Work::Work(Mappings& m, SelectedLibraries& libs, WId handle) :
+Work::Work(Mappings& m, SelectedLibraries& libs, TrackLogger &logger, WId handle) :
     libs(libs),
-    tracker(std::make_shared<Tracker>(m, libs)),
+    tracker(std::make_shared<Tracker>(m, libs, logger)),
     sc(std::make_shared<Shortcuts>()),
     handle(handle),
     keys {
diff --git a/opentrack-logic/work.hpp b/opentrack-logic/work.hpp
index 70322be2..6425b11c 100644
--- a/opentrack-logic/work.hpp
+++ b/opentrack-logic/work.hpp
@@ -33,7 +33,7 @@ struct OPENTRACK_LOGIC_EXPORT Work
     std::vector<key_tuple> keys;
     main_settings s;
 
-    Work(Mappings& m, SelectedLibraries& libs, WId handle);
+    Work(Mappings& m, SelectedLibraries& libs, TrackLogger &logger, WId handle);
     ~Work();
     void reload_shortcuts();
 };