1 files changed, 73 insertions, 64 deletions

diff --git a/ftnoir_tracker_pt/point_extractor.cpp b/ftnoir_tracker_pt/point_extractor.cpp
index 5f7f6829..c186f5f2 100644
--- a/ftnoir_tracker_pt/point_extractor.cpp
+++ b/ftnoir_tracker_pt/point_extractor.cpp
@@ -25,85 +25,94 @@ std::vector<Vec2f> PointExtractor::extract_points(Mat& frame)
 	const int W = frame.cols;
 	const int H = frame.rows; 
 
-    if (frame_last.cols != W || frame_last.rows != H)
-    {
-        frame_last = cv::Mat();
-    }
+	// clear old points
+	points.clear();
 
-	// convert to grayscale
-	Mat frame_gray;
+    // convert to grayscale
+    Mat frame_gray;
     cvtColor(frame, frame_gray, cv::COLOR_RGB2GRAY);
-
-	int secondary = s.threshold_secondary;
+    
     int primary = s.threshold;
 	
-	// mask for everything that passes the threshold (or: the upper threshold of the hysteresis)
-	Mat frame_bin;
-	// only used if draw_output
-	Mat frame_bin_copy;
-	// mask for everything that passes
-	Mat frame_bin_low;
-	// mask for lower-threshold && combined result of last, needs to remain in scope until drawing, but is only used if secondary != 0
-	Mat frame_last_and_low;
-
-	if(secondary==0){
-		threshold(frame_gray, frame_bin, primary, 255, THRESH_BINARY);
-	}else{
-		// we recombine a number of buffers, this might be slower than a single loop of per-pixel logic
-		// but it might as well be faster if openCV makes good use of SIMD
-		float t = primary;
-		//float hyst = float(threshold_secondary_val)/512.;
-		//threshold(frame_gray, frame_bin, (t + ((255.-t)*hyst)), 255, THRESH_BINARY);
-		float hyst = float(primary)/(256.*8.);
-		threshold(frame_gray, frame_bin, t, 255, THRESH_BINARY); 
-		threshold(frame_gray, frame_bin_low,std::max(float(1), t - (t*hyst)), 255, THRESH_BINARY);
-
-		frame_bin.copyTo(frame_bin_copy);
-        if(frame_last.empty()){
-			frame_bin.copyTo(frame_last);
-		}else{
-			// keep pixels from last if they are above lower threshold
-			bitwise_and(frame_last, frame_bin_low, frame_last_and_low);
-			// union of pixels >= higher threshold and pixels >= lower threshold
-			bitwise_or(frame_bin, frame_last_and_low, frame_last);
-			frame_last.copyTo(frame_bin);
-		}
-	}
+    // mask for everything that passes the threshold (or: the upper threshold of the hysteresis)
+    Mat frame_bin;
+    
+    threshold(frame_gray, frame_bin, primary, 255, THRESH_BINARY);
     
     int min_size = s.min_point_size;
     int max_size = s.max_point_size;
     
-	unsigned int region_size_min = 3.14*min_size*min_size/4.0;
+    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;
-    
-    std::vector<std::vector<cv::Point>> contours;
-    cv::findContours(frame_bin, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
-    
-    // clear old points
-	points.clear();
-    
-    for (auto& c : contours)
-    {
-        auto m = cv::moments(cv::Mat(c));
-        const double area = m.m00;
-        if (area == 0.)
-            continue;
-        cv::Vec2f pos(m.m10 / m.m00, m.m01 / m.m00);
-        if (area < region_size_min || area > region_size_max)
-            continue;
-        pos[0] =  (pos[0] - W/2)/W;
-        pos[1] = -(pos[1] - H/2)/W;
-        
-        points.push_back(pos);
+
+	int blob_index = 1;
+	for (int y=0; y<H; y++)
+	{
+		if (blob_index >= 255) break;
+		for (int x=0; x<W; x++)
+		{
+			if (blob_index >= 255) break;
+
+			// find connected components with floodfill
+			if (frame_bin.at<unsigned char>(y,x) != 255) continue;
+			Rect rect;
+
+			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++;
+			 	}
+			}
+			
+			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;
+
+                                        val = frame_gray.at<unsigned char>(i,j);
+                                        val = float(val - primary)/(256 - primary);
+                                        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;
+			//qDebug()<<blob_index<<"  => "<<c[0]<<" "<<c[1];
+			points.push_back(c);
+		}
     }
-	
-	// draw output image
+    
+    // draw output image
     vector<Mat> channels;
     frame_bin.setTo(170, frame_bin);
     channels.push_back(frame_gray + frame_bin);
     channels.push_back(frame_gray - frame_bin);
     channels.push_back(frame_gray - frame_bin);
     merge(channels, frame);
-
+    
 	return points;
 }