26 #include <fvmodels/shape/rht_circle.h>
34 #define TEST_IF_IS_A_PIXEL(x) ((x) > 230)
36 #define TBY_SQUARED_DIST(x1, y1, x2, y2) \
37 (((x1) - (x2)) * ((x1) - (x2)) + ((y1) - (y2)) * ((y1) - (y2)))
38 #define TBY_RADIUS_DIFF(x1, y1, x2, y2, r) \
39 (sqrt(((x1) - (x2)) * ((x1) - (x2)) + ((y1) - (y2)) * ((y1) - (y2)) - (r) * (r)))
41 namespace firevision {
43 const float RhtCircleModel::RHT_MIN_RADIUS = 40.0;
44 const float RhtCircleModel::RHT_MAX_RADIUS = 500.0;
51 RhtCircleModel::RhtCircleModel(
void)
56 RhtCircleModel::~RhtCircleModel(
void)
67 RhtCircleModel::parseImage(
unsigned char *buf,
ROI *roi)
72 vector<upoint_t> pixels;
73 struct timeval start, end;
82 const int RHT_MAX_TIME = 10000;
83 const int RHT_MAX_ITER = 1000;
86 const float RHT_MIN_VOTE_RATE = 0.0f;
89 const int RHT_XY_SCALE = 8;
90 const int RHT_RADIUS_SCALE = 8;
94 const float RHT_FITTING_DIST_DIFF = 15.0f;
97 const float ROI_HOLLOW_RATE = 0.70f;
99 const unsigned int roi_hollow_top = (int)(roi->
height * ((1.0f - ROI_HOLLOW_RATE) / 2));
100 const unsigned int roi_hollow_bottom = roi->
height - roi_hollow_top;
101 const unsigned int roi_hollow_left = (int)(roi->
width * ((1.0f - ROI_HOLLOW_RATE) / 2));
102 const unsigned int roi_hollow_right = roi->
width - roi_hollow_left;
107 unsigned char *line_start = buffer;
109 gettimeofday(&start, NULL);
110 end.tv_usec = start.tv_usec;
113 for (y = 0; y < roi_hollow_top; ++y) {
114 for (x = 0; x < roi->
width; ++x) {
115 if (TEST_IF_IS_A_PIXEL(*buffer)) {
117 pixels.push_back(pt);
126 for (y = roi_hollow_top; y < roi_hollow_bottom; ++y) {
127 for (x = 0; x < roi_hollow_left; ++x) {
128 if (TEST_IF_IS_A_PIXEL(*buffer)) {
130 pixels.push_back(pt);
135 buffer += (roi_hollow_right - roi_hollow_left);
136 for (x = roi_hollow_right; x < roi->
width; ++x) {
137 if (TEST_IF_IS_A_PIXEL(*buffer)) {
139 pixels.push_back(pt);
148 for (y = roi_hollow_bottom; y < roi->
height; ++y) {
149 for (x = 0; x < roi->
width; ++x) {
150 if (TEST_IF_IS_A_PIXEL(*buffer)) {
152 pixels.push_back(pt);
165 vector<upoint_t>::iterator pos;
167 int num_points = (int)pixels.size();
168 if (num_points == 0) {
173 while ((num_iter++ < RHT_MAX_ITER)
174 && (((end.tv_usec - start.tv_usec) < RHT_MAX_TIME)
175 || ((end.tv_usec + 1000000 - start.tv_usec) < RHT_MAX_TIME))
179 for (
int i = 0; i < 3; ++i) {
180 int ri = rand() % num_points;
181 pos = pixels.begin() + ri;
187 calcCircle(p[0], p[1], p[2], center, radius);
190 if (radius > RHT_MIN_RADIUS && radius < RHT_MAX_RADIUS) {
191 accumulator.accumulate((
int)(center.
x / RHT_XY_SCALE),
192 (
int)(center.
y / RHT_XY_SCALE),
193 (
int)(radius / RHT_RADIUS_SCALE));
196 gettimeofday(&end, NULL);
200 int max, x_max, y_max, r_max;
201 max = accumulator.getMax(x_max, y_max, r_max);
203 cout <<
"Max vote is with " << max <<
" of the " << num_iter <<
" ones." << endl;
206 if (max > num_iter * RHT_MIN_VOTE_RATE) {
208 center.
x = (float)(x_max * RHT_XY_SCALE + RHT_XY_SCALE / 2);
209 center.
y = (float)(y_max * RHT_XY_SCALE + RHT_XY_SCALE / 2);
210 float c_r = (float)(r_max * RHT_RADIUS_SCALE + RHT_RADIUS_SCALE / 2);
213 for (vector<upoint_t>::iterator pos = pixels.begin(); pos != pixels.end();) {
214 if (TBY_RADIUS_DIFF(pos->x, pos->y, center.
x, center.
y, c_r) > RHT_FITTING_DIST_DIFF) {
224 m_Circles.push_back(c);
241 RhtCircleModel::getShapeCount(
void)
const
243 return m_Circles.size();
247 RhtCircleModel::getShape(
int id)
const
249 if (
id < 0 || (
unsigned int)
id >= m_Circles.size()) {
252 return const_cast<Circle *
>(&m_Circles[id]);
257 RhtCircleModel::getMostLikelyShape(
void)
const
259 if (m_Circles.size() == 0) {
261 }
else if (m_Circles.size() == 1) {
262 return const_cast<Circle *
>(&m_Circles[0]);
265 for (
unsigned int i = 1; i < m_Circles.size(); ++i) {
266 if (m_Circles[i].count > m_Circles[cur].count) {
270 return const_cast<Circle *
>(&m_Circles[cur]);
275 RhtCircleModel::calcCircle(
const upoint_t & p1,
284 const int &x1 = p1.
x, &y1 = p1.
y, &x2 = p2.
x, &y2 = p2.
y, &x3 = p3.
x, &y3 = p3.
y;
286 int div = 2 * ((x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1));
293 center.
x = ((float)((x2 * x2 + y2 * y2 - x1 * x1 - y1 * y1) * (y3 - y1)
294 - (x3 * x3 + y3 * y3 - x1 * x1 - y1 * y1) * (y2 - y1))
296 center.
y = ((float)((x2 - x1) * (x3 * x3 + y3 * y3 - x1 * x1 - y1 * y1)
297 - (x3 - x1) * (x2 * x2 + y2 * y2 - x1 * x1 - y1 * y1))
301 radius = (float)sqrt(dx * dx + dy * dy);