tracker/trackhat: implement extended point metadata

6 files changed, 113 insertions, 65 deletions

diff --git a/tracker-trackhat/camera.cpp b/tracker-trackhat/camera.cpp
index 9609132e..c744a9f9 100644
--- a/tracker-trackhat/camera.cpp
+++ b/tracker-trackhat/camera.cpp
@@ -43,27 +43,24 @@ pt_camera::result trackhat_camera::get_frame(pt_frame& frame_)
 {
     auto& ret = *frame_.as<trackhat_frame>();
     trackhat_frame frame;
+    trackHat_ExtendedPoints_t points = {};
 
     if (status < th_running || error_code != TH_SUCCESS)
-    {
-        if (status >= th_running)
-            qDebug() << "trackhat: disconnected, status" << (void*)error_code;
         goto error;
-    }
 
-    if (TH_ErrorCode error = trackHat_GetDetectedPoints(&device, &frame.points); error != TH_SUCCESS)
+    if (TH_ErrorCode error = trackHat_GetDetectedPointsExtended(&device, &points); error != TH_SUCCESS)
     {
         error_code = error;
         goto error;
     }
 
-    ret.points = frame.points;
+    ret.init_points(points, s.min_point_size, s.max_point_size);
     return {true, get_desired()};
 
 error:
-    if (error_code != TH_SUCCESS)
+    if (status >= th_running)
         qDebug() << "trackhat: error" << (void*)error_code;
-    ret.points = {};
+    ret.init_points(points, s.min_point_size, s.max_point_size);
     stop();
     return {false, get_desired()};
 }
@@ -79,6 +76,24 @@ error:
         }                                                       \
     } while (false)
 
+int trackhat_camera::init_regs()
+{
+    constexpr uint8_t regs[][3] = {
+        { 0x0c, 0x0f, 0xf0 },  // exposure lo
+        { 0x0c, 0x10, 0x7f },  // exposure hi
+        { 0x01, 0x01, 0x01 },  // bank1 sync
+    };
+
+    for (const auto& reg : regs)
+    {
+        trackHat_SetRegister_t r{reg[0], reg[1], reg[2]};
+        if (TH_ErrorCode error = trackHat_SetRegisterValue(&device, &r); error != TH_SUCCESS)
+            return error;
+    }
+
+    return TH_SUCCESS;
+}
+
 bool trackhat_camera::start(const pt_settings&)
 {
     int attempts = 0;
@@ -96,12 +111,9 @@ start:
 
     CHECK(trackHat_Initialize(&device)); status = th_init;
     CHECK(trackHat_DetectDevice(&device)); status = th_detect;
-    CHECK(trackHat_Connect(&device)); status = th_connect;
-    CHECK(trackHat_GetUptime(&device, &uptime)); status = th_running;
-
-#if 0
-    qDebug() << "trackhat start: device uptime" << uptime << "seconds";
-#endif
+    CHECK(trackHat_Connect(&device, TH_FRAME_EXTENDED)); status = th_connect;
+    CHECK(trackHat_GetUptime(&device, &uptime));
+    CHECK((TH_ErrorCode)init_regs()); status = th_running;
 
     return true;
 error:
@@ -133,4 +145,3 @@ void trackhat_camera::stop()
     status = th_noinit;
     device = {};
 }
-
diff --git a/tracker-trackhat/extractor.cpp b/tracker-trackhat/extractor.cpp
index aa4d38ef..58b9fd05 100644
--- a/tracker-trackhat/extractor.cpp
+++ b/tracker-trackhat/extractor.cpp
@@ -8,19 +8,14 @@ void trackhat_extractor::extract_points(const pt_frame& data,
 {
     points.clear();
     points.reserve(trackhat_camera::point_count);
-    trackHat_Points_t copy = data.as_const<trackhat_frame>()->points;
+    const auto& copy = data.as_const<trackhat_frame>()->points;
 
-    std::sort(std::begin(copy.m_point), std::end(copy.m_point),
-              [](trackHat_Point_t p1, trackHat_Point_t p2) {
-        return p1.m_brightness > p2.m_brightness;
-    });
-
-    for (const auto& pt : copy.m_point)
+    for (const auto& pt : copy)
     {
-        if (pt.m_brightness == 0)
+        if (!pt.ok)
             continue;
         constexpr int sz = trackhat_camera::sensor_size;
-        auto [ x, y ] = to_screen_pos(sz-1-pt.m_x, pt.m_y, sz, sz);
+        auto [ x, y ] = to_screen_pos(pt.x, pt.y, sz, sz);
         points.push_back({x, y});
     }
 }
diff --git a/tracker-trackhat/frame.cpp b/tracker-trackhat/frame.cpp
index 12d9d890..1ae3413f 100644
--- a/tracker-trackhat/frame.cpp
+++ b/tracker-trackhat/frame.cpp
@@ -59,32 +59,56 @@ void trackhat_preview::draw_center()
 
 void trackhat_preview::draw_points()
 {
-    for (unsigned i = 0; i < std::size(points.m_point); i++)
+    for (const auto& pt : points)
     {
-        const auto pt = points.m_point[i];
+        if (pt.brightness == 0)
+            continue;
+
+        constexpr int sz = trackhat_camera::sensor_size;
+        constexpr f scaling_factor = 5;
+        const int x = pt.x * frame_bgr.cols / sz, y = pt.y * frame_bgr.rows / sz;
+        const f dpi = (f)frame_bgr.cols / f(320);
+        const int W = std::max(1, iround(pt.W * frame_bgr.cols * scaling_factor / sz)),
+                  H = std::max(1, iround(pt.H * frame_bgr.rows * scaling_factor / sz));
+        const auto point_color = progn(double c = pt.brightness; return cv::Scalar{c, c, c};);
+        const auto outline_color = pt.ok
+                                   ? cv::Scalar{255, 255, 0}
+                                   : cv::Scalar{192, 192, 192};
+
+        cv::ellipse(frame_bgr, cv::Point{x, y}, {W, H},
+                    0, 0, 360, point_color, -1, cv::LINE_AA);
+        cv::ellipse(frame_bgr, {x, y}, {iround(W + 2*dpi), iround(H + 2*dpi)},
+                    0, 0, 360, outline_color, iround(dpi), cv::LINE_AA);
+    }
+}
+
+void trackhat_frame::init_points(trackHat_ExtendedPoints_t points_, double min_size, double max_size)
+{
+    std::sort(std::begin(points_.m_point), std::end(points_.m_point),
+        [](trackHat_ExtendedPoint_t p1, trackHat_ExtendedPoint_t p2) {
+      return p1.m_averageBrightness > p2.m_averageBrightness;
+    });
+
+    unsigned i = 0;
 
-        if (pt.m_brightness == 0)
+    for (const trackHat_ExtendedPoint_t& pt : points_.m_point)
+    {
+        if (pt.m_averageBrightness == 0)
             continue;
 
-        constexpr f sz = trackhat_camera::sensor_size;
-        int x = iround(std::clamp((f)pt.m_x, f(0), sz-1) * (f)frame_bgr.cols / sz),
-            y = iround(std::clamp((f)pt.m_y, f(0), sz-1) * (f)frame_bgr.rows / sz);
+        point p = {};
 
-        const f dpi = (f)frame_bgr.cols / f(320);
-        int c = (int)pt.m_brightness;
-        constexpr int point_size = 6;
-        auto outline_color = i < 3 ? cv::Scalar{255, 255, 0} : cv::Scalar{192, 192, 192};
-
-        cv::circle(frame_bgr,
-                   {x, y},
-                   iround(point_size * dpi),
-                   outline_color,
-                   iround(dpi), cv::LINE_AA);
-
-        cv::circle(frame_bgr,
-                   {x, y},
-                   iround((point_size-2) * dpi),
-                   cv::Scalar(c, c, c),
-                   -1);
+        const double radius = std::sqrt(pt.m_area) / std::sqrt(M_PI);
+        if (i < 3 && radius >= min_size && radius <= max_size)
+            p.ok = true;
+
+        p.radius = radius;
+        p.brightness = pt.m_averageBrightness;
+        p.W = std::max(1, pt.m_boundryRigth - pt.m_boundryLeft);
+        p.H = std::max(1, pt.m_boundryDown  - pt.m_boundryUp);
+        p.x = trackhat_camera::sensor_size-1-pt.m_coordinateX;
+        p.y = pt.m_coordinateY;
+
+        points[i++] = p;
     }
 }
diff --git a/tracker-trackhat/metadata.cpp b/tracker-trackhat/metadata.cpp
index 4145384e..a6dd12f8 100644
--- a/tracker-trackhat/metadata.cpp
+++ b/tracker-trackhat/metadata.cpp
@@ -1,8 +1,9 @@
 #include "metadata.hpp"
 #include "api/plugin-api.hpp"
+#include "trackhat.hpp"
 
 // XXX TODO
-static const QString bundle_name = QStringLiteral("tracker-pt");
+const QString trackhat_metadata::module_name = QStringLiteral("tracker-pt");
 
 pt_runtime_traits::pointer<pt_camera> trackhat_metadata::make_camera() const
 {
@@ -26,7 +27,7 @@ pt_runtime_traits::pointer<pt_preview> trackhat_metadata::make_preview(int w, in
 
 QString trackhat_metadata::get_module_name() const
 {
-    return bundle_name;
+    return trackhat_metadata::module_name;
 }
 
 trackhat_pt::trackhat_pt() :
@@ -34,7 +35,7 @@ trackhat_pt::trackhat_pt() :
 {
 }
 
-trackhat_pt_dialog::trackhat_pt_dialog() : TrackerDialog_PT(bundle_name)
+trackhat_pt_dialog::trackhat_pt_dialog() : TrackerDialog_PT(trackhat_metadata::module_name)
 {
 }
 
diff --git a/tracker-trackhat/metadata.hpp b/tracker-trackhat/metadata.hpp
index 9bbd8617..d8d053b1 100644
--- a/tracker-trackhat/metadata.hpp
+++ b/tracker-trackhat/metadata.hpp
@@ -26,4 +26,5 @@ class trackhat_module final : public Metadata
 public:
     QString name() override { return tr("TrackHat Point Tracker"); }
     QIcon icon() override { return QIcon(":/images/trackhat-64x64.png"); }
+    static const QString module_name;
 };
diff --git a/tracker-trackhat/trackhat.hpp b/tracker-trackhat/trackhat.hpp
index 26738839..b0096cc3 100644
--- a/tracker-trackhat/trackhat.hpp
+++ b/tracker-trackhat/trackhat.hpp
@@ -4,8 +4,32 @@
 #include "track_hat_driver.h"
 #include "compat/macros.hpp"
 
+#include <array>
 #include <opencv2/core.hpp>
 
+struct trackhat_metadata final : pt_runtime_traits
+{
+    pt_runtime_traits::pointer<pt_camera> make_camera() const override;
+    pt_runtime_traits::pointer<pt_point_extractor> make_point_extractor() const override;
+    pt_runtime_traits::pointer<pt_frame> make_frame() const override;
+    pt_runtime_traits::pointer<pt_preview> make_preview(int w, int h) const override;
+    QString get_module_name() const override;
+
+    OTR_DISABLE_MOVE_COPY(trackhat_metadata);
+
+    trackhat_metadata() = default;
+    ~trackhat_metadata() override = default;
+
+    static const QString module_name;
+};
+
+struct point
+{
+    double radius;
+    int brightness = 0, x, y, W, H;
+    bool ok = false;
+};
+
 struct trackhat_camera final : pt_camera
 {
     trackhat_camera();
@@ -15,6 +39,7 @@ struct trackhat_camera final : pt_camera
 
     bool start(const pt_settings& s) override;
     void stop() override;
+    [[nodiscard]] int init_regs();
 
     pt_camera::result get_frame(pt_frame& frame) override;
     pt_camera::result get_info() const override;
@@ -36,14 +61,16 @@ private:
     trackHat_Device_t device {};
     device_status status = th_noinit;
     TH_ErrorCode error_code = TH_SUCCESS;
+    pt_settings s{trackhat_metadata::module_name};
 };
 
 struct trackhat_frame final : pt_frame
 {
-    trackHat_Points_t points = {};
-
+    void init_points(trackHat_ExtendedPoints_t points, double min_size, double max_size);
     trackhat_frame() = default;
     ~trackhat_frame() override = default;
+
+    std::array<point, trackhat_camera::point_count> points;
 };
 
 struct trackhat_preview final : pt_preview
@@ -61,7 +88,7 @@ struct trackhat_preview final : pt_preview
 
     cv::Mat frame_bgr, frame_bgra;
     numeric_types::vec2 center{-1, -1};
-    trackHat_Points_t points = {};
+    std::array<point, trackhat_camera::point_count> points;
 };
 
 struct trackhat_extractor final : pt_point_extractor
@@ -72,18 +99,7 @@ struct trackhat_extractor final : pt_point_extractor
 
     trackhat_extractor() = default;
     ~trackhat_extractor() override = default;
-};
 
-struct trackhat_metadata final : pt_runtime_traits
-{
-    pt_runtime_traits::pointer<pt_camera> make_camera() const override;
-    pt_runtime_traits::pointer<pt_point_extractor> make_point_extractor() const override;
-    pt_runtime_traits::pointer<pt_frame> make_frame() const override;
-    pt_runtime_traits::pointer<pt_preview> make_preview(int w, int h) const override;
-    QString get_module_name() const override;
-
-    OTR_DISABLE_MOVE_COPY(trackhat_metadata);
-
-    trackhat_metadata() = default;
-    ~trackhat_metadata() override = default;
+private:
+    pt_settings s{trackhat_metadata::module_name};
 };