From a205d7bbe8085d22d2f2931812a7975c981bf363 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Fri, 26 Jul 2013 16:37:32 +0200
Subject: cherry pick new point extractor from Patrick

---
 ftnoir_tracker_pt/point_extractor.cpp | 128 +++++++++++++++++-----------------
 1 file changed, 63 insertions(+), 65 deletions(-)

diff --git a/ftnoir_tracker_pt/point_extractor.cpp b/ftnoir_tracker_pt/point_extractor.cpp
index 4213eb06..61e86bec 100644
--- a/ftnoir_tracker_pt/point_extractor.cpp
+++ b/ftnoir_tracker_pt/point_extractor.cpp
@@ -11,87 +11,85 @@
 using namespace cv;
 using namespace std;
 
-struct BlobInfo
-{
-    BlobInfo() : m00(0), m10(0), m01(0) {}
-    long m00;
-    long m10;
-    long m01;
-};
-
 // ----------------------------------------------------------------------------
 const vector<Vec2f>& PointExtractor::extract_points(Mat frame, float dt, bool draw_output)
 {
-	/*
-	// sensitivity test for tracker
-	static int n = 0;
-	if (points.size() == 3)
-	{
-		for (int i=0; i<3; ++i)
-		{
-			points[i][0] -= 1e-4*sin(n/100.0);
-			points[i][1] -= 1e-4*cos(n/100.0);
-		}
-		++n;
-		return points;
-	}
-	*/
+    const int W = frame.cols;
+	const int H = frame.rows; 
+
+	// clear old points
+	points.clear();
 
 	// convert to grayscale
-	Mat frame_bw;
-    cvtColor(frame, frame_bw, COLOR_BGR2GRAY);
+	Mat frame_grey;
+	cvtColor(frame, frame_grey, COLOR_BGR2GRAY);
 
 	// convert to binary
-	threshold(frame_bw, frame_bw, threshold_val, 255, THRESH_BINARY);
-	//erode(frame_bw, frame_bw, Mat(), Point(-1,-1), min_size); //destroys information -> bad for subpixel accurarcy
+	Mat frame_bin;
+	threshold(frame_grey, frame_bin, threshold_val, 255, THRESH_BINARY);
 
-	// find connected components...
-	// extract blobs with floodfill
-	vector<BlobInfo> blobs;
-    int blob_count = 1;
+	unsigned int region_size_min = 3.14*min_size*min_size;
+    unsigned int region_size_max = 3.14*max_size*max_size;
 
-    for (int y=0; y < frame_bw.rows; y++) {
-        for (int x=0; x < frame_bw.cols; x++) {
-			if (frame_bw.at<unsigned char>(y,x) != 255) continue;
+    int blob_index = 1;
+    for (int y=0; y<H; y++)
+	{
+        for (int x=0; x<W; x++)
+		{
+			// find connected components with floodfill
+			if (frame_bin.at<unsigned char>(y,x) != 255) continue;
             Rect rect;
-            floodFill(frame_bw, Point(x,y), Scalar(blob_count), &rect, Scalar(0), Scalar(0), 4);
-			BlobInfo blob;
-            for (int i=rect.y; i < (rect.y+rect.height); i++) {
-                for (int j=rect.x; j < (rect.x+rect.width); j++) {
-                    if (frame_bw.at<unsigned char>(i,j) != blob_count) continue;
-                    blob.m00++;
-					blob.m01+=i;
-					blob.m10+=j;
+            floodFill(frame_bin, Point(x,y), Scalar(blob_index), &rect, Scalar(0), Scalar(0), FLOODFILL_FIXED_RANGE);
+			blob_index++;
+
+			// calculate the size of the connected component
+			unsigned int region_size = 0;
+            for (int i=rect.y; i < (rect.y+rect.height); i++)
+			{
+                for (int j=rect.x; j < (rect.x+rect.width); j++)
+				{
+                    if (frame_bin.at<unsigned char>(i,j) != blob_index-1) continue;
+					region_size++;
                 }
             }
-            blobs.push_back(blob);
-            blob_count++;
-			if (blob_count >= 255) break;
+			
+			if (region_size < region_size_min || region_size > region_size_max) continue; 
+
+			// calculate the center of mass:
+			// mx = (sum_ij j*f(frame_grey_ij)) / (sum_ij f(frame_grey_ij))
+			// my = ...
+			// f maps from [threshold,256] -> [0, 1], lower values are mapped to 0 
+			float m = 0;
+			float mx = 0;
+			float my = 0;
+			for (int i=rect.y; i < (rect.y+rect.height); i++)
+			{
+                for (int j=rect.x; j < (rect.x+rect.width); j++)
+				{
+                    if (frame_bin.at<unsigned char>(i,j) != blob_index-1) continue;
+					float val = frame_grey.at<unsigned char>(i,j);
+					val = float(val - threshold_val)/(256 - threshold_val);
+					val = val*val; // makes it more stable (less emphasis on low values, more on the peak)
+					m  +=     val;
+					mx += j * val;
+					my += i * val; 
+                }
+            }
+
+			// convert to centered camera coordinate system with y axis upwards 
+			Vec2f c;
+			c[0] =  (mx/m - W/2)/W;
+			c[1] = -(my/m - H/2)/W;
+			points.push_back(c);
+            
+			if (blob_index >= 255) break;
         }
-		if (blob_count >= 255) break;
+		if (blob_index >= 255) break;
     }
-
-	// extract points
-	Vec2f c;
-	points.clear();
-	float m00_min = 3.14*min_size*min_size;
-	float m00_max = 3.14*max_size*max_size;
-	for (vector<BlobInfo>::iterator iter = blobs.begin();
-		 iter!= blobs.end();
-		 ++iter)
-	{
-		const BlobInfo& m = *iter;
-		if (m.m00 < m00_min || m.m00 > m00_max) continue;
-		// convert to centered camera coordinate system with y axis upwards
-		c[0] =  (m.m10/float(m.m00) - frame.cols/2)/frame.cols;
-		c[1] = -(m.m01/float(m.m00) - frame.rows/2)/frame.cols;
-		points.push_back(c);
-	}
 	
 	// draw output image
-	if (draw_output)
-	{	
-		frame.setTo(Scalar(255,0,0), frame_bw);
+	if (draw_output) {	
+		frame.setTo(Scalar(255,0,0), frame_bin);
 	}
 
 	return points;
-- 
cgit v1.2.3