From ef1b350512efc5218f5b127a30ee497be89df972 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Mon, 9 May 2022 19:14:32 +0200
Subject: tracker/{pt-base,trackhat}: add variable deadzone factor

---
 tracker-pt/ftnoir_tracker_pt.cpp | 2 +-
 tracker-pt/point-filter.cpp      | 5 ++---
 tracker-pt/point-filter.hpp      | 2 +-
 tracker-pt/point_tracker.cpp     | 5 +++--
 tracker-pt/point_tracker.h       | 3 ++-
 tracker-pt/pt-api.hpp            | 1 +
 tracker-trackhat/trackhat.hpp    | 2 ++
 7 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp
index d43e861f..0f8495d9 100644
--- a/tracker-pt/ftnoir_tracker_pt.cpp
+++ b/tracker-pt/ftnoir_tracker_pt.cpp
@@ -101,7 +101,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, filter);
+                    point_tracker.track(points, PointModel(s), info, dynamic_pose_ms, filter, camera->deadzone_amount());
                     ever_success.store(true, std::memory_order_relaxed);
                 }
 
diff --git a/tracker-pt/point-filter.cpp b/tracker-pt/point-filter.cpp
index 9fe23a07..10448fe2 100644
--- a/tracker-pt/point-filter.cpp
+++ b/tracker-pt/point-filter.cpp
@@ -10,7 +10,7 @@ void point_filter::reset()
     t = std::nullopt;
 }
 
-const PointOrder& point_filter::operator()(const PointOrder& input)
+const PointOrder& point_filter::operator()(const PointOrder& input, f deadzone_amount)
 {
     using std::fmod;
     using std::sqrt;
@@ -32,7 +32,6 @@ const PointOrder& point_filter::operator()(const PointOrder& input)
     }
 
     constexpr auto E = (f)1.75;
-    constexpr int DZ = 10;
     const f limit = (f)*s.point_filter_limit;
     const f C = progn(
         constexpr int A = 1'000'000;
@@ -41,7 +40,7 @@ const PointOrder& point_filter::operator()(const PointOrder& input)
         return A * pow((f)10, (f)-log10_pos) * rest;
     );
 
-    f dist = 0, dz = DZ * (f)s.point_filter_deadzone / 800; // sqrt(640^2 + 480^2)
+    f dist = 0, dz = deadzone_amount * (f)s.point_filter_deadzone / 800; // sqrt(640^2 + 480^2)
 
     for (unsigned i = 0; i < 3; i++)
     {
diff --git a/tracker-pt/point-filter.hpp b/tracker-pt/point-filter.hpp
index cc86505b..c3c045dd 100644
--- a/tracker-pt/point-filter.hpp
+++ b/tracker-pt/point-filter.hpp
@@ -19,7 +19,7 @@ class point_filter final
 
 public:
     void reset();
-    const PointOrder& operator()(const PointOrder& input);
+    const PointOrder& operator()(const PointOrder& input, f deadzone_amount);
 
     explicit point_filter(const pt_settings& s);
     ~point_filter() = default;
diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp
index aca07d8e..39e96038 100644
--- a/tracker-pt/point_tracker.cpp
+++ b/tracker-pt/point_tracker.cpp
@@ -137,7 +137,8 @@ void PointTracker::track(const std::vector<vec2>& points,
                          const PointModel& model,
                          const pt_camera_info& info,
                          int init_phase_timeout,
-                         point_filter& filter)
+                         point_filter& filter,
+                         f deadzone_amount)
 {
     const f fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y);
     PointOrder order;
@@ -150,7 +151,7 @@ void PointTracker::track(const std::vector<vec2>& points,
     else
         order = find_correspondences_previous(points.data(), model, info);
 
-    if (POSIT(model, filter(order), fx) != -1)
+    if (POSIT(model, filter(order, deadzone_amount), fx) != -1)
     {
         init_phase = false;
         t.start();
diff --git a/tracker-pt/point_tracker.h b/tracker-pt/point_tracker.h
index 06388f49..7492b4eb 100644
--- a/tracker-pt/point_tracker.h
+++ b/tracker-pt/point_tracker.h
@@ -61,7 +61,8 @@ public:
                const PointModel& model,
                const pt_camera_info& info,
                int init_phase_timeout,
-               point_filter& filter);
+               point_filter& filter,
+               f deadzone_amount);
     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-api.hpp b/tracker-pt/pt-api.hpp
index 1a6444e5..15021ff3 100644
--- a/tracker-pt/pt-api.hpp
+++ b/tracker-pt/pt-api.hpp
@@ -98,6 +98,7 @@ struct pt_camera
 
     virtual void set_fov(f value) = 0;
     virtual void show_camera_settings() = 0;
+    virtual f deadzone_amount() const { return 1; }
 };
 
 struct pt_point_extractor : pt_pixel_pos_mixin
diff --git a/tracker-trackhat/trackhat.hpp b/tracker-trackhat/trackhat.hpp
index 6e228f9c..d94841d2 100644
--- a/tracker-trackhat/trackhat.hpp
+++ b/tracker-trackhat/trackhat.hpp
@@ -124,6 +124,8 @@ struct trackhat_camera final : pt_camera
     void set_fov(f value) override;
     void show_camera_settings() override;
 
+    f deadzone_amount() const override { return 10; }
+
     static constexpr int sensor_size = 2940;
     static constexpr int sensor_fov = 52;
     static constexpr int point_count = TRACK_HAT_NUMBER_OF_POINTS;
-- 
cgit v1.2.3