tracker/pt: add point filter

13 files changed, 246 insertions, 15 deletions

diff --git a/tracker-pt/FTNoIR_PT_Controls.ui b/tracker-pt/FTNoIR_PT_Controls.ui
index fb41d27e..edb401da 100644
--- a/tracker-pt/FTNoIR_PT_Controls.ui
+++ b/tracker-pt/FTNoIR_PT_Controls.ui
@@ -10,7 +10,7 @@
     <x>0</x>
     <y>0</y>
     <width>420</width>
-    <height>724</height>
+    <height>703</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -702,6 +702,78 @@
          </layout>
         </widget>
        </item>
+       <item>
+        <widget class="QGroupBox" name="groupBox">
+         <property name="title">
+          <string>Point filter</string>
+         </property>
+         <layout class="QHBoxLayout" name="horizontalLayout">
+          <item>
+           <widget class="QCheckBox" name="enable_point_filter">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Maximum" vsizetype="Maximum">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="toolTip">
+             <string>Filter point centers prior to pose estimation.</string>
+            </property>
+            <property name="text">
+             <string>Enable</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QSlider" name="point_filter_slider">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimum">
+             <number>0</number>
+            </property>
+            <property name="maximum">
+             <number>400</number>
+            </property>
+            <property name="singleStep">
+             <number>1</number>
+            </property>
+            <property name="pageStep">
+             <number>10</number>
+            </property>
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QDoubleSpinBox" name="point_filter_label">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Maximum" vsizetype="Maximum">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+            </property>
+            <property name="readOnly">
+             <bool>true</bool>
+            </property>
+            <property name="buttonSymbols">
+             <enum>QAbstractSpinBox::NoButtons</enum>
+            </property>
+            <property name="maximum">
+             <double>999.990000000000009</double>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </item>
       </layout>
      </widget>
      <widget class="QWidget" name="tab_4">
@@ -1463,23 +1535,22 @@ Don't roll or change position.</string>
   <tabstop>fps_spin</tabstop>
   <tabstop>use_mjpeg</tabstop>
   <tabstop>fov</tabstop>
-  <tabstop>dynamic_pose</tabstop>
   <tabstop>init_phase_timeout</tabstop>
-  <tabstop>blob_color</tabstop>
+  <tabstop>dynamic_pose</tabstop>
   <tabstop>camera_settings</tabstop>
+  <tabstop>blob_color</tabstop>
   <tabstop>auto_threshold</tabstop>
   <tabstop>threshold_slider</tabstop>
   <tabstop>mindiam_spin</tabstop>
   <tabstop>maxdiam_spin</tabstop>
+  <tabstop>enable_point_filter</tabstop>
+  <tabstop>point_filter_slider</tabstop>
+  <tabstop>point_filter_label</tabstop>
   <tabstop>model_tabs</tabstop>
   <tabstop>clip_tlength_spin</tabstop>
   <tabstop>clip_theight_spin</tabstop>
   <tabstop>clip_bheight_spin</tabstop>
   <tabstop>clip_blength_spin</tabstop>
-  <tabstop>tx_spin</tabstop>
-  <tabstop>ty_spin</tabstop>
-  <tabstop>tz_spin</tabstop>
-  <tabstop>tcalib_button</tabstop>
   <tabstop>cap_length_spin</tabstop>
   <tabstop>cap_width_spin</tabstop>
   <tabstop>cap_height_spin</tabstop>
@@ -1487,8 +1558,12 @@ Don't roll or change position.</string>
   <tabstop>m1y_spin</tabstop>
   <tabstop>m1z_spin</tabstop>
   <tabstop>m2x_spin</tabstop>
-  <tabstop>m2z_spin</tabstop>
   <tabstop>m2y_spin</tabstop>
+  <tabstop>m2z_spin</tabstop>
+  <tabstop>tx_spin</tabstop>
+  <tabstop>ty_spin</tabstop>
+  <tabstop>tz_spin</tabstop>
+  <tabstop>tcalib_button</tabstop>
  </tabstops>
  <resources>
   <include location="module/tracker_pt.qrc"/>
diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp
index 2911a217..64d08c26 100644
--- a/tracker-pt/ftnoir_tracker_pt.cpp
+++ b/tracker-pt/ftnoir_tracker_pt.cpp
@@ -88,10 +88,7 @@ void Tracker_PT::run()
                 {
                     int dynamic_pose_ms = s.dynamic_pose ? s.init_phase_timeout : 0;
 
-                    point_tracker.track(points,
-                                        PointModel(s),
-                                        info,
-                                        dynamic_pose_ms);
+                    point_tracker.track(points, PointModel(s), info, dynamic_pose_ms, filter);
                     ever_success.store(true, std::memory_order_relaxed);
                 }
 
diff --git a/tracker-pt/ftnoir_tracker_pt.h b/tracker-pt/ftnoir_tracker_pt.h
index 210c6a01..fa3d94ad 100644
--- a/tracker-pt/ftnoir_tracker_pt.h
+++ b/tracker-pt/ftnoir_tracker_pt.h
@@ -13,6 +13,7 @@
 #include "point_tracker.h"
 #include "cv/numeric.hpp"
 #include "video/video-widget.hpp"
+#include "point-filter.hpp"
 
 #include <atomic>
 #include <memory>
@@ -74,6 +75,7 @@ private:
     std::atomic<unsigned> point_count { 0 };
     std::atomic<bool> ever_success = false;
     mutable QMutex center_lock, data_lock;
+    point_filter filter{s};
 };
 
 } // ns pt_impl
diff --git a/tracker-pt/ftnoir_tracker_pt_dialog.cpp b/tracker-pt/ftnoir_tracker_pt_dialog.cpp
index 0f056e82..5e95b533 100644
--- a/tracker-pt/ftnoir_tracker_pt_dialog.cpp
+++ b/tracker-pt/ftnoir_tracker_pt_dialog.cpp
@@ -120,6 +120,12 @@ TrackerDialog_PT::TrackerDialog_PT(const QString& module_name) :
     tie_setting(s.auto_threshold,
                 this,
                 [this](bool) { s.threshold_slider.notify(); });
+
+    tie_setting(s.enable_point_filter, ui.enable_point_filter);
+    tie_setting(s.point_filter_coefficient, ui.point_filter_slider);
+    connect(&s.point_filter_coefficient, value_::value_changed<slider_value>(),
+            ui.point_filter_label, [this] { ui.point_filter_label->setValue(*s.point_filter_coefficient); } );
+    ui.point_filter_label->setValue(*s.point_filter_coefficient);
 }
 
 QString TrackerDialog_PT::threshold_display_text(int threshold_value)
diff --git a/tracker-pt/lang/nl_NL.ts b/tracker-pt/lang/nl_NL.ts
index 41cd8e4b..99efccda 100644
--- a/tracker-pt/lang/nl_NL.ts
+++ b/tracker-pt/lang/nl_NL.ts
@@ -280,6 +280,18 @@ Don&apos;t roll or change position.</source>
         <source>MJPEG compression</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Point filter</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Filter point centers prior to pose estimation.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>pt_impl::TrackerDialog_PT</name>
diff --git a/tracker-pt/lang/ru_RU.ts b/tracker-pt/lang/ru_RU.ts
index f2266d82..3b288720 100644
--- a/tracker-pt/lang/ru_RU.ts
+++ b/tracker-pt/lang/ru_RU.ts
@@ -285,6 +285,18 @@ ROLL или X/Y-смещения.</translation>
         <source>MJPEG compression</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Point filter</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Filter point centers prior to pose estimation.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>pt_impl::TrackerDialog_PT</name>
diff --git a/tracker-pt/lang/stub.ts b/tracker-pt/lang/stub.ts
index da299466..77823135 100644
--- a/tracker-pt/lang/stub.ts
+++ b/tracker-pt/lang/stub.ts
@@ -280,6 +280,18 @@ Don&apos;t roll or change position.</source>
         <source>MJPEG compression</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Point filter</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Filter point centers prior to pose estimation.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>pt_impl::TrackerDialog_PT</name>
diff --git a/tracker-pt/lang/zh_CN.ts b/tracker-pt/lang/zh_CN.ts
index c9d320a1..99ab5ea1 100644
--- a/tracker-pt/lang/zh_CN.ts
+++ b/tracker-pt/lang/zh_CN.ts
@@ -280,6 +280,18 @@ Don&apos;t roll or change position.</source>
         <source>MJPEG compression</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Point filter</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Filter point centers prior to pose estimation.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>pt_impl::TrackerDialog_PT</name>
diff --git a/tracker-pt/point-filter.cpp b/tracker-pt/point-filter.cpp
new file mode 100644
index 00000000..4217ac1c
--- /dev/null
+++ b/tracker-pt/point-filter.cpp
@@ -0,0 +1,62 @@
+#include "point-filter.hpp"
+#include <QDebug>
+
+namespace pt_point_filter_impl {
+
+void point_filter::reset()
+{
+    t = std::nullopt;
+}
+
+const PointOrder& point_filter::operator()(const PointOrder& input)
+{
+    if (!s.enable_point_filter)
+    {
+        t = std::nullopt;
+        state_ = input;
+        return state_;
+    }
+
+    if (!t)
+    {
+        t.emplace();
+        state_ = input;
+        return state_;
+    }
+
+    const f K = progn(
+        constexpr int C = 1'000'000;
+        f x = (f)*s.point_filter_coefficient;
+        f log10_pos = -2+(int)x, rest = 1+fmod(x, (f)1)*9;
+        return C * pow((f)10, (f)-log10_pos) * rest;
+    );
+    f dt = (f)t->elapsed_seconds(); t->start();
+    f dist[3], norm = 0;
+
+    for (unsigned i = 0; i < 3; i++)
+    {
+        vec2 tmp = input[i] - state_[i];
+        dist[i] = sqrt(tmp.dot(tmp));
+        norm += dist[i];
+    }
+
+    if (norm < (f)1e-6)
+        return state_;
+
+    // gain
+    float delta = std::clamp(norm * norm * K * dt, (f)0, (f)1);
+
+    for (unsigned i = 0; i < 3; i++)
+    {
+        f x = delta * dist[i] / norm;
+        state_[i] += x*(input[i] - state_[i]);
+    }
+
+    return state_;
+}
+
+point_filter::point_filter(const pt_settings& s) : s{s}
+{
+}
+
+} // ns pt_point_filter_impl
diff --git a/tracker-pt/point-filter.hpp b/tracker-pt/point-filter.hpp
new file mode 100644
index 00000000..cc86505b
--- /dev/null
+++ b/tracker-pt/point-filter.hpp
@@ -0,0 +1,32 @@
+#pragma once
+
+#include "cv/numeric.hpp"
+#include "compat/timer.hpp"
+#include "pt-settings.hpp"
+#include <optional>
+#include <array>
+
+namespace pt_point_filter_impl {
+
+using namespace numeric_types;
+using PointOrder = std::array<numeric_types::vec2, 3>;
+
+class point_filter final
+{
+    PointOrder state_;
+    std::optional<Timer> t;
+    const pt_settings& s;
+
+public:
+    void reset();
+    const PointOrder& operator()(const PointOrder& input);
+
+    explicit point_filter(const pt_settings& s);
+    ~point_filter() = default;
+
+    OTR_DISABLE_MOVE_COPY(point_filter);
+};
+
+} // ns pt_point_filter_impl
+
+using point_filter = pt_point_filter_impl::point_filter;
diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp
index 850a5b46..b0ed7a6c 100644
--- a/tracker-pt/point_tracker.cpp
+++ b/tracker-pt/point_tracker.cpp
@@ -135,7 +135,8 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const vec2*
 void PointTracker::track(const std::vector<vec2>& points,
                          const PointModel& model,
                          const pt_camera_info& info,
-                         int init_phase_timeout)
+                         int init_phase_timeout,
+                         point_filter& filter)
 {
     const f fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y);
     PointOrder order;
@@ -148,7 +149,7 @@ void PointTracker::track(const std::vector<vec2>& points,
     else
         order = find_correspondences_previous(points.data(), model, info);
 
-    if (POSIT(model, order, fx) != -1)
+    if (POSIT(model, filter(order), fx) != -1)
     {
         init_phase = false;
         t.start();
diff --git a/tracker-pt/point_tracker.h b/tracker-pt/point_tracker.h
index 326fb7df..54c33a5e 100644
--- a/tracker-pt/point_tracker.h
+++ b/tracker-pt/point_tracker.h
@@ -11,6 +11,7 @@
 #include "cv/affine.hpp"
 #include "cv/numeric.hpp"
 #include "pt-api.hpp"
+#include "point-filter.hpp"
 
 #include <cstddef>
 #include <memory>
@@ -60,7 +61,11 @@ public:
     // track the pose using the set of normalized point coordinates (x pos in range -0.5:0.5)
     // f : (focal length)/(sensor width)
     // dt : time since last call
-    void track(const std::vector<vec2>& projected_points, const PointModel& model, const pt_camera_info& info, int init_phase_timeout);
+    void track(const std::vector<vec2>& projected_points,
+               const PointModel& model,
+               const pt_camera_info& info,
+               int init_phase_timeout,
+               point_filter& filter);
     Affine pose() const { return X_CM; }
     vec2 project(const vec3& v_M, f focal_length);
     vec2 project(const vec3& v_M, f focal_length, const Affine& X_CM);
diff --git a/tracker-pt/pt-settings.hpp b/tracker-pt/pt-settings.hpp
index e2004ad7..8aa3c39b 100644
--- a/tracker-pt/pt-settings.hpp
+++ b/tracker-pt/pt-settings.hpp
@@ -68,6 +68,9 @@ struct pt_settings final : options::opts
 
     value<slider_value> threshold_slider { b, "threshold-slider", { 128, 0, 255 } };
 
+    value<bool> enable_point_filter{ b, "enable-point-filter", false };
+    value<slider_value> point_filter_coefficient { b, "point-filter-coefficient", { 1.5, 0, 4 } };
+
     explicit pt_settings(const QString& name) : opts(name) {}
 };