tracker/pt: isolate point extractor and image type

Issue: #718

12 files changed, 261 insertions, 65 deletions

diff --git a/tracker-pt/camera.cpp b/tracker-pt/camera.cpp
index bbc37cb1..20acf27a 100644
--- a/tracker-pt/camera.cpp
+++ b/tracker-pt/camera.cpp
@@ -6,13 +6,17 @@
  */
 
 #include "camera.h"
+#include "frame.hpp"
+
 #include "compat/sleep.hpp"
 #include "compat/camera-names.hpp"
 #include "compat/math-imports.hpp"
 
+#include <opencv2/imgproc.hpp>
+
 #include "cv/video-property-page.hpp"
 
-constexpr double Camera::dt_eps;
+using namespace pt_module;
 
 Camera::Camera(const QString& module_name) : dt_mean(0), fov(0), s(module_name) {}
 
@@ -45,8 +49,10 @@ Camera::result Camera::get_info() const
     return result(true, cam_info);
 }
 
-Camera::result Camera::get_frame(cv::Mat& frame)
+Camera::result Camera::get_frame(pt_frame& frame_)
 {
+    cv::Mat& frame = frame_.as<Frame>()->mat;
+
     const bool new_frame = _get_frame(frame);
 
     if (new_frame)
@@ -160,3 +166,4 @@ void Camera::camera_deleter::operator()(cv::VideoCapture* cap)
         delete cap;
     }
 }
+
diff --git a/tracker-pt/camera.h b/tracker-pt/camera.h
index f5fd8f81..35e887ad 100644
--- a/tracker-pt/camera.h
+++ b/tracker-pt/camera.h
@@ -13,13 +13,17 @@
 #include "compat/util.hpp"
 #include "compat/timer.hpp"
 
+#include <functional>
+#include <memory>
+#include <tuple>
+
 #include <opencv2/core.hpp>
 #include <opencv2/videoio.hpp>
 
-#include <memory>
-#include <tuple>
 #include <QString>
 
+namespace pt_module {
+
 struct Camera final : pt_camera
 {
     Camera(const QString& module_name);
@@ -27,7 +31,7 @@ struct Camera final : pt_camera
     pt_camera_open_status start(int idx, int fps, int res_x, int res_y) override;
     void stop() override;
 
-    result get_frame(cv::Mat& frame) override;
+    result get_frame(pt_frame& Frame) override;
     result get_info() const override;
 
     pt_camera_info get_desired() const override { return cam_desired; }
@@ -37,11 +41,10 @@ struct Camera final : pt_camera
     operator bool() const override { return cap && cap->isOpened(); }
 
     void set_fov(double value) override { fov = value; }
-
     void show_camera_settings() override;
 
 private:
-    warn_result_unused bool _get_frame(cv::Mat& frame);
+    warn_result_unused bool _get_frame(cv::Mat& Frame);
 
     double dt_mean, fov;
 
@@ -62,5 +65,7 @@ private:
 
     pt_settings s;
 
-    static constexpr double dt_eps = 1./384;
+    static constexpr inline double dt_eps = 1./384;
 };
+
+} // ns pt_module
diff --git a/tracker-pt/frame.cpp b/tracker-pt/frame.cpp
new file mode 100644
index 00000000..ca72bd16
--- /dev/null
+++ b/tracker-pt/frame.cpp
@@ -0,0 +1,85 @@
+#include "frame.hpp"
+
+#include <cstring>
+#include <tuple>
+
+#include <opencv2/imgproc.hpp>
+
+#pragma strict_gs_check(on)
+
+using namespace pt_module;
+
+Preview& Preview::operator=(const pt_frame& frame_)
+{
+    const cv::Mat& frame = frame_.as_const<const Frame>()->mat;
+    const int w = frame.cols, h = frame.rows;
+    ensure_size(frame_copy, w, h, CV_8UC3);
+
+    if (frame.channels() != 3)
+    {
+        once_only(qDebug() << "tracker/pt: camera frame depth: 3 !=" << frame.channels());
+        return *this;
+    }
+
+    const bool need_resize = w != frame_out.cols || h != frame_out.rows;
+    if (need_resize)
+        cv::resize(frame, frame_copy, cv::Size(frame_resize.cols, frame_resize.rows), 0, 0, cv::INTER_NEAREST);
+    else
+        frame.copyTo(frame_copy);
+
+    return *this;
+}
+
+Preview::Preview(int w, int h)
+{
+    ensure_size(frame_resize, w, h, CV_8UC4);
+    ensure_size(frame_out, w, h, CV_8UC4);
+
+    frame_out.setTo(cv::Scalar(0, 0, 0, 0));
+}
+
+QImage Preview::get_bitmap()
+{
+    int stride = frame_out.step.p[0];
+
+    if (stride < 64 || stride < frame_out.cols * 4)
+    {
+        once_only(qDebug() << "bad stride" << stride
+                           << "for bitmap size" << frame_copy.cols << frame_copy.rows);
+        return QImage();
+    }
+
+    cv::cvtColor(frame_copy, frame_out, cv::COLOR_BGR2BGRA);
+
+    return QImage((const unsigned char*) frame_out.data,
+                  frame_out.cols, frame_out.rows,
+                  stride,
+                  QImage::Format_ARGB32);
+}
+
+void Preview::draw_head_center(double x, double y)
+{
+    double px_, py_;
+
+    std::tie(px_, py_) = to_pixel_pos(x, y, frame_copy.cols, frame_copy.rows);
+
+    int px = iround(px_), py = iround(py_);
+
+    constexpr int len = 9;
+
+    static const cv::Scalar color(0, 255, 255);
+    cv::line(frame_copy,
+             cv::Point(px - len, py),
+             cv::Point(px + len, py),
+             color, 1);
+    cv::line(frame_copy,
+             cv::Point(px, py - len),
+             cv::Point(px, py + len),
+             color, 1);
+}
+
+void Preview::ensure_size(cv::Mat& frame, int w, int h, int type)
+{
+    if (frame.cols != w || frame.rows != h)
+        frame = cv::Mat(h, w, type);
+}
diff --git a/tracker-pt/frame.hpp b/tracker-pt/frame.hpp
new file mode 100644
index 00000000..f8c33ebe
--- /dev/null
+++ b/tracker-pt/frame.hpp
@@ -0,0 +1,36 @@
+#pragma once
+
+#include "pt-api.hpp"
+
+#include <opencv2/core.hpp>
+#include <QImage>
+
+namespace pt_module {
+
+struct Frame final : pt_frame
+{
+    cv::Mat mat;
+
+    operator const cv::Mat&() const& { return mat; }
+    operator cv::Mat&() & { return mat; }
+};
+
+struct Preview final : pt_preview
+{
+    Preview(int w, int h);
+
+    Preview& operator=(const pt_frame& frame) override;
+    QImage get_bitmap() override;
+    void draw_head_center(double x, double y) override;
+
+    operator cv::Mat&() { return frame_copy; }
+    operator cv::Mat const&() const { return frame_copy; }
+
+private:
+    static void ensure_size(cv::Mat& frame, int w, int h, int type);
+
+    bool fresh = true;
+    cv::Mat frame_copy, frame_color, frame_resize, frame_out;
+};
+
+} // ns pt_module
diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp
index b2d9bcbe..bded07d3 100644
--- a/tracker-pt/ftnoir_tracker_pt.cpp
+++ b/tracker-pt/ftnoir_tracker_pt.cpp
@@ -24,10 +24,14 @@
 
 using namespace types;
 
+static constexpr inline int preview_width = 320, preview_height = 240;
+
 Tracker_PT::Tracker_PT(const pt_runtime_traits& traits) :
     s(traits.get_module_name()),
     point_extractor(std::move(traits.make_point_extractor())),
-    camera(std::move(traits.make_camera()))
+    camera(std::move(traits.make_camera())),
+    frame(std::move(traits.make_frame())),
+    preview_frame(std::move(traits.make_preview(preview_width, preview_height)))
 {
     cv::setBreakOnError(true);
 
@@ -64,16 +68,14 @@ void Tracker_PT::run()
             QMutexLocker l(&camera_mtx);
 
             if (camera)
-                std::tie(new_frame, cam_info) = camera->get_frame(frame);
+                std::tie(new_frame, cam_info) = camera->get_frame(*frame);
         }
 
         if (new_frame)
         {
-            cv::resize(frame, preview_frame,
-                       cv::Size(preview_size.width(), preview_size.height()),
-                       0, 0, cv::INTER_NEAREST);
+            *preview_frame = *frame;
 
-            point_extractor->extract_points(frame, preview_frame, points);
+            point_extractor->extract_points(*frame, *preview_frame, points);
             point_count = points.size();
 
             const double fx = cam_info.get_focal_length();
@@ -100,26 +102,11 @@ void Tracker_PT::run()
                 Affine X_MH(mat33::eye(), vec3(s.t_MH_x, s.t_MH_y, s.t_MH_z));
                 Affine X_GH = X_CM * X_MH;
                 vec3 p = X_GH.t; // head (center?) position in global space
-                vec2 p_((p[0] * fx) / p[2], (p[1] * fx) / p[2]);  // projected to screen
-
-                constexpr int len = 9;
-
-                cv::Point p2(iround(p_[0] * preview_frame.cols + preview_frame.cols/2),
-                             iround(-p_[1] * preview_frame.cols + preview_frame.rows/2));
-                static const cv::Scalar color(0, 255, 255);
-                cv::line(preview_frame,
-                         cv::Point(p2.x - len, p2.y),
-                         cv::Point(p2.x + len, p2.y),
-                         color,
-                         1);
-                cv::line(preview_frame,
-                         cv::Point(p2.x, p2.y - len),
-                         cv::Point(p2.x, p2.y + len),
-                         color,
-                         1);
+
+                preview_frame->draw_head_center((p[0] * fx) / p[2], (p[1] * fx) / p[2]);
             }
 
-            video_widget->update_image(preview_frame);
+            video_widget->update_image(preview_frame->get_bitmap());
         }
     }
     qDebug() << "pt: thread stopped";
@@ -137,7 +124,6 @@ void Tracker_PT::maybe_reopen_camera()
     case cam_open_error:
         break;
     case cam_open_ok_change:
-        frame = cv::Mat();
         break;
     case cam_open_ok_no_change:
         break;
@@ -155,9 +141,6 @@ module_status Tracker_PT::start_tracker(QFrame* video_frame)
     //video_frame->setAttribute(Qt::WA_NativeWindow);
     preview_size = video_frame->size();
 
-    preview_frame = cv::Mat(video_frame->height(), video_frame->width(), CV_8UC3);
-    preview_frame.setTo(cv::Scalar(0, 0, 0));
-
     video_widget = std::make_unique<cv_video_widget>(video_frame);
     layout = std::make_unique<QHBoxLayout>(video_frame);
     layout->setContentsMargins(0, 0, 0, 0);
diff --git a/tracker-pt/ftnoir_tracker_pt.h b/tracker-pt/ftnoir_tracker_pt.h
index 9cf14cfd..deddeea8 100644
--- a/tracker-pt/ftnoir_tracker_pt.h
+++ b/tracker-pt/ftnoir_tracker_pt.h
@@ -26,10 +26,11 @@
 #include <QThread>
 #include <QMutex>
 #include <QLayout>
+#include <QTimer>
 
 class TrackerDialog_PT;
 
-namespace pt_impl {
+namespace pt_module {
 
 using namespace types;
 
@@ -67,8 +68,9 @@ private:
     std::unique_ptr<cv_video_widget> video_widget;
     std::unique_ptr<QLayout> layout;
 
-    cv::Mat frame, preview_frame;
     std::vector<vec2> points;
+    std::unique_ptr<pt_frame> frame;
+    std::unique_ptr<pt_preview> preview_frame;
 
     QSize preview_size;
 
@@ -81,4 +83,4 @@ private:
 
 } // ns pt_impl
 
-using pt_impl::Tracker_PT;
+using pt_module::Tracker_PT;
diff --git a/tracker-pt/module.cpp b/tracker-pt/module.cpp
index d0f097b9..d977aa16 100644
--- a/tracker-pt/module.cpp
+++ b/tracker-pt/module.cpp
@@ -2,6 +2,7 @@
 #include "api/plugin-api.hpp"
 
 #include "camera.h"
+#include "frame.hpp"
 #include "point_extractor.h"
 #include "ftnoir_tracker_pt_dialog.h"
 
@@ -11,6 +12,8 @@
 
 static const QString module_name = "tracker-pt";
 
+using namespace pt_module;
+
 struct pt_module_traits final : pt_runtime_traits
 {
     std::unique_ptr<pt_camera> make_camera() const override
@@ -27,24 +30,38 @@ struct pt_module_traits final : pt_runtime_traits
     {
         return module_name;
     }
+
+    std::unique_ptr<pt_frame> make_frame() const override
+    {
+        return std::unique_ptr<pt_frame>(new Frame);
+    }
+
+    std::unique_ptr<pt_preview> make_preview(int w, int h) const override
+    {
+        return std::unique_ptr<pt_preview>(new Preview(w, h));
+    }
 };
 
-struct pt_tracker_module : Tracker_PT
+struct tracker_pt : Tracker_PT
 {
-    pt_tracker_module() : Tracker_PT(pt_module_traits())
+    tracker_pt() : Tracker_PT(pt_module_traits())
     {
     }
 };
 
-struct pt_tracker_dialog_module : TrackerDialog_PT
+struct dialog_pt : TrackerDialog_PT
 {
-    pt_tracker_dialog_module() : TrackerDialog_PT(module_name) {}
+    dialog_pt() : TrackerDialog_PT(module_name) {}
 };
 
-class pt_module_metadata : public Metadata
+class metadata_pt : public Metadata
 {
     QString name() { return _("PointTracker 1.1"); }
     QIcon icon() { return QIcon(":/Resources/Logo_IR.png"); }
 };
 
-OPENTRACK_DECLARE_TRACKER(pt_tracker_module, pt_tracker_dialog_module, pt_module_metadata)
+// ns pt_module
+
+using namespace pt_module;
+
+OPENTRACK_DECLARE_TRACKER(tracker_pt, dialog_pt, metadata_pt)
diff --git a/tracker-pt/point_extractor.cpp b/tracker-pt/point_extractor.cpp
index 464c25db..0904d9cb 100644
--- a/tracker-pt/point_extractor.cpp
+++ b/tracker-pt/point_extractor.cpp
@@ -9,6 +9,8 @@
 #include "point_extractor.h"
 #include "compat/util.hpp"
 #include "point_tracker.h"
+#include "frame.hpp"
+
 #include <QDebug>
 
 #include "cv/numeric.hpp"
@@ -29,7 +31,7 @@
 #include <QDebug>
 
 using namespace types;
-using namespace pt_impl;
+using namespace pt_module;
 
 /*
 http://en.wikipedia.org/wiki/Mean-shift
@@ -195,8 +197,11 @@ void PointExtractor::threshold_image(const cv::Mat& frame_gray, cv::Mat1b& outpu
     }
 }
 
-void PointExtractor::extract_points(const cv::Mat& frame, cv::Mat& preview_frame, std::vector<vec2>& points)
+void PointExtractor::extract_points(const pt_frame& frame_, pt_preview& preview_frame_, std::vector<vec2>& points)
 {
+    const cv::Mat& frame = frame_.as_const<Frame>()->mat;
+    cv::Mat& preview_frame = *preview_frame_.as<Preview>();
+
     ensure_buffers(frame);
     color_to_grayscale(frame, frame_gray);
 
@@ -335,20 +340,17 @@ end:
         cv::circle(preview_frame, p, iround((b.radius + 3.3) * c_ * c_fract), circle_color, 1, cv::LINE_AA, fract_bits);
 
         char buf[16];
-        std::snprintf(buf, sizeof(buf), "%.2fpx", b.radius);
         buf[sizeof(buf)-1] = '\0';
+        std::snprintf(buf, sizeof(buf) - 1, "%.2fpx", b.radius);
 
         auto text_color = k >= PointModel::N_POINTS
                           ? cv::Scalar(160, 160, 160)
                           : cv::Scalar(0, 0, 255);
 
-        cv::putText(preview_frame,
-                    buf,
-                    cv::Point(iround(b.pos[0]*cx+offx), iround(b.pos[1]*cy+offy)),
-                cv::FONT_HERSHEY_PLAIN,
-                1,
-                text_color,
-                1);
+        cv::Point pos(iround(b.pos[0]*cx+offx), iround(b.pos[1]*cy+offy));
+        cv::putText(preview_frame, buf, pos,
+                    cv::FONT_HERSHEY_PLAIN,
+                    1, text_color, 1);
     }
 
     // End of mean shift code. At this point, blob positions are updated with hopefully less noisy less biased values.
@@ -359,7 +361,8 @@ end:
     {
         // note: H/W is equal to fx/fy
 
-        vec2 p((b.pos[0] - W/2)/W, -(b.pos[1] - H/2)/W);
+        vec2 p;
+        std::tie(p[0], p[1]) = to_screen_pos(b.pos[0], b.pos[1], W, H);
         points.push_back(p);
     }
 }
diff --git a/tracker-pt/point_extractor.h b/tracker-pt/point_extractor.h
index 266964e5..1b6f55a2 100644
--- a/tracker-pt/point_extractor.h
+++ b/tracker-pt/point_extractor.h
@@ -15,7 +15,7 @@
 #include <opencv2/core.hpp>
 #include <opencv2/imgproc.hpp>
 
-namespace pt_impl {
+namespace pt_module {
 
 using namespace types;
 
@@ -33,7 +33,7 @@ class PointExtractor final : public pt_point_extractor
 public:
     // extracts points from frame and draws some processing info into frame, if draw_output is set
     // dt: time since last call in seconds
-    void extract_points(const cv::Mat& frame, cv::Mat& preview_frame, std::vector<vec2>& points) override;
+    void extract_points(const pt_frame& frame, pt_preview& preview_frame, std::vector<vec2>& points) override;
     PointExtractor(const QString& module_name);
 private:
     static constexpr int max_blobs = 16;
@@ -57,4 +57,3 @@ private:
 
 } // ns impl
 
-using pt_impl::PointExtractor;
diff --git a/tracker-pt/point_tracker.h b/tracker-pt/point_tracker.h
index 816e02de..26677042 100644
--- a/tracker-pt/point_tracker.h
+++ b/tracker-pt/point_tracker.h
@@ -19,7 +19,7 @@
 #include <array>
 #include <QObject>
 
-namespace pt_impl {
+namespace pt_module {
 
 // ----------------------------------------------------------------------------
 // Describes a 3-point model
@@ -82,5 +82,5 @@ private:
 
 } // ns pt_impl
 
-using pt_impl::PointTracker;
-using pt_impl::PointModel;
+using pt_module::PointTracker;
+using pt_module::PointModel;
diff --git a/tracker-pt/pt-api.cpp b/tracker-pt/pt-api.cpp
index c11f372f..0a6c8e2b 100644
--- a/tracker-pt/pt-api.cpp
+++ b/tracker-pt/pt-api.cpp
@@ -60,3 +60,22 @@ double pt_point_extractor::threshold_radius_value(int w, int h, int threshold)
 
     return radius;
 }
+
+
+std::tuple<double, double> pt_pixel_pos_mixin::to_pixel_pos(double x, double y, int w, int h)
+{
+    return std::make_tuple(w*(x+.5), .5*(h - 2*y*w));
+}
+
+std::tuple<double, double> pt_pixel_pos_mixin::to_screen_pos(double px, double py, int w, int h)
+{
+    return std::make_tuple((px - w/2.)/w, -(py - h/2.)/w);
+}
+
+pt_frame::pt_frame()
+{
+}
+
+pt_frame::~pt_frame()
+{
+}
diff --git a/tracker-pt/pt-api.hpp b/tracker-pt/pt-api.hpp
index 79a47526..833a640e 100644
--- a/tracker-pt/pt-api.hpp
+++ b/tracker-pt/pt-api.hpp
@@ -5,10 +5,14 @@
 #include "cv/numeric.hpp"
 #include "options/options.hpp"
 
+#include <tuple>
+#include <type_traits>
 #include <memory>
 
 #include <opencv2/core.hpp>
 
+#include <QImage>
+
 struct OTR_PT_EXPORT pt_camera_info final
 {
     pt_camera_info();
@@ -34,6 +38,40 @@ enum pt_color_type
     pt_color_blue_only = 6,
 };
 
+struct OTR_PT_EXPORT pt_frame
+{
+    pt_frame();
+    virtual ~pt_frame();
+
+    template<typename t>
+    t* as() &
+    {
+        using u = std::decay_t<t>;
+        static_assert(std::is_convertible_v<u*, pt_frame*>, "must be derived from pt_image");
+
+        return static_cast<t*>(this);
+    }
+
+    template<typename t>
+    t const* as_const() const&
+    {
+        return const_cast<pt_frame*>(this)->as<const t>();
+    }
+};
+
+struct OTR_PT_EXPORT pt_pixel_pos_mixin
+{
+    static std::tuple<double, double> to_pixel_pos(double x, double y, int w, int h);
+    static std::tuple<double, double> to_screen_pos(double px, double py, int w, int h);
+};
+
+struct OTR_PT_EXPORT pt_preview : pt_frame, pt_pixel_pos_mixin
+{
+    virtual pt_preview& operator=(const pt_frame&) = 0;
+    virtual QImage get_bitmap() = 0;
+    virtual void draw_head_center(double x, double y) = 0;
+};
+
 struct OTR_PT_EXPORT pt_camera
 {
     using result = std::tuple<bool, pt_camera_info>;
@@ -43,7 +81,7 @@ struct OTR_PT_EXPORT pt_camera
 
     virtual warn_result_unused pt_camera_open_status start(int idx, int fps, int res_x, int res_y) = 0;
     virtual void stop() = 0;
-    virtual warn_result_unused result get_frame(cv::Mat& frame) = 0;
+    virtual warn_result_unused result get_frame(pt_frame& frame) = 0;
 
     virtual warn_result_unused result get_info() const = 0;
     virtual pt_camera_info get_desired() const = 0;
@@ -57,13 +95,13 @@ struct OTR_PT_EXPORT pt_camera
     virtual void show_camera_settings() = 0;
 };
 
-struct OTR_PT_EXPORT pt_point_extractor
+struct OTR_PT_EXPORT pt_point_extractor : pt_pixel_pos_mixin
 {
     using vec2 = types::vec2;
 
     pt_point_extractor();
     virtual ~pt_point_extractor();
-    virtual void extract_points(const cv::Mat& frame, cv::Mat& preview_frame, std::vector<vec2>& points) = 0;
+    virtual void extract_points(const pt_frame& image, pt_preview& preview_frame, std::vector<vec2>& points) = 0;
 
     static double threshold_radius_value(int w, int h, int threshold);
 };
@@ -115,5 +153,7 @@ struct OTR_PT_EXPORT pt_runtime_traits
 
     virtual std::unique_ptr<pt_camera> make_camera() const = 0;
     virtual std::unique_ptr<pt_point_extractor> make_point_extractor() const = 0;
+    virtual std::unique_ptr<pt_frame> make_frame() const = 0;
+    virtual std::unique_ptr<pt_preview> make_preview(int w, int h) const = 0;
     virtual QString get_module_name() const = 0;
 };