Fawkes API Fawkes Development Version
rcd_circle.cpp
1
2/***************************************************************************
3 * rcd_circle.cpp - Implementation of a circle shape finder
4 *
5 * Created: Thu May 16 00:00:00 2005
6 * Copyright 2005 Tim Niemueller [www.niemueller.de]
7 * Hu Yuxiao <Yuxiao.Hu@rwth-aachen.de>
8 *
9 ****************************************************************************/
10
11/* This program is free software; you can redistribute it and/or modify
12 * it under the terms of the GNU General Public License as published by
13 * the Free Software Foundation; either version 2 of the License, or
14 * (at your option) any later version. A runtime exception applies to
15 * this software (see LICENSE.GPL_WRE file mentioned below for details).
16 *
17 * This program is distributed in the hope that it will be useful,
18 * but WITHOUT ANY WARRANTY; without even the implied warranty of
19 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20 * GNU Library General Public License for more details.
21 *
22 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
23 */
24
25#include <fvmodels/shape/rcd_circle.h>
26#include <sys/time.h>
27
28#include <cmath>
29#include <cstdlib>
30
31using namespace std;
32using namespace fawkes;
33
34namespace firevision {
35
36#define TBY_GRAYSCALE
37#ifdef TBY_GRAYSCALE
38# define TEST_IF_IS_A_PIXEL(x) ((x) > 230)
39#else
40# define TEST_IF_IS_A_PIXEL(x) ((x) == 0)
41#endif // TBY_GRAYSCALE
42
43#define TBY_SQUARED_DIST(x1, y1, x2, y2) \
44 (((x1) - (x2)) * ((x1) - (x2)) + ((y1) - (y2)) * ((y1) - (y2)))
45#define TBY_RADIUS_DIFF(x1, y1, x2, y2, r) \
46 (((x1) - (x2)) * ((x1) - (x2)) + ((y1) - (y2)) * ((y1) - (y2)) - (r) * (r))
47
48/** @class RcdCircleModel <fvmodels/shape/rcd_circle.h>
49 * RCD circle model from the following literature
50 * An Efficient Randomized Algorithm for Detecting Circles
51 */
52
53/** Create a new circle model which uses RCD to detect circles
54 * @param max_failures Max. number of failures
55 * @param min_pixels Min number of available edge pixels
56 * @param min_interpix_dist Min distance between chosen pixels
57 * @param max_dist_p4 Max. distance of fourth pixel to circle
58 * @param max_dist_a Max. distance for all other pixels to circle
59 * @param hw_ratio Ratio height/width
60 * @param hollow_rate size of the hollow window in the ROI.
61 * @param max_time Maximum runtime per loop
62 */
63RcdCircleModel::RcdCircleModel(unsigned int max_failures,
64 unsigned int min_pixels,
65 unsigned int min_interpix_dist,
66 unsigned int max_dist_p4,
67 unsigned int max_dist_a,
68 float hw_ratio,
69 float hollow_rate,
70 float max_time)
71{
72 RCD_MAX_FAILURES = max_failures;
73 RCD_MIN_PIXELS = min_pixels;
74 RCD_MIN_INTERPIX_DIST = min_interpix_dist;
75 RCD_MAX_DIST_P4 = max_dist_p4;
76 RCD_MAX_DIST_A = max_dist_a;
77 RCD_HW_RATIO = hw_ratio;
78 RCD_MAX_TIME = max_time;
79 RCD_ROI_HOLLOW_RATE = hollow_rate;
80}
81
82/** Destrcutor. */
84{
85 m_Circles.clear();
86}
87
88int
89RcdCircleModel::parseImage(unsigned char *buf, ROI *roi)
90{
91 unsigned char *buffer = roi->get_roi_buffer_start(buf);
92 unsigned char *line_start = buffer;
93
94 unsigned int x, y;
95 vector<upoint_t> pixels, remove_list;
96 unsigned int f = 0; // number of failures
97 int count; // number of pixels on the circle
98 int num_circles = 0;
99 struct timeval start, now;
100
101 // clear all the remembered circles
102 m_Circles.clear();
103
104 const unsigned int roi_hollow_top = (int)(roi->height * ((1.0f - RCD_ROI_HOLLOW_RATE) / 2));
105 const unsigned int roi_hollow_bottom = roi->height - roi_hollow_top;
106 const unsigned int roi_hollow_left = (int)(roi->width * ((1.0f - RCD_ROI_HOLLOW_RATE) / 2));
107 const unsigned int roi_hollow_right = roi->width - roi_hollow_left;
108
109 // First, find all the pixels on the edges,
110 // and store them in the 'pixels' vector.
111 // NEW: excluding the hollow window
112 buffer = roi->get_roi_buffer_start(buf);
113 line_start = buffer;
114
115 // Find the boundary of the ball,
116 // following used for ball pixel threshold.
117 unsigned int boundary_right = 0;
118 unsigned int boundary_bottom = 0;
119
120 gettimeofday(&start, NULL);
121
122 pixels.clear();
123
124 // top "1/3"
125 for (y = 0; y < roi_hollow_top; ++y) {
126 for (x = 0; x < roi->width; ++x) {
127 if (TEST_IF_IS_A_PIXEL(*buffer)) {
128 upoint_t pt = {x, y};
129 pixels.push_back(pt);
130 if (x > boundary_right)
131 boundary_right = x;
132 boundary_bottom = y;
133 }
134 // NOTE: this assumes roi->pixel_step == 1
135 ++buffer;
136 }
137 line_start += roi->line_step;
138 buffer = line_start;
139 }
140 // middle "1/3"
141 for (y = roi_hollow_top; y < roi_hollow_bottom; ++y) {
142 for (x = 0; x < roi_hollow_left; ++x) {
143 if (TEST_IF_IS_A_PIXEL(*buffer)) {
144 upoint_t pt = {x, y};
145 pixels.push_back(pt);
146 if (x > boundary_right)
147 boundary_right = x;
148 boundary_bottom = y;
149 }
150 // NOTE: this assumes roi->pixel_step == 1
151 ++buffer;
152 }
153 buffer += (roi_hollow_right - roi_hollow_left);
154 for (x = roi_hollow_right; x < roi->width; ++x) {
155 if (TEST_IF_IS_A_PIXEL(*buffer)) {
156 upoint_t pt = {x, y};
157 pixels.push_back(pt);
158 if (x > boundary_right)
159 boundary_right = x;
160 boundary_bottom = y;
161 }
162 // NOTE: this assumes roi->pixel_step == 1
163 ++buffer;
164 }
165 line_start += roi->line_step;
166 buffer = line_start;
167 }
168 // bottom "1/3"
169 for (y = roi_hollow_bottom; y < roi->height; ++y) {
170 for (x = 0; x < roi->width; ++x) {
171 if (TEST_IF_IS_A_PIXEL(*buffer)) {
172 upoint_t pt = {x, y};
173 pixels.push_back(pt);
174 }
175 // NOTE: this assumes roi->pixel_step == 1
176 ++buffer;
177 }
178 line_start += roi->line_step;
179 buffer = line_start;
180 }
181
182 // Then perform the RCD algorithm
183 upoint_t p[4];
184 center_in_roi_t center;
185 float radius;
186 vector<upoint_t>::iterator pos;
187
188 if (pixels.size() < RCD_MIN_PIXELS) {
189 return 0;
190 }
191
192 do {
193 // Pick four points, and move them to the remove_list.
194 for (int i = 0; i < 4; ++i) {
195 int ri = rand() % ((int)pixels.size());
196 pos = pixels.begin() + ri;
197 p[i] = *pos; // use * operator of iterator
198 pixels.erase(pos);
199 remove_list.push_back(p[i]);
200 }
201
202 if (TBY_SQUARED_DIST(p[1].x, p[1].y, p[2].x, p[2].y) < RCD_MIN_INTERPIX_DIST
203 || TBY_SQUARED_DIST(p[2].x, p[2].y, p[0].x, p[0].y) < RCD_MIN_INTERPIX_DIST
204 || TBY_SQUARED_DIST(p[0].x, p[0].y, p[1].x, p[1].y) < RCD_MIN_INTERPIX_DIST) {
205 // there are two points that are too near
206 // to each other
207 ++f;
208
209 // restore all the pixels in remove_list to pixels
210 pixels.push_back(p[0]);
211 pixels.push_back(p[1]);
212 pixels.push_back(p[2]);
213 pixels.push_back(p[3]);
214
215 remove_list.clear();
216
217 gettimeofday(&now, NULL);
218 continue;
219 }
220
221 // Now calculate the center and radius
222 // based on the first three points.
223 calcCircle(p[0], p[1], p[2], center, radius);
224
225 // Test if the fourth point on this circle
226 int r = (int)sqrt(TBY_SQUARED_DIST(center.x, center.y, pixels[3].x, pixels[3].y));
227 int dist = (int)(r - radius);
228 dist = (dist >= 0) ? dist : -dist;
229 if (radius <= 0 || (unsigned int)dist > RCD_MAX_DIST_P4) {
230 ++f;
231
232 //restore all the pixels
233 pixels.push_back(p[0]);
234 pixels.push_back(p[1]);
235 pixels.push_back(p[2]);
236 pixels.push_back(p[3]);
237
238 remove_list.clear();
239
240 gettimeofday(&now, NULL);
241 continue;
242 }
243
244 // count how many pixels are on the circle
245 count = 0;
246 for (unsigned int i = 0; i < pixels.size(); ++i) {
247 int r = (int)sqrt(TBY_SQUARED_DIST(center.x, center.y, pixels[i].x, pixels[i].y));
248 int dist = (int)(r - radius);
249 dist = (dist >= 0) ? dist : -dist;
250 if ((unsigned int)dist <= RCD_MAX_DIST_A) {
251 pos = pixels.begin() + i;
252 ++count;
253 // move this pixel to the remove_list
254 remove_list.push_back(pixels[i]);
255 pixels.erase(pos--); // need "--"? not sure yet!
256 }
257 }
258
259 // test if there are enough points on the circle
260 // to convince us that this is indeed a circle
261 if ((float)count
262 > (boundary_right > boundary_bottom ? boundary_right : boundary_bottom) * RCD_HW_RATIO) {
263 // this is indeed a circle
264 if (radius > TBY_CIRCLE_RADIUS_MIN && radius < TBY_CIRCLE_RADIUS_MAX) {
265 // only ball of size in the range are saved in the candidate pool.
266
267 // use least square fitting to reduce error
268 Circle c;
269 c.fitCircle(remove_list);
270 c.count = count;
271
272 // save circle
273 m_Circles.push_back(c);
274 }
275 remove_list.clear();
276 ++num_circles;
277 } else {
278 // not a circle, charge a failure
279 ++f;
280
281 while (!remove_list.empty()) {
282 pixels.push_back(remove_list.back());
283 remove_list.pop_back();
284 }
285 gettimeofday(&now, NULL);
286 continue;
287 }
288
289 gettimeofday(&now, NULL);
290
291 diff_sec = now.tv_sec - start.tv_sec;
292 diff_usec = now.tv_usec - start.tv_usec;
293 if (diff_usec < 0) {
294 diff_sec -= 1;
295 diff_usec += 1000000;
296 }
297
298 f_diff_sec = diff_sec + diff_usec / 1000000.f;
299
300 } while ((f < RCD_MAX_FAILURES) && (pixels.size() > RCD_MIN_PIXELS)
301 && (f_diff_sec < RCD_MAX_TIME));
302
303 return num_circles;
304}
305
306int
308{
309 return m_Circles.size();
310}
311
312Circle *
314{
315 if (id < 0 || (unsigned int)id >= m_Circles.size()) {
316 return NULL;
317 } else {
318 return const_cast<Circle *>(&m_Circles[id]); // or use const Shape* def?!...
319 }
320}
321
322Circle *
324{
325 int cur = 0;
326 switch (m_Circles.size()) {
327 case 0: return NULL;
328 case 1: return const_cast<Circle *>(&m_Circles[0]); // or use const Shape* def?!...
329 default:
330 for (unsigned int i = 1; i < m_Circles.size(); ++i)
331 if (m_Circles[i].radius > m_Circles[cur].radius)
332 cur = i;
333 return const_cast<Circle *>(&m_Circles[cur]); // or use const Shape* definition?!...
334 }
335}
336
337void
338RcdCircleModel::calcCircle(const upoint_t & p1,
339 const upoint_t & p2,
340 const upoint_t & p3,
341 center_in_roi_t &center,
342 float & radius)
343// Given three points p1, p2, p3,
344// this function calculates the center and radius
345// of the circle that is determined
346{
347 const int &x1 = p1.x, &y1 = p1.y, &x2 = p2.x, &y2 = p2.y, &x3 = p3.x, &y3 = p3.y;
348 float dx, dy;
349 int div = 2 * ((x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1));
350
351 if (div == 0) {
352 // p1, p2 and p3 are in a straight line.
353 radius = -1.0;
354 return;
355 }
356 center.x = ((float)((x2 * x2 + y2 * y2 - x1 * x1 - y1 * y1) * (y3 - y1)
357 - (x3 * x3 + y3 * y3 - x1 * x1 - y1 * y1) * (y2 - y1))
358 / div);
359 center.y = ((float)((x2 - x1) * (x3 * x3 + y3 * y3 - x1 * x1 - y1 * y1)
360 - (x3 - x1) * (x2 * x2 + y2 * y2 - x1 * x1 - y1 * y1))
361 / div);
362 dx = center.x - x1;
363 dy = center.y - y1;
364 radius = (float)sqrt(dx * dx + dy * dy);
365}
366
367} // end namespace firevision
Circle shape.
Definition: circle.h:43
void fitCircle(std::vector< fawkes::upoint_t > &points)
Fit circle.
Definition: circle.cpp:73
int count
Number of pixels.
Definition: circle.h:61
Region of interest.
Definition: roi.h:55
unsigned int height
ROI height.
Definition: roi.h:119
unsigned char * get_roi_buffer_start(unsigned char *buffer) const
Get ROI buffer start.
Definition: roi.cpp:526
unsigned int line_step
line step
Definition: roi.h:125
unsigned int width
ROI width.
Definition: roi.h:117
RcdCircleModel(unsigned int max_failures=300, unsigned int min_pixels=20, unsigned int min_interpix_dist=10, unsigned int max_dist_p4=2, unsigned int max_dist_a=10, float hw_ratio=0.6, float hollow_rate=0.f, float max_time=0.01)
Create a new circle model which uses RCD to detect circles.
Definition: rcd_circle.cpp:63
virtual ~RcdCircleModel(void)
Destrcutor.
Definition: rcd_circle.cpp:83
Circle * getMostLikelyShape(void) const
Get best candidate.
Definition: rcd_circle.cpp:323
Circle * getShape(int id) const
Get specific shape.
Definition: rcd_circle.cpp:313
int parseImage(unsigned char *buffer, ROI *roi)
Parse image for given ROI.
Definition: rcd_circle.cpp:89
int getShapeCount(void) const
Get number of shapes.
Definition: rcd_circle.cpp:307
Fawkes library namespace.
Point with cartesian coordinates as unsigned integers.
Definition: types.h:35
unsigned int x
x coordinate
Definition: types.h:36
unsigned int y
y coordinate
Definition: types.h:37
Center in ROI.
Definition: types.h:38
float y
y in pixels
Definition: types.h:40
float x
x in pixels
Definition: types.h:39