1 files changed, 154 insertions, 9 deletions

diff --git a/gui/options-dialog.cpp b/gui/options-dialog.cpp
index fef2b0ca..06346b25 100644
--- a/gui/options-dialog.cpp
+++ b/gui/options-dialog.cpp
@@ -7,11 +7,12 @@
  */
 
 #include "options-dialog.hpp"
+#include "tracker-pt/camera.h"
 #include "keyboard.h"
 #include <QPushButton>
 #include <QLayout>
 
-OptionsDialog::OptionsDialog()
+OptionsDialog::OptionsDialog(State& state) : state(state), trans_calib_running(false)
 {
     ui.setupUi( this );
 
@@ -25,13 +26,6 @@ OptionsDialog::OptionsDialog()
     tie_setting(s.s_main.tcomp_p, ui.tcomp_enable);
     tie_setting(s.s_main.tcomp_tz, ui.tcomp_rz);
 
-    tie_setting(s.s_main.a_x.zero, ui.pos_tx);
-    tie_setting(s.s_main.a_y.zero, ui.pos_ty);
-    tie_setting(s.s_main.a_z.zero, ui.pos_tz);
-    tie_setting(s.s_main.a_yaw.zero, ui.pos_rx);
-    tie_setting(s.s_main.a_pitch.zero, ui.pos_ry);
-    tie_setting(s.s_main.a_roll.zero, ui.pos_rz);
-
     tie_setting(s.s_main.a_yaw.invert, ui.invert_yaw);
     tie_setting(s.s_main.a_pitch.invert, ui.invert_pitch);
     tie_setting(s.s_main.a_roll.invert, ui.invert_roll);
@@ -49,8 +43,48 @@ OptionsDialog::OptionsDialog()
     tie_setting(s.s_main.camera_yaw, ui.camera_yaw);
     tie_setting(s.s_main.camera_pitch, ui.camera_pitch);
     tie_setting(s.s_main.camera_roll, ui.camera_roll);
+    
+    tie_setting(pt.camera_mode, ui.camera_mode);
+
+    tie_setting(pt.threshold, ui.threshold_slider);
 
-    tie_setting(s.s_main.center_method, ui.center_method);
+    tie_setting(pt.min_point_size, ui.mindiam_spin);
+    tie_setting(pt.max_point_size, ui.maxdiam_spin);
+
+    tie_setting(pt.t_MH_x, ui.tx_spin);
+    tie_setting(pt.t_MH_y, ui.ty_spin);
+    tie_setting(pt.t_MH_z, ui.tz_spin);
+    
+    tie_setting(pt.fov, ui.camera_fov);
+    
+    tie_setting(pt.model_used, ui.model_used);
+    
+    connect(ui.ewma_slider, SIGNAL(valueChanged(int)), this, SLOT(update_ewma_display(int)));
+    connect(ui.rotation_slider, SIGNAL(valueChanged(int)), this, SLOT(update_rot_display(int)));
+    connect(ui.rot_dz_slider, SIGNAL(valueChanged(int)), this, SLOT(update_rot_dz_display(int)));
+    connect(ui.translation_slider, SIGNAL(valueChanged(int)), this, SLOT(update_trans_display(int)));
+    connect(ui.trans_dz_slider, SIGNAL(valueChanged(int)), this, SLOT(update_trans_dz_display(int)));
+    
+    tie_setting(acc.rot_threshold, ui.rotation_slider);
+    tie_setting(acc.trans_threshold, ui.translation_slider);
+    tie_setting(acc.ewma, ui.ewma_slider);
+    tie_setting(acc.rot_deadzone, ui.rot_dz_slider);
+    tie_setting(acc.trans_deadzone, ui.trans_dz_slider);
+    
+    update_rot_display(ui.rotation_slider->value());
+    update_trans_display(ui.translation_slider->value());
+    update_ewma_display(ui.ewma_slider->value());
+    update_rot_dz_display(ui.rot_dz_slider->value());
+    update_trans_dz_display(ui.trans_dz_slider->value());
+    
+    tie_setting(pt.dynamic_pose, ui.dynamic_pose);
+    tie_setting(pt.init_phase_timeout, ui.init_phase_timeout);
+    tie_setting(pt.auto_threshold, ui.auto_threshold);
+    
+    connect(&timer,SIGNAL(timeout()), this,SLOT(poll_tracker_info()));
+    connect( ui.tcalib_button,SIGNAL(toggled(bool)), this,SLOT(startstop_trans_calib(bool)) );
+    
+    timer.start(100);
 
     connect(ui.bind_center, &QPushButton::pressed, [&]() -> void { bind_key(s.center.keycode, ui.center_text); });
     connect(ui.bind_zero, &QPushButton::pressed, [&]() -> void { bind_key(s.zero.keycode, ui.zero_text); });
@@ -81,7 +115,9 @@ void OptionsDialog::bind_key(value<QString>& ret, QLabel* label)
 
 void OptionsDialog::doOK() {
     s.b->save();
+    pt.b->save();
     s.s_main.b->save();
+    acc.b->save();
     ui.game_detector->save();
     this->close();
     emit reload();
@@ -89,8 +125,117 @@ void OptionsDialog::doOK() {
 
 void OptionsDialog::doCancel() {
     s.b->reload();
+    pt.b->reload();
     s.s_main.b->reload();
+    acc.b->reload();
     ui.game_detector->revert();
     close();
 }
 
+void OptionsDialog::startstop_trans_calib(bool start)
+{
+    auto tracker = get_pt();
+    if (!tracker)
+    {
+        ui.tcalib_button->setChecked(false);
+        return;
+    }
+        
+    if (start)
+    {
+        qDebug()<<"TrackerDialog:: Starting translation calibration";
+        trans_calib.reset();
+        trans_calib_running = true;
+        pt.t_MH_x = 0;
+        pt.t_MH_y = 0;
+        pt.t_MH_z = 0;
+    }
+    else
+    {
+        qDebug()<<"TrackerDialog:: Stopping translation calibration";
+        trans_calib_running = false;
+        {
+            auto tmp = trans_calib.get_estimate();
+            pt.t_MH_x = tmp[0];
+            pt.t_MH_y = tmp[1];
+            pt.t_MH_z = tmp[2];
+        }
+    }
+}
+
+void OptionsDialog::poll_tracker_info()
+{
+    auto tracker = get_pt();
+    CamInfo info;
+    if (tracker && tracker->get_cam_info(&info))
+    {
+        QString to_print;
+
+        // display caminfo
+        to_print = QString::number(info.res_x)+"x"+QString::number(info.res_y)+" @ "+QString::number(info.fps)+" FPS";
+        ui.caminfo_label->setText(to_print);
+
+        // display pointinfo
+        int n_points = tracker->get_n_points();
+        to_print = QString::number(n_points);
+        if (n_points == 3)
+            to_print += " OK!";
+        else
+            to_print += " BAD!";
+        ui.pointinfo_label->setText(to_print);
+
+        // update calibration
+        if (trans_calib_running) trans_calib_step();
+    }
+    else
+    {
+        ui.caminfo_label->setText("Tracker offline");
+        ui.pointinfo_label->setText("");
+    }
+}
+
+void OptionsDialog::trans_calib_step()
+{
+    auto tracker = get_pt();
+    if (tracker)
+    {
+        Affine X_CM = tracker->pose();
+        trans_calib.update(X_CM.R, X_CM.t);
+    }
+}
+
+Tracker_PT* OptionsDialog::get_pt()
+{
+    auto work = state.work.get();
+    if (!work)
+        return nullptr;
+    auto ptr = work->libs.pTracker;
+    if (ptr)
+        return static_cast<Tracker_PT*>(ptr.get());
+    return nullptr;
+}
+
+void OptionsDialog::update_rot_display(int value)
+{
+    ui.rot_gain->setText(QString::number((value + 1) * 10 / 100.) + "°");
+}
+
+void OptionsDialog::update_trans_display(int value)
+{
+    ui.trans_gain->setText(QString::number((value + 1) * 5 / 100.) + "mm");
+}
+
+void OptionsDialog::update_ewma_display(int value)
+{
+    ui.ewma_label->setText(QString::number(value * 2) + "ms");
+}
+
+void OptionsDialog::update_rot_dz_display(int value)
+{
+    ui.rot_dz->setText(QString::number(value * 2 / 100.) + "°");
+}
+
+void OptionsDialog::update_trans_dz_display(int value)
+{
+    ui.trans_dz->setText(QString::number(value * 1 / 100.) + "mm");
+}