tracker/pt: replace point extractor with original one by Patrick Ruoff

The new extractor we've been using doesn't take into account brightness
at all. All contours give is the ability to sort points by circularity.

v2:

Change the auto threshold point size range to 2->7 pixels radius.
Issue: #389

v3: sort by radius instead

2 files changed, 71 insertions, 70 deletions

diff --git a/tracker-pt/point_extractor.cpp b/tracker-pt/point_extractor.cpp
index e3472391..23fe7a38 100644
--- a/tracker-pt/point_extractor.cpp
+++ b/tracker-pt/point_extractor.cpp
@@ -13,7 +13,11 @@
 #   include "opentrack-compat/timer.hpp"
 #endif
 
+#include <opencv2/highgui.hpp>
+
 #include <cmath>
+#include <algorithm>
+#include <cinttypes>
 
 PointExtractor::PointExtractor()
 {
@@ -21,9 +25,7 @@ PointExtractor::PointExtractor()
     points.reserve(max_blobs);
 }
 
-using vec2 = pt_types::vec2;
-
-const std::vector<vec2>& PointExtractor::extract_points(cv::Mat& frame)
+const std::vector<PointExtractor::vec2>& PointExtractor::extract_points(cv::Mat& frame)
 {
     const int W = frame.cols;
     const int H = frame.rows;
@@ -31,7 +33,8 @@ const std::vector<vec2>& PointExtractor::extract_points(cv::Mat& frame)
     if (frame_gray.rows != frame.rows || frame_gray.cols != frame.cols)
     {
         frame_gray = cv::Mat(frame.rows, frame.cols, CV_8U);
-        frame_bin = cv::Mat(frame.rows, frame.cols, CV_8U);;
+        frame_bin = cv::Mat(frame.rows, frame.cols, CV_8U);
+        frame_blobs = cv::Mat(frame.rows, frame.cols, CV_8U);
     }
 
     // convert to grayscale
@@ -40,14 +43,10 @@ const std::vector<vec2>& PointExtractor::extract_points(cv::Mat& frame)
     const double region_size_min = s.min_point_size;
     const double region_size_max = s.max_point_size;
 
-    const int thres = s.threshold;
-
-    contours.clear();
-
     if (!s.auto_threshold)
     {
+        const int thres = s.threshold;
         cv::threshold(frame_gray, frame_bin, thres, 255, cv::THRESH_BINARY);
-        cv::findContours(frame_bin, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
     }
     else
     {
@@ -59,87 +58,90 @@ const std::vector<vec2>& PointExtractor::extract_points(cv::Mat& frame)
                      std::vector<float> { 0, 256 },
                      false);
         const int sz = hist.cols * hist.rows;
-        int val = 0;
+        int thres = s.threshold;
         int cnt = 0;
-        constexpr int min_pixels = int(10 * 10 * 3 * pi);
-        const int pixels_to_include = std::max<int>(0, min_pixels * s.threshold * s.threshold / (256 * 256));
+        constexpr double min_pixels = 2 * 2 * 3 * pi;
+        constexpr double max_pixels = 7 * 7 * 3 * pi;
+        constexpr double range_pixels = max_pixels - min_pixels;
+        const int pixels_to_include = std::max<int>(0, int(min_pixels + range_pixels * s.threshold / 256));
         auto ptr = reinterpret_cast<const float*>(hist.ptr(0));
-        for (int i = sz-1; i >= 0; i--)
+        for (int i = sz-1; i > 0; i--)
         {
             cnt += ptr[i];
             if (cnt >= pixels_to_include)
             {
-                val = i;
+                thres = i;
                 break;
             }
         }
         //val *= 240./256.;
         //qDebug() << "val" << val;
 
-        cv::threshold(frame_gray, frame_bin, val, 255, CV_THRESH_BINARY);
-        cv::findContours(frame_bin, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
+        cv::threshold(frame_gray, frame_bin, thres, 255, CV_THRESH_BINARY);
     }
 
     blobs.clear();
+    frame_gray.copyTo(frame_blobs);
 
-    for (auto& c : contours)
+    unsigned idx = 0;
+    for (int y=0; y < frame_gray.rows; y++)
     {
-        using std::fabs;
+        if (idx > max_blobs) break;
 
-        const auto m = cv::moments(cv::Mat(c));
-
-        if (fabs(m.m00) < 1e-3)
-            continue;
-
-        const cv::Vec2d pos(m.m10 / m.m00, m.m01 / m.m00);
-
-        double radius;
-// following based on OpenCV SimpleBlobDetector
+        const unsigned char* ptr_bin = frame_bin.ptr(y);
+        for (int x=0; x < frame_gray.cols; x++)
         {
-            std::vector<double> dists;
-            for (auto& k : c)
+            if (idx > max_blobs) break;
+
+            if (ptr_bin[x] != 255)
+                continue;
+            idx = blobs.size();
+            cv::Rect rect;
+            cv::floodFill(frame_blobs,
+                          cv::Point(x,y),
+                          cv::Scalar(idx),
+                          &rect,
+                          cv::Scalar(0),
+                          cv::Scalar(0),
+                          4);
+            long m00 = 0;
+            long m10 = 0;
+            long m01 = 0;
+            long cnt = 0;
+            for (int i=rect.y; i < (rect.y+rect.height); i++)
             {
-                dists.push_back(cv::norm(pos - cv::Vec2d(k.x, k.y)));
+                const unsigned char* ptr_blobs = frame_blobs.ptr(i);
+                const unsigned char* ptr_gray = frame_gray.ptr(i);
+                unsigned char* ptr_bin = frame_bin.ptr(i);
+                for (int j=rect.x; j < (rect.x+rect.width); j++)
+                {
+                    if (ptr_blobs[j] != idx) continue;
+                    ptr_bin[j] = 0;
+                    const long val = ptr_gray[j];
+                    m00 += val;
+                    m01 += i * val;
+                    m10 += j * val;
+                    cnt++;
+                }
             }
-            std::sort(dists.begin(), dists.end());
-            radius = (dists[(dists.size() - 1)/2] + dists[dists.size()/2])/2;
-        }
-
-        if (radius < region_size_min || radius > region_size_max)
-            continue;
-
-        double confid = 1;
-        {
-            double denominator = std::sqrt(std::pow(2 * m.mu11, 2) + std::pow(m.mu20 - m.mu02, 2));
-            const double eps = 1e-2;
-            if (denominator > eps)
+            if (m00 > 0)
             {
-                double cosmin = (m.mu20 - m.mu02) / denominator;
-                double sinmin = 2 * m.mu11 / denominator;
-                double cosmax = -cosmin;
-                double sinmax = -sinmin;
-
-                double imin = 0.5 * (m.mu20 + m.mu02) - 0.5 * (m.mu20 - m.mu02) * cosmin - m.mu11 * sinmin;
-                double imax = 0.5 * (m.mu20 + m.mu02) - 0.5 * (m.mu20 - m.mu02) * cosmax - m.mu11 * sinmax;
-                confid = imin / imax;
+                const double radius = std::sqrt(cnt / pi);
+                if (radius > region_size_max || radius < region_size_min)
+                    continue;
+                const double norm = double(m00);
+                blob b(radius, cv::Vec2d(m10 / norm, m01 / norm));
+                blobs.push_back(b);
+                {
+                    char buf[64];
+                    sprintf(buf, "%.2fpx", radius);
+                    cv::putText(frame, buf, cv::Point(b.pos[0]+30, b.pos[1]+20), cv::FONT_HERSHEY_DUPLEX, 1, cv::Scalar(0, 0, 255), 1);
+                }
             }
         }
-// end SimpleBlobDetector
-
-        {
-            char buf[64];
-            sprintf(buf, "%.2fpx %.2fc", radius, confid);
-            cv::putText(frame, buf, cv::Point(pos[0]+30, pos[1]+20), cv::FONT_HERSHEY_DUPLEX, 1, cv::Scalar(0, 0, 255), 1);
-        }
-
-        blobs.push_back(blob(radius, pos, confid));
-
-        if (blobs.size() == max_blobs)
-            break;
     }
 
-    using b = const blob;
-    std::sort(blobs.begin(), blobs.end(), [](b& b1, b& b2) {return b1.confid > b2.confid;});
+    std::sort(blobs.begin(), blobs.end(), [](const blob& b1, const blob& b2) -> bool { return b2.radius < b1.radius; });
 
     QMutexLocker l(&mtx);
     points.clear();
diff --git a/tracker-pt/point_extractor.h b/tracker-pt/point_extractor.h
index 16740b91..478418e6 100644
--- a/tracker-pt/point_extractor.h
+++ b/tracker-pt/point_extractor.h
@@ -37,22 +37,21 @@ private:
     cv::Mat frame_gray;
     cv::Mat frame_bin;
     cv::Mat hist;
+    cv::Mat frame_blobs;
 
-    enum { max_blobs = 16 };
+    static constexpr int max_blobs = 16;
 
     struct blob
     {
         double radius;
-        cv::Vec2d pos;
-        double confid;
-        blob(double radius, const cv::Vec2d& pos, double confid) : radius(radius), pos(pos), confid(confid)
+        vec2 pos;
+        blob(double radius, const cv::Vec2d& pos) : radius(radius), pos(pos)
         {
-            //qDebug() << "radius" << radius << "pos" << pos[0] << pos[1] << "confid" << confid;
+            //qDebug() << "radius" << radius << "pos" << pos[0] << pos[1];
         }
     };
 
     std::vector<blob> blobs;
-    std::vector<std::vector<cv::Point>> contours;
 };
 
 #endif //POINTEXTRACTOR_H