pt: change extraction code

Point size is now specified by its radius.

Points are extracted in one pass, using multiple passes caused missed
detection for some users.

Circularity is now displayed as part of overlay.

Points are now sorted by circularity, which is our confidence metric.

Tracker code ignores additional points, keeping only those with the
highest confidence metric.

2 files changed, 73 insertions, 165 deletions

diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index 4fa70ccf..19d1bd7f 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -93,6 +93,10 @@ void Tracker_PT::run()
             QMutexLocker lock(&mutex);
 
             std::vector<cv::Vec2f> points = point_extractor.extract_points(frame);
+
+            // blobs are sorted in order of circularity
+            if (points.size() > PointModel::N_POINTS)
+                points.resize(PointModel::N_POINTS);
             
             bool success = points.size() == PointModel::N_POINTS;
             
diff --git a/ftnoir_tracker_pt/point_extractor.cpp b/ftnoir_tracker_pt/point_extractor.cpp
index c1dd9a54..fcdbbaed 100644
--- a/ftnoir_tracker_pt/point_extractor.cpp
+++ b/ftnoir_tracker_pt/point_extractor.cpp
@@ -28,215 +28,119 @@ std::vector<cv::Vec2f> PointExtractor::extract_points(cv::Mat& frame)
     cv::Mat frame_gray;
     cv::cvtColor(frame, frame_gray, cv::COLOR_RGB2GRAY);
 
-    int min_size = s.min_point_size;
-    int max_size = s.max_point_size;
+    const int region_size_min = s.min_point_size;
+    const int region_size_max = s.max_point_size;
     
-    unsigned int region_size_min = 3.14*min_size*min_size/4.0;
-    unsigned int region_size_max = 3.14*max_size*max_size/4.0;
-    
-    // testing indicates threshold difference of 45 from lowest to highest
-    // that's applicable to poor lighting conditions.
-    
-    static constexpr int diff = 20;
-    static constexpr int steps = 5;
-    static constexpr int successes = 5;
-    
-    int thres = s.threshold;
-    
-    struct blob {
-        std::vector<cv::Vec2d> pos;
-        std::vector<double> confids;
-        std::vector<double> areas;
-        
-        cv::Vec2d effective_pos() const
-        {
-            double x = 0;
-            double y = 0;
-            double norm = 0;
-            for (unsigned i = 0; i < pos.size(); i++)
-            {
-                const double w = confids[i] * areas[i];
-                x += pos[i][0] * w;
-                y += pos[i][1] * w;
-                norm += w;
-            }
-            cv::Vec2d ret(x, y);
-            ret *= 1./norm;
-            return ret;
-        }
-
-        double effective_area() const
-        {
-            double norm = 0, ret = 0;
-            for (unsigned i = 0; i < areas.size(); i++)
-            {
-                const double w = confids[i];
-                norm += w;
-                ret += w * areas[i];
-            }
-            return ret/norm;
-        }
-    };
+    const int thres = s.threshold;
     
     struct simple_blob
     {
-        double radius_2;
+        double radius;
         cv::Vec2d pos;
         double confid;
         bool taken;
         double area;
-        simple_blob(double radius, const cv::Vec2d& pos, double confid, double area) : radius_2(radius*radius), pos(pos), confid(confid), taken(false), area(area)
+        simple_blob(double radius, const cv::Vec2d& pos, double confid, double area) : radius(radius), pos(pos), confid(confid), taken(false), area(area)
         {
             //qDebug() << "radius" << radius << "pos" << pos[0] << pos[1] << "confid" << confid;
         }
         bool inside(const simple_blob& other)
         {
             cv::Vec2d tmp = pos - other.pos;
-            return tmp.dot(tmp) < radius_2;
-        }
-        static std::vector<blob> merge(cv::Mat& frame, std::vector<simple_blob>& blobs)
-        {
-#ifdef DEBUG_EXTRACTION
-            static Timer t;
-            bool debug = t.elapsed_ms() > 100;
-            if (debug) t.start();
-#endif
-            
-            std::vector<blob> ret;
-            for (unsigned i = 0; i < blobs.size(); i++)
-            {
-                auto& b = blobs[i];
-                if (b.taken)
-                    continue;
-                b.taken = true;
-                blob b_;
-                b_.pos.push_back(b.pos);
-                b_.confids.push_back(b.confid);
-                b_.areas.push_back(b.area);
-                
-                for (unsigned j = i+1; j < blobs.size(); j++)
-                {
-                    auto& b2 = blobs[j];
-                    if (b2.taken)
-                        continue;
-                    if (b.inside(b2) || b2.inside(b))
-                    {
-                        b2.taken = true;
-                        b_.pos.push_back(b2.pos);
-                        b_.confids.push_back(b2.confid);
-                        b_.areas.push_back(b2.area);
-                    }
-                }
-                if (b_.pos.size() >= successes)
-                    ret.push_back(b_);
-
-                char buf[64];
-                sprintf(buf, "%d%% %d px", (int)(b_.pos.size()*100/successes), (int)(2.*sqrt(b_.effective_area()) / sqrt(3.14)));
-                const auto pos = b_.effective_pos();
-                cv::putText(frame, buf, cv::Point(pos[0]+30, pos[1]+20), cv::FONT_HERSHEY_DUPLEX, 1, cv::Scalar(0, 0, 255), 1);
-            }
-#ifdef DEBUG_EXTRACTION
-            if (debug)
-            {
-                double diff = 0;
-                for (unsigned j = 0; j < ret.size(); j++)
-                {
-                    auto& b = ret[j];
-                    cv::Vec2d pos = b.effective_pos();
-                    for (unsigned i = 0; i < b.pos.size(); i++)
-                    {
-                        auto tmp = pos - b.pos[i];
-                        diff += std::abs(tmp.dot(tmp));
-                    }
-                }
-                qDebug() << "diff" << diff;
-            }
-#endif
-            return ret;
+            return sqrt(tmp.dot(tmp)) < radius;
         }
     };
     
     // mask for everything that passes the threshold (or: the upper threshold of the hysteresis)
     cv::Mat frame_bin = cv::Mat::zeros(H, W, CV_8U);
     
-    const int min = std::max(0, thres - diff/2);
-    const int max = std::min(255, thres + diff/2);
-    const int step = std::max(1, diff / steps);
-    
     std::vector<simple_blob> blobs;
-    
-    // this code is based on OpenCV SimpleBlobDetector
-    for (int i = min; i < max; i += step)
+    std::vector<std::vector<cv::Point>> contours;
     {
         cv::Mat frame_bin_;
-        cv::threshold(frame_gray, frame_bin_, i, 255, cv::THRESH_BINARY);
+        cv::threshold(frame_gray, frame_bin_, thres, 255, cv::THRESH_BINARY);
         frame_bin.setTo(170, frame_bin_);
-        
-        std::vector<std::vector<cv::Point>> contours;
         cv::findContours(frame_bin_, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
-        
-        int cnt = 0;
-        
-        for (auto& c : contours)
+    }
+
+    int cnt = 0;
+
+    for (auto& c : contours)
+    {
+        if (cnt++ > 30)
+            break;
+
+        const auto m = cv::moments(cv::Mat(c));
+        const double area = m.m00;
+        if (area == 0.)
+            continue;
+        const cv::Vec2d pos(m.m10 / m.m00, m.m01 / m.m00);
+
+        double radius;
+// following based on OpenCV SimpleBlobDetector
         {
-            if (cnt++ > 30)
-                break;
-            
-            auto m = cv::moments(cv::Mat(c));
-            const double area = m.m00;
-            if (area == 0.)
-                continue;
-            cv::Vec2d pos(m.m10 / m.m00, m.m01 / m.m00);
-            if (area < region_size_min || area > region_size_max)
-                continue;
-            
-            double radius = 0;
-            
+            std::vector<double> dists;
             for (auto& k : c)
             {
-                cv::Vec2d pos_(k.x, k.y);
-                cv::Vec2d tmp = pos_ - pos;
-                radius = std::max(radius, sqrt(1e-2 + tmp.dot(tmp)));
+                dists.push_back(cv::norm(pos - cv::Vec2d(k.x, k.y)));
             }
-            double confid = 1;
+            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)
             {
-                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)
-                {
-                    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;
-                }
+                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;
             }
-            blobs.push_back(simple_blob(radius, pos, confid, area));
         }
+// 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(simple_blob(radius, pos, confid, area));
     }
     
     // clear old points
 	points.clear();
+
+    using b = const simple_blob;
+    std::sort(blobs.begin(), blobs.end(), [](b& b1, b& b2) {return b1.confid > b2.confid;});
     
-    for (auto& b : simple_blob::merge(frame, blobs))
+    for (auto& b : blobs)
     {
-        auto pos = b.effective_pos();
-        cv::Vec2f p((pos[0] - W/2)/W, -(pos[1] - H/2)/W);
+        cv::Vec2f p((b.pos[0] - W/2)/W, -(b.pos[1] - H/2)/W);
         points.push_back(p);
     }
     
+    // draw output image
     std::vector<cv::Mat> channels_;
     cv::split(frame, channels_);
-    // draw output image
-    cv::Mat frame_bin_ = frame_bin * .5;
     std::vector<cv::Mat> channels;
-    channels.push_back(channels_[0] + frame_bin_);
-    channels.push_back(channels_[1] - frame_bin_);
-    channels.push_back(channels_[2] - frame_bin_);
-    cv::merge(channels, frame);
+    {
+        cv::Mat frame_bin__ = frame_bin * .5;
+        channels.push_back(channels_[0] + frame_bin__);
+        channels.push_back(channels_[1] - frame_bin__);
+        channels.push_back(channels_[2] - frame_bin__);
+        cv::merge(channels, frame);
+    }
 
     return points;
 }