1 files changed, 91 insertions, 3 deletions

diff --git a/tracker-pt/point_extractor.cpp b/tracker-pt/point_extractor.cpp
index a9f431d9..e09af056 100644
--- a/tracker-pt/point_extractor.cpp
+++ b/tracker-pt/point_extractor.cpp
@@ -8,6 +8,8 @@
 
 #include "point_extractor.h"
 #include "compat/util.hpp"
+#include "point_tracker.h"
+#include <QDebug>
 
 #include <opencv2/videoio.hpp>
 
@@ -19,6 +21,59 @@
 
 using namespace types;
 
+/*
+http://en.wikipedia.org/wiki/Mean-shift
+In this application the idea, is to eliminate any bias of the point estimate 
+which is introduced by the rather arbitrary thresholded area. One must recognize
+that the thresholded area can only move in one pixel increments since it is
+binary. Thus, its center of mass might make "jumps" as pixels are added/removed
+from the thresholded area.
+With mean-shift, a moving "window" or kernel is multiplied with the gray-scale
+image, and the COM is calculated of the result. This is iterated where the
+kernel center is set the previously computed COM. Thus, peaks in the image intensity
+distribution "pull" the kernel towards themselves. Eventually it stops moving, i.e.
+then the computed COM coincides with the kernel center. We hope that the 
+corresponding location is a good candidate for the extracted point.
+The idea similar to the window scaling suggested in  Berglund et al. "Fast, bias-free 
+algorithm for tracking single particles with variable size and shape." (2008).
+*/
+static cv::Vec2d MeanShiftIteration(const cv::Mat &frame_gray, const cv::Vec2d &current_center, double filter_width)
+{
+    // Most amazingling this function runs faster with doubles than with floats.
+    const double s = 1.0 / filter_width;
+
+    auto EvalFilter = [&current_center, s](int row, int col)
+    {
+        double dx = (col - current_center[0])*s;
+        double dy = (row - current_center[1])*s;
+        double f = std::max(0.0, 1.0 - dx*dx - dy*dy);
+        return f;
+    };
+
+    double m = 0;
+    cv::Vec2d com(0.0, 0.0);
+    for (int i = 0; i < frame_gray.rows; i++)
+    {
+        auto frame_ptr = (uint8_t *)frame_gray.ptr(i);
+        for (int j = 0; j < frame_gray.cols; j++)
+        {
+            double val = frame_ptr[j];
+            val = val * val; // taking the square wights brighter parts of the image stronger.
+            val *= EvalFilter(i, j);
+            m += val;
+            com[0] += j * val;
+            com[1] += i * val;
+        }
+    }
+    if (m > 0.1)
+    {
+        com *= 1.0 / m;
+        return com;
+    }
+    else
+        return current_center;
+}
+
 PointExtractor::PointExtractor()
 {
     blobs.reserve(max_blobs);
@@ -135,12 +190,13 @@ void PointExtractor::extract_points(cv::Mat& frame, std::vector<vec2>& points)
                     cnt++;
                 }
             }
-            if (norm > 1e0)
+            if (norm > 0)
             {
-                const double radius = sqrt(cnt / M_PI);
+                const double radius = sqrt(cnt / M_PI), N = double(norm);
                 if (radius > region_size_max || radius < region_size_min)
                     continue;
-                blob b(radius, cv::Vec2d(m10 / norm, m01 / norm), norm/sqrt(double(cnt)));
+
+                blob b(radius, cv::Vec2d(m10 / N, m01 / N), N/sqrt(double(cnt)), rect);
                 blobs.push_back(b);
                 {
                     char buf[64];
@@ -162,6 +218,33 @@ end:
 
     sort(blobs.begin(), blobs.end(), [](const blob& b1, const blob& b2) -> bool { return b2.brightness < b1.brightness; });
 
+    for (idx = 0; idx < std::min(PointModel::N_POINTS, unsigned(blobs.size())); ++idx)
+    {
+        blob &b = blobs[idx];
+        cv::Rect rect = b.rect;
+        rect.x -= rect.width / 2;
+        rect.y -= rect.height / 2;
+        rect.width *= 2;
+        rect.height *= 2;
+        rect &= cv::Rect(0, 0, W, H);  // crop at frame boundaries
+
+        cv::Mat frame_roi = frame_gray(rect);
+
+        const double kernel_radius = b.radius * 1.5;
+        cv::Vec2d pos(b.pos[0] - rect.x, b.pos[1] - rect.y); // position relative to ROI.
+
+        for (int iter = 0; iter < 10; ++iter)
+        {
+            cv::Vec2d com_new = MeanShiftIteration(frame_roi, pos, kernel_radius);
+            cv::Vec2d delta = com_new - b.pos;
+            pos = com_new;
+            if (delta.dot(delta) < 1.0e-2) break;
+        }
+
+        b.pos[0] = pos[0] + rect.x;
+        b.pos[1] = pos[1] + rect.y;
+    }
+    // End of mean shift code. At this point, blob positions are updated which hopefully less noisy less biased values.
     points.reserve(max_blobs);
     points.clear();
 
@@ -171,3 +254,8 @@ end:
         points.push_back(p);
     }
 }
+
+PointExtractor::blob::blob(double radius, const cv::Vec2d& pos, double brightness, cv::Rect& rect) : radius(radius), brightness(brightness), pos(pos), rect(rect)
+{
+    //qDebug() << "radius" << radius << "pos" << pos[0] << pos[1];
+}