diff options
| -rw-r--r-- | ftnoir_tracker_pt/point_extractor.cpp | 128 |
1 files 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;
|