pt: try simple method of point extraction

To be reverted if broken

1 files changed, 25 insertions, 82 deletions

diff --git a/ftnoir_tracker_pt/point_extractor.cpp b/ftnoir_tracker_pt/point_extractor.cpp
index 2c39c2e2..5f7f6829 100644
--- a/ftnoir_tracker_pt/point_extractor.cpp
+++ b/ftnoir_tracker_pt/point_extractor.cpp
@@ -30,9 +30,6 @@ std::vector<Vec2f> PointExtractor::extract_points(Mat& frame)
         frame_last = cv::Mat();
     }
 
-	// clear old points
-	points.clear();
-
 	// convert to grayscale
 	Mat frame_gray;
     cvtColor(frame, frame_gray, cv::COLOR_RGB2GRAY);
@@ -78,88 +75,34 @@ std::vector<Vec2f> PointExtractor::extract_points(Mat& frame)
     
 	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;
-
-	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;
-
-					if(secondary==0){
-						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)
-					}else{
-						//hysteresis point detection gets stability from ignoring pixel noise so we decidedly leave the actual pixel values out of the picture
-						val = frame_last.at<unsigned char>(i,j) / 256.;
-					}
-
-					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);
-		}
-	}
+    
+    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);
+    }
 	
 	// draw output image
     vector<Mat> channels;
-    if(secondary==0){
-        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);
-    }else{
-        frame_bin_copy.setTo(120, frame_bin_copy);
-        //frame_bin_low.setTo(90, frame_bin_low);
-        channels.push_back(frame_gray + frame_bin_copy);
-        channels.push_back(frame_gray - frame_bin_copy);
-        channels.push_back(frame_gray - frame_bin_copy);
-        //channels.push_back(frame_gray + frame_bin);
-    }
+    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;