1 files changed, 97 insertions, 0 deletions
diff --git a/ftnoir_tracker_pt/point_extractor.cpp b/ftnoir_tracker_pt/point_extractor.cpp
new file mode 100644
index 00000000..4aa1a658
--- /dev/null
+++ b/ftnoir_tracker_pt/point_extractor.cpp
@@ -0,0 +1,97 @@
+/* Copyright (c) 2012 Patrick Ruoff
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ */
+
+#include "point_extractor.h"
+#include <QDebug>
+
+using namespace cv;
+using namespace std;
+
+// ----------------------------------------------------------------------------
+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;
+	}
+	*/
+
+	// convert to grayscale
+	Mat frame_bw;
+	cvtColor(frame, frame_bw, CV_RGB2GRAY);
+
+	// 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
+
+	// find connected components...
+	// extract blobs with floodfill
+	struct BlobInfo
+	{
+		BlobInfo() : m00(0), m10(0), m01(0) {}
+		long m00;
+		long m10;
+		long m01;
+	};
+	vector<BlobInfo> blobs;
+    int blob_count = 1;
+
+    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;
+            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;
+                }
+            }
+            blobs.push_back(blob);
+            blob_count++;
+			if (blob_count >= 255) break;
+        }
+		if (blob_count >= 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);
+	}
+
+	return points;
+}