From 51449ce32dcac7692d5b29fc49f8102b3478466c Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Tue, 28 Sep 2021 19:31:15 +0200 Subject: tracker/pt: add point filter --- tracker-pt/FTNoIR_PT_Controls.ui | 91 ++++++++++++++++++++++++++++++--- tracker-pt/ftnoir_tracker_pt.cpp | 5 +- tracker-pt/ftnoir_tracker_pt.h | 2 + tracker-pt/ftnoir_tracker_pt_dialog.cpp | 6 +++ tracker-pt/lang/nl_NL.ts | 12 +++++ tracker-pt/lang/ru_RU.ts | 12 +++++ tracker-pt/lang/stub.ts | 12 +++++ tracker-pt/lang/zh_CN.ts | 12 +++++ tracker-pt/point-filter.cpp | 62 ++++++++++++++++++++++ tracker-pt/point-filter.hpp | 32 ++++++++++++ tracker-pt/point_tracker.cpp | 5 +- tracker-pt/point_tracker.h | 7 ++- tracker-pt/pt-settings.hpp | 3 ++ 13 files changed, 246 insertions(+), 15 deletions(-) create mode 100644 tracker-pt/point-filter.cpp create mode 100644 tracker-pt/point-filter.hpp 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 @@ 0 0 420 - 724 + 703 @@ -702,6 +702,78 @@ + + + + Point filter + + + + + + + 0 + 0 + + + + Filter point centers prior to pose estimation. + + + Enable + + + + + + + + 0 + 0 + + + + 0 + + + 400 + + + 1 + + + 10 + + + Qt::Horizontal + + + + + + + + 0 + 0 + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + QAbstractSpinBox::NoButtons + + + 999.990000000000009 + + + + + + @@ -1463,23 +1535,22 @@ Don't roll or change position. fps_spin use_mjpeg fov - dynamic_pose init_phase_timeout - blob_color + dynamic_pose camera_settings + blob_color auto_threshold threshold_slider mindiam_spin maxdiam_spin + enable_point_filter + point_filter_slider + point_filter_label model_tabs clip_tlength_spin clip_theight_spin clip_bheight_spin clip_blength_spin - tx_spin - ty_spin - tz_spin - tcalib_button cap_length_spin cap_width_spin cap_height_spin @@ -1487,8 +1558,12 @@ Don't roll or change position. m1y_spin m1z_spin m2x_spin - m2z_spin m2y_spin + m2z_spin + tx_spin + ty_spin + tz_spin + tcalib_button 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 #include @@ -74,6 +75,7 @@ private: std::atomic point_count { 0 }; std::atomic 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(), + 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't roll or change position. MJPEG compression + + Point filter + + + + Filter point centers prior to pose estimation. + + + + Enable + + pt_impl::TrackerDialog_PT 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-смещения. MJPEG compression + + Point filter + + + + Filter point centers prior to pose estimation. + + + + Enable + + pt_impl::TrackerDialog_PT 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't roll or change position. MJPEG compression + + Point filter + + + + Filter point centers prior to pose estimation. + + + + Enable + + pt_impl::TrackerDialog_PT 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't roll or change position. MJPEG compression + + Point filter + + + + Filter point centers prior to pose estimation. + + + + Enable + + pt_impl::TrackerDialog_PT 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 + +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 +#include + +namespace pt_point_filter_impl { + +using namespace numeric_types; +using PointOrder = std::array; + +class point_filter final +{ + PointOrder state_; + std::optional 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& 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& 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 #include @@ -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& projected_points, const PointModel& model, const pt_camera_info& info, int init_phase_timeout); + void track(const std::vector& 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 threshold_slider { b, "threshold-slider", { 128, 0, 255 } }; + value enable_point_filter{ b, "enable-point-filter", false }; + value point_filter_coefficient { b, "point-filter-coefficient", { 1.5, 0, 4 } }; + explicit pt_settings(const QString& name) : opts(name) {} }; -- cgit v1.2.3