Merge pull request #398 from DaMichel/logging

Track Logging

12 files changed, 299 insertions, 7 deletions

diff --git a/gui/options-dialog.cpp b/gui/options-dialog.cpp
index 3b54ae9a..9dcb7879 100644
--- a/gui/options-dialog.cpp
+++ b/gui/options-dialog.cpp
@@ -11,6 +11,7 @@
 #include <QPushButton>
 #include <QLayout>
 #include <QDialog>
+#include <QFileDialog>
 
 static QString kopts_to_string(const key_opts& kopts)
 {
@@ -72,6 +73,9 @@ OptionsDialog::OptionsDialog(std::function<void(bool)> pause_keybindings) :
 
     tie_setting(main.center_method, ui.center_method);
 
+    tie_setting(main.tracklogging_enabled, ui.tracklogging_enabled);
+    tie_setting(main.tracklogging_filename, ui.tracklogging_filenameedit);
+
     struct tmp
     {
         key_opts& opt;
@@ -102,6 +106,8 @@ OptionsDialog::OptionsDialog(std::function<void(bool)> pause_keybindings) :
             connect(val.button, &QPushButton::pressed, this, [=]() -> void { bind_key(val.opt, val.label); });
         }
     }
+
+    connect(ui.tracklogging_fileselectbtn, SIGNAL(clicked()), this, SLOT(browse_datalogging_file()));
 }
 
 void OptionsDialog::bind_key(key_opts& kopts, QLabel* label)
@@ -163,3 +169,23 @@ void OptionsDialog::doCancel()
     close();
 }
 
+void OptionsDialog::browse_datalogging_file()
+{
+    QString filename = ui.tracklogging_filenameedit->text();
+    if (filename.isEmpty())
+        filename = QDir::currentPath();
+    /* Sometimes this function freezes the app before opening the dialog.
+       Might be related to https://forum.qt.io/topic/49209/qfiledialog-getopenfilename-hangs-in-windows-when-using-the-native-dialog/8
+       and be a known problem. Possible solution is to use the QFileDialog::DontUseNativeDialog flag.
+       Since the freeze is apparently random, I'm not sure it helped.
+    */
+    QString newfilename = QFileDialog::getSaveFileName(this, tr("Select Filename"), filename, tr("CSV File (*.csv)"), nullptr, QFileDialog::DontUseNativeDialog);
+    if (!newfilename.isEmpty())
+        ui.tracklogging_filenameedit->setText(newfilename);
+}
+
+void OptionsDialog::update_widgets_states(bool tracker_is_running)
+{
+    ui.tracklogging_enabled->setEnabled(!tracker_is_running);
+    ui.tracklogging_fileselectbtn->setEnabled(!tracker_is_running);
+}
\ No newline at end of file
diff --git a/gui/options-dialog.hpp b/gui/options-dialog.hpp
index 66386a79..f84ee8f6 100644
--- a/gui/options-dialog.hpp
+++ b/gui/options-dialog.hpp
@@ -13,6 +13,8 @@ signals:
     void saving();
 public:
     OptionsDialog(std::function<void(bool)> pause_keybindings);
+public slots:
+    void update_widgets_states(bool tracker_is_running);
 private:
     main_settings main;
     std::function<void(bool)> pause_keybindings;
@@ -22,4 +24,5 @@ private slots:
     void doOK();
     void doCancel();
     void bind_key(key_opts &kopts, QLabel* label);
+    void browse_datalogging_file();
 };
diff --git a/gui/settings.ui b/gui/settings.ui
index 4654dd85..9c573da2 100644
--- a/gui/settings.ui
+++ b/gui/settings.ui
@@ -33,7 +33,7 @@
       </sizepolicy>
      </property>
      <property name="currentIndex">
-      <number>0</number>
+      <number>2</number>
      </property>
      <widget class="QWidget" name="tab">
       <attribute name="title">
@@ -1205,6 +1205,86 @@
         </widget>
        </item>
        <item>
+        <widget class="QGroupBox" name="groupBox_10">
+         <property name="sizePolicy">
+          <sizepolicy hsizetype="Preferred" vsizetype="Expanding">
+           <horstretch>0</horstretch>
+           <verstretch>0</verstretch>
+          </sizepolicy>
+         </property>
+         <property name="minimumSize">
+          <size>
+           <width>0</width>
+           <height>20</height>
+          </size>
+         </property>
+         <property name="title">
+          <string>Data Logging</string>
+         </property>
+         <layout class="QVBoxLayout" name="verticalLayout_2">
+          <item>
+           <widget class="QLabel" name="tracklogging_label">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Preferred" vsizetype="Expanding">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="text">
+             <string>Record pose data in a csv file. WARNING: overwrites file contents without warning every time the tracker is started.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QCheckBox" name="tracklogging_enabled">
+            <property name="text">
+             <string>Enable</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <layout class="QHBoxLayout" name="horizontalLayout_2">
+            <property name="spacing">
+             <number>0</number>
+            </property>
+            <property name="bottomMargin">
+             <number>0</number>
+            </property>
+            <item>
+             <widget class="QPushButton" name="tracklogging_fileselectbtn">
+              <property name="text">
+               <string>Select File ...</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QLineEdit" name="tracklogging_filenameedit">
+              <property name="enabled">
+               <bool>false</bool>
+              </property>
+              <property name="acceptDrops">
+               <bool>true</bool>
+              </property>
+              <property name="inputMask">
+               <string notr="true"/>
+              </property>
+              <property name="text">
+               <string notr="true"/>
+              </property>
+              <property name="readOnly">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+           </layout>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item>
         <spacer name="verticalSpacer_3">
          <property name="orientation">
           <enum>Qt::Vertical</enum>
diff --git a/gui/ui.cpp b/gui/ui.cpp
index 85d5d029..94fd0cbf 100644
--- a/gui/ui.cpp
+++ b/gui/ui.cpp
@@ -285,6 +285,7 @@ void MainWindow::reload_options()
     ensure_tray();
 }
 
+
 void MainWindow::startTracker()
 {
     if (work)
@@ -331,6 +332,12 @@ void MainWindow::startTracker()
     // trackers take care of layout state updates
     const bool is_inertial = ui.video_frame->layout() == nullptr;
     updateButtonState(true, is_inertial);
+    
+    // Update the state of the options window directly.
+    // Might be better to emit signals and allow the options window
+    // to connect its slots to them (?)
+    if (options_widget)
+        options_widget->update_widgets_states(true);
 
     ui.btnStopTracker->setFocus();
 }
@@ -364,6 +371,9 @@ void MainWindow::stopTracker()
         display_pose(p, p);
     }
     updateButtonState(false, false);
+    
+    if (options_widget)
+        options_widget->update_widgets_states(false);
 
     set_title();
 
@@ -495,6 +505,7 @@ void MainWindow::show_options_dialog()
     if (mk_window(&options_widget, [&](bool flag) -> void { set_keys_enabled(!flag); }))
     {
         connect(options_widget.get(), &OptionsDialog::saving, this, &MainWindow::reload_options);
+        options_widget->update_widgets_states(work != nullptr);
     }
 }
 
diff --git a/gui/ui.h b/gui/ui.h
index 7415d3d4..d6f5e400 100644
--- a/gui/ui.h
+++ b/gui/ui.h
@@ -102,6 +102,7 @@ private slots:
     void startTracker();
     void stopTracker();
     void reload_options();
+
 signals:
     void emit_start_tracker();
     void emit_stop_tracker();
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/tracker.cpp b/opentrack-logic/tracker.cpp
index a1f206a1..f29e3100 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
+
     if (is_nan(raw))
         raw = last_raw;
 
@@ -207,10 +210,13 @@ void Tracker::logic()
         }
     }
 
+    logger.write_pose(value); // "corrected" - 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); // "filtered"
     }
     else
     {
@@ -220,6 +226,7 @@ void Tracker::logic()
             if (libs.pFilter)
                 libs.pFilter->filter(tmp, value);
         }
+        logger.write_pose(value); // "filtered"
 
         // CAVEAT rotation only, due to tcomp
         for (int i = 3; i < 6; i++)
@@ -256,6 +263,8 @@ void Tracker::logic()
     for (int i = 0; i < 3; i++)
         value(i) = map(value(i), m(i));
 
+    logger.write_pose(value); // "mapped"
+
     if (nan)
     {
         value = last_mapped;
@@ -265,6 +274,8 @@ void Tracker::logic()
             (void) map(value(i), m(i));
     }
 
+    logger.next_line();
+
     libs.pProtocol->pose(value);
 
     last_mapped = value;
@@ -283,8 +294,28 @@ void Tracker::run()
     (void) timeBeginPeriod(1);
 #endif
 
+    {
+        const char* posechannels[6] = { "TX", "TY", "TZ", "Yaw", "Pitch", "Roll" };
+        const char* datachannels[5] = { "dt", "raw", "corrected", "filtered", "mapped" };
+        logger.write(datachannels[0]);
+        char buffer[128];
+        for (int j = 1; j < 5; ++j)
+        {
+            for (int i = 0; i < 6; ++i)
+            {
+                snprintf(buffer, 128, "%s%s", datachannels[j], posechannels[i]);
+                logger.write(buffer);
+            }
+        }
+    }
+    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..f007c2bc
--- /dev/null
+++ b/opentrack-logic/tracklogger.cpp
@@ -0,0 +1,33 @@
+#include "tracklogger.hpp"
+#include "tracker.h"
+
+void TrackLoggerCSV::handle_first_col_sep()
+{
+    if (!first_col)
+        out.put(',');
+    first_col = false;
+}
+
+void TrackLoggerCSV::write(const char *s)
+{
+    handle_first_col_sep();
+    out << s;
+}
+
+
+void TrackLoggerCSV::write(const double *p, int n)
+{
+    handle_first_col_sep();
+    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..65128d48
--- /dev/null
+++ b/opentrack-logic/tracklogger.hpp
@@ -0,0 +1,53 @@
+#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()
+    {
+    }
+
+    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;
+    inline void handle_first_col_sep();
+public:
+    TrackLoggerCSV(const QString &filename)  :  TrackLogger(),
+        first_col(true)
+    {
+        out.open(filename.toStdString());
+    }
+
+    bool is_open() const { return out.is_open(); }
+    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..8d00270b 100644
--- a/opentrack-logic/work.cpp
+++ b/opentrack-logic/work.cpp
@@ -1,9 +1,50 @@
 #include "work.hpp"
 
+#include <QMessageBox>
+
+
+std::shared_ptr<TrackLogger> Work::make_logger(const main_settings &s)
+{
+    if (s.tracklogging_enabled)
+    {
+        if (static_cast<QString>(s.tracklogging_filename).isEmpty())
+        {
+            QMessageBox::warning(nullptr, "Logging Error",
+                "No filename given for track logging. Proceeding without logging.",
+                QMessageBox::Ok,
+                QMessageBox::NoButton);
+        }
+        else
+        {
+            auto logger = std::make_shared<TrackLoggerCSV>(s.tracklogging_filename);
+            if (!logger->is_open())
+            {
+                logger = nullptr;
+                QMessageBox::warning(nullptr, "Logging Error",
+                    "Unable to open file: " + s.tracklogging_filename + ". Proceeding  without logging.",
+                    QMessageBox::Ok,
+                    QMessageBox::NoButton);
+            }
+            else
+            {
+                /* As this function has the potential to fill up the hard drive
+                   of the unwary with junk data, a warning is in order. */
+                QMessageBox::warning(nullptr, "Logging Active",
+                    "Just a heads up. You are recoding pose data to " + s.tracklogging_filename + "!",
+                    QMessageBox::Ok,
+                    QMessageBox::NoButton);
+                return logger;
+            }
+        }
+    }
+    return std::make_shared<TrackLogger>();
+}
+
 
 Work::Work(Mappings& m, SelectedLibraries& libs, WId handle) :
     libs(libs),
-    tracker(std::make_shared<Tracker>(m, libs)),
+    logger(make_logger(s)),
+    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..4afb1da4 100644
--- a/opentrack-logic/work.hpp
+++ b/opentrack-logic/work.hpp
@@ -13,6 +13,7 @@
 #include "tracker.h"
 #include "shortcuts.h"
 #include "export.hpp"
+#include "tracklogger.hpp"
 
 #include <QObject>
 #include <QFrame>
@@ -25,15 +26,18 @@ struct OPENTRACK_LOGIC_EXPORT Work
 {
     using fn_t = std::function<void(bool)>;
     using key_tuple = std::tuple<key_opts&, fn_t, bool>;
-
+    main_settings s; // tracker needs settings, so settings must come before it
     SelectedLibraries& libs;
+    std::shared_ptr<TrackLogger> logger; // must come before tracker, since tracker depends on it
     std::shared_ptr<Tracker> tracker;
     std::shared_ptr<Shortcuts> sc;
     WId handle;
     std::vector<key_tuple> keys;
-    main_settings s;
 
     Work(Mappings& m, SelectedLibraries& libs, WId handle);
     ~Work();
     void reload_shortcuts();
+
+private:
+    std::shared_ptr<TrackLogger> make_logger(const main_settings &s);
 };