rework accela, less nonsensical user-facing options

6 files changed, 109 insertions, 138 deletions

diff --git a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
index 3c7f52ab..310b08cc 100644
--- a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
+++ b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
@@ -9,8 +9,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>277</width>
-    <height>447</height>
+    <width>288</width>
+    <height>432</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -39,36 +39,14 @@
          </sizepolicy>
         </property>
         <property name="text">
-         <string>Rotation</string>
+         <string>Fast rotation speed</string>
         </property>
         <property name="alignment">
          <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
         </property>
        </widget>
       </item>
-      <item row="0" column="1">
-       <widget class="QDoubleSpinBox" name="rotation_alpha">
-        <property name="alignment">
-         <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
-        </property>
-        <property name="correctionMode">
-         <enum>QAbstractSpinBox::CorrectToPreviousValue</enum>
-        </property>
-        <property name="decimals">
-         <number>4</number>
-        </property>
-        <property name="minimum">
-         <double>0.100000000000000</double>
-        </property>
-        <property name="maximum">
-         <double>65535.000000000000000</double>
-        </property>
-        <property name="value">
-         <double>1.000000000000000</double>
-        </property>
-       </widget>
-      </item>
-      <item row="1" column="0">
+      <item row="2" column="0">
        <widget class="QLabel" name="label_3">
         <property name="sizePolicy">
          <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
@@ -81,51 +59,6 @@
         </property>
        </widget>
       </item>
-      <item row="1" column="1">
-       <widget class="QDoubleSpinBox" name="translation_alpha">
-        <property name="alignment">
-         <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
-        </property>
-        <property name="decimals">
-         <number>4</number>
-        </property>
-        <property name="minimum">
-         <double>0.100000000000000</double>
-        </property>
-        <property name="maximum">
-         <double>65535.000000000000000</double>
-        </property>
-        <property name="value">
-         <double>1.000000000000000</double>
-        </property>
-       </widget>
-      </item>
-      <item row="2" column="0">
-       <widget class="QLabel" name="label_11">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-        <property name="text">
-         <string>Fast alpha</string>
-        </property>
-       </widget>
-      </item>
-      <item row="2" column="1">
-       <widget class="QSlider" name="ui_fast_alpha">
-        <property name="maximum">
-         <number>100</number>
-        </property>
-        <property name="pageStep">
-         <number>8</number>
-        </property>
-        <property name="orientation">
-         <enum>Qt::Horizontal</enum>
-        </property>
-       </widget>
-      </item>
       <item row="3" column="0">
        <widget class="QLabel" name="lblSensYaw_6">
         <property name="sizePolicy">
@@ -200,8 +133,52 @@ background:none;</string>
         </property>
        </widget>
       </item>
-      <item row="5" column="0">
-       <widget class="QLabel" name="label_10">
+      <item row="2" column="1">
+       <widget class="QSlider" name="trans_smoothing">
+        <property name="minimum">
+         <number>1</number>
+        </property>
+        <property name="maximum">
+         <number>100</number>
+        </property>
+        <property name="pageStep">
+         <number>5</number>
+        </property>
+        <property name="orientation">
+         <enum>Qt::Horizontal</enum>
+        </property>
+        <property name="tickPosition">
+         <enum>QSlider::TicksAbove</enum>
+        </property>
+        <property name="tickInterval">
+         <number>25</number>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="1">
+       <widget class="QSlider" name="rot_plus">
+        <property name="minimum">
+         <number>1</number>
+        </property>
+        <property name="maximum">
+         <number>100</number>
+        </property>
+        <property name="pageStep">
+         <number>5</number>
+        </property>
+        <property name="orientation">
+         <enum>Qt::Horizontal</enum>
+        </property>
+        <property name="tickPosition">
+         <enum>QSlider::TicksAbove</enum>
+        </property>
+        <property name="tickInterval">
+         <number>25</number>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="0">
+       <widget class="QLabel" name="label_4">
         <property name="sizePolicy">
          <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
           <horstretch>0</horstretch>
@@ -209,23 +186,32 @@ background:none;</string>
          </sizepolicy>
         </property>
         <property name="text">
-         <string>Exponent</string>
+         <string>Slow rotation speed</string>
+        </property>
+        <property name="alignment">
+         <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
         </property>
        </widget>
       </item>
-      <item row="5" column="1">
-       <widget class="QDoubleSpinBox" name="expt">
-        <property name="decimals">
-         <number>3</number>
-        </property>
+      <item row="1" column="1">
+       <widget class="QSlider" name="rot_minus">
         <property name="minimum">
-         <double>0.050000000000000</double>
+         <number>1</number>
         </property>
         <property name="maximum">
-         <double>100.000000000000000</double>
+         <number>100</number>
         </property>
-        <property name="singleStep">
-         <double>0.050000000000000</double>
+        <property name="pageStep">
+         <number>5</number>
+        </property>
+        <property name="orientation">
+         <enum>Qt::Horizontal</enum>
+        </property>
+        <property name="tickPosition">
+         <enum>QSlider::TicksAbove</enum>
+        </property>
+        <property name="tickInterval">
+         <number>25</number>
         </property>
        </widget>
       </item>
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
index 8a98e528..370d19db 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
@@ -12,24 +12,24 @@
 #include "opentrack/plugin-api.hpp"
 using namespace std;
 
-FTNoIR_Filter::FTNoIR_Filter() : first_run(true), fast_state { 0,0,0, 0,0,0 }
+FTNoIR_Filter::FTNoIR_Filter() : first_run(true), fast_state { 0,0,0 }
 {
 }
 
-static inline double parabola(const double a, const double x, const double dz, const double expt)
+static inline double dz(double x, double dz)
 {
-    const double sign = x > 0 ? 1 : -1;
-    const double a1 = 1./a;
-    return a1 * pow(std::max(0., fabs(x) - dz), expt) * sign;
+    return std::max(0., fabs(x) - dz) * (x < 0. ? -1. : 1.);
 }
 
 void FTNoIR_Filter::filter(const double* input, double *output)
 {
     if (first_run)
     {
+        for (int i = 0; i < 3; i++)
+            fast_state[i] = 0;
+
         for (int i = 0; i < 6; i++)
         {
-            fast_state[i] = 0;
             output[i] = input[i];
             for (int j = 0; j < 3; j++)
                 last_output[i] = input[i];
@@ -38,46 +38,45 @@ void FTNoIR_Filter::filter(const double* input, double *output)
         return;
     }
 
-    const double fast_c = s.fast_alpha / 100.;
     const double rot_dz = s.rot_deadzone;
     const double trans_dz = s.trans_deadzone;
-    const double rot_a = s.rotation_alpha;
-    const double trans_a = s.translation_alpha;
-    const double expt = s.expt;
+
+    const int s_rot_plus = s.rot_plus, s_rot_minus = s.rot_minus;
+
+    const double a_rot_plus = s_rot_plus/100.;
+    const double a_rot_minus = s_rot_minus/100. * a_rot_plus;
+    const double a_trans = s.trans_smoothing/100.;
 
     static constexpr double fast_alpha = Hz/(Hz + fast_alpha_seconds);
 
-    for (int i=0;i<6;i++)
+    for (int i = 0; i < 6; i++)
     {
         const double vec = input[i] - last_output[i];
-        const double a = i >= 3 ? rot_a : trans_a;
-        const double deadzone = i >= 3 ? rot_dz : trans_dz;
-
-        double datum;
+        double datum = Hz * 16;
 
         if (i >= 3)
         {
-            const double cur_fast = std::abs(vec) * fast_alpha + fast_state[i]*(1. - fast_alpha);
-            fast_state[i] = cur_fast;
-            const double how_fast = std::max(0., fast_c * (cur_fast - max_slow_delta));
-            datum = parabola(a, vec * (1.-damping + (1.+damping)*how_fast), deadzone, s.expt);
+            int k = i - 3;
+            const double vec_ = dz(vec, rot_dz);
+            const double cur_fast = fabs(vec_) * fast_alpha + fast_state[k]*(1. - fast_alpha);
+            fast_state[k] = cur_fast;
+            const double c = cur_fast > max_slow_delta ? a_rot_plus : a_rot_minus;
+            datum *= vec_ * c;
         }
         else
-            datum = parabola(a, vec, deadzone, expt);
+            datum *= dz(vec, trans_dz) * a_trans;
 
         const double result = last_output[i] + datum;
         const bool negp = vec < 0.;
         const bool done = negp ? result <= input[i] : result >= input[i];
 
-        last_output[i] = last_output[i];
-        last_output[i] = last_output[i];
         const double ret = done ? input[i] : result;
         last_output[i] = output[i] = ret;
     }
 
-    state.y = output[Yaw] - input[Yaw];
-    state.p = output[Pitch] - input[Pitch];
-    state.r = output[Roll] - input[Roll];
+    state.y = fast_state[0];
+    state.p = fast_state[1];
+    state.r = fast_state[2];
 }
 
 extern "C" OPENTRACK_EXPORT IFilter* GetConstructor()
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.h b/ftnoir_filter_accela/ftnoir_filter_accela.h
index a7821516..afc85d47 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.h
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.h
@@ -5,30 +5,20 @@
 #include <QMutex>
 #include <QTimer>
 
-#define ACCELA_SMOOTHING_ROTATION 60.0
-#define ACCELA_SMOOTHING_TRANSLATION 40.0
-#define ACCELA_fast_ALPHA 100.0
-#define ACCELA_THIRD_ORDER_ALPHA 180.0
-
 #include "opentrack/options.hpp"
 using namespace options;
 
 struct settings {
     pbundle b;
-    value<double> rotation_alpha,
-                  translation_alpha,
-                  rot_deadzone,
-                  trans_deadzone,
-                  expt;
-    value<int> fast_alpha;
+    value<double> rot_deadzone, trans_deadzone;
+    value<int> rot_plus, rot_minus, trans_smoothing;
     settings() :
         b(bundle("Accela")),
-        rotation_alpha(b, "rotation-alpha", ACCELA_SMOOTHING_ROTATION),
-        translation_alpha(b, "translation-alpha", ACCELA_SMOOTHING_TRANSLATION),
         rot_deadzone(b, "rotation-deadband", 0),
         trans_deadzone(b, "translation-deadband", 0),
-        expt(b, "exponent", 2),
-        fast_alpha(b, "fast-alpha", 0)
+        rot_plus(b, "rotation-increase", 30),
+        rot_minus(b, "rotation-decrease", 25),
+        trans_smoothing(b, "translation-smoothing", 0)
     {}
 };
 
@@ -44,20 +34,16 @@ public:
     FTNoIR_Filter();
     void filter(const double* target_camera_position, double *new_camera_position);
     state_display state;
-private:
-    // hardcoded distance between filter() calls
     static constexpr double Hz = 3./1000;
-    // moving average history
-    static constexpr double fast_alpha_seconds = 0.6;
+private:
+    // moving average history amount
+    static constexpr double fast_alpha_seconds = 0.7;
     // max degrees considered "slow" after moving average
     static constexpr double max_slow_delta = 0.9;
-    // if set to zero, never decreases response above, can also slow
-    // down due to fast_alpha
-    static constexpr double damping = 0.; 
     settings s;
     bool first_run;
     double last_output[6];
-    double fast_state[6];
+    double fast_state[3];
 };
 
 class FilterControls: public IFilterDialog
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp b/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp
index 0f21cbcb..cbfe650f 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp
+++ b/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp
@@ -12,12 +12,12 @@ FilterControls::FilterControls() :
     connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
     connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
 
-    tie_setting(s.rotation_alpha, ui.rotation_alpha);
-    tie_setting(s.translation_alpha, ui.translation_alpha);
-    tie_setting(s.fast_alpha, ui.ui_fast_alpha);
     tie_setting(s.rot_deadzone, ui.rot_deadzone);
     tie_setting(s.trans_deadzone, ui.trans_deadzone);
-    tie_setting(s.expt, ui.expt);
+
+    tie_setting(s.rot_plus , ui.rot_plus);
+    tie_setting(s.rot_minus , ui.rot_minus);
+    tie_setting(s.trans_smoothing, ui.trans_smoothing);
 
     connect(&t, SIGNAL(timeout()), this, SLOT(timer_fired()));
 
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 67dd8433..f7e1da5b 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -219,7 +219,7 @@ void Tracker::run()
 
         std::vector< aruco::Marker > markers;
 
-        const double size_min = 0.07;
+        const double size_min = 0.02;
         const double size_max = 0.4;
         
         bool roi_valid = false;
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
index ad1938f9..24482bfc 100644
--- a/opentrack/tracker.cpp
+++ b/opentrack/tracker.cpp
@@ -92,12 +92,12 @@ static cv::Matx33d euler_to_rmat(const double* input)
 
 void Tracker::t_compensate(const cv::Matx33d& rmat, const double* xyz, double* output, bool rz)
 {
-    const double xyz_[3] = { xyz[2], xyz[0], -xyz[1] };
+    const double xyz_[3] = { xyz[2], -xyz[0], -xyz[1] };
     cv::Matx31d tvec(xyz_);
     const cv::Matx31d ret = rmat * tvec;
     if (!rz)
         output[2] = ret(0, 0);
-    output[0] = ret(1, 0);
+    output[0] = -ret(1, 0);
     output[1] = -ret(2, 0);
 }