1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
|
/* 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)
{
const int W = frame.cols;
const int H = frame.rows;
// clear old points
points.clear();
// convert to grayscale
Mat frame_gray;
cvtColor(frame, frame_gray, CV_RGB2GRAY);
// convert to binary
Mat frame_bin;
threshold(frame_gray, frame_bin, threshold_val, 255, THRESH_BINARY);
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 = frame_gray.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);
}
}
// draw output image
if (draw_output) {
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;
}
|
