summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2021-09-28 19:31:15 +0200
committerStanislaw Halik <sthalik@misaki.pl>2021-09-28 19:33:26 +0200
commit51449ce32dcac7692d5b29fc49f8102b3478466c (patch)
tree9a176d4a3872dc74f324f5de5d1aee3ac32f487a
parentdce50f45d7dcb4fda3c4f30bbb89a2dab9500afd (diff)
tracker/pt: add point filter
-rw-r--r--tracker-pt/FTNoIR_PT_Controls.ui91
-rw-r--r--tracker-pt/ftnoir_tracker_pt.cpp5
-rw-r--r--tracker-pt/ftnoir_tracker_pt.h2
-rw-r--r--tracker-pt/ftnoir_tracker_pt_dialog.cpp6
-rw-r--r--tracker-pt/lang/nl_NL.ts12
-rw-r--r--tracker-pt/lang/ru_RU.ts12
-rw-r--r--tracker-pt/lang/stub.ts12
-rw-r--r--tracker-pt/lang/zh_CN.ts12
-rw-r--r--tracker-pt/point-filter.cpp62
-rw-r--r--tracker-pt/point-filter.hpp32
-rw-r--r--tracker-pt/point_tracker.cpp5
-rw-r--r--tracker-pt/point_tracker.h7
-rw-r--r--tracker-pt/pt-settings.hpp3
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) {}
};