Fawkes API Fawkes Development Version
rht_circle.cpp
1
2/***************************************************************************
3 * rht_circle.cpp - Implementation of a circle shape finder
4 * with Randomized Hough Transform
5 *
6 * Created: Tue Jun 28 00:00:00 2005
7 * Copyright 2005 Hu Yuxiao <Yuxiao.Hu@rwth-aachen.de>
8 * Tim Niemueller [www.niemueller.de]
9 *
10 ****************************************************************************/
11
12/* This program is free software; you can redistribute it and/or modify
13 * it under the terms of the GNU General Public License as published by
14 * the Free Software Foundation; either version 2 of the License, or
15 * (at your option) any later version. A runtime exception applies to
16 * this software (see LICENSE.GPL_WRE file mentioned below for details).
17 *
18 * This program is distributed in the hope that it will be useful,
19 * but WITHOUT ANY WARRANTY; without even the implied warranty of
20 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21 * GNU Library General Public License for more details.
22 *
23 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
24 */
25
26#include <fvmodels/shape/rht_circle.h>
27#include <sys/time.h>
28
29#include <cmath>
30
31using namespace std;
32using namespace fawkes;
33
34#define TEST_IF_IS_A_PIXEL(x) ((x) > 230)
35
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)))
40
41namespace firevision {
42
43const float RhtCircleModel::RHT_MIN_RADIUS = 40.0;
44const float RhtCircleModel::RHT_MAX_RADIUS = 500.0;
45
46/** @class RhtCircleModel <fvmodels/shape/rht_circle.h>
47 * Randomized Hough-Transform circle model.
48 */
49
50/** Constructor. */
52{
53}
54
55/** Destructor. */
57{
58 m_Circles.clear();
59}
60
61/**************************************************************
62 * In this function I implement the circle detection algorithm
63 * from the following literature
64 * Randomized Hough Transform
65 **************************************************************/
66int
67RhtCircleModel::parseImage(unsigned char *buf, ROI *roi)
68{
69 unsigned char *buffer = roi->get_roi_buffer_start(buf);
70
71 unsigned int x, y;
72 vector<upoint_t> pixels;
73 struct timeval start, end;
74
75 // clear the accumulator
76 accumulator.reset();
77
78 // clear all the remembered circles
79 m_Circles.clear();
80
81 // The following constants are used as stopping criteria
82 const int RHT_MAX_TIME = 10000; // = 10ms (is given in microseconds)
83 const int RHT_MAX_ITER = 1000; // Maximal number of iterations.
84
85 // The following constant is used for eliminating circles with too few votes
86 const float RHT_MIN_VOTE_RATE = 0.0f;
87
88 // The following constants are used for RHT accumulator precision
89 const int RHT_XY_SCALE = 8;
90 const int RHT_RADIUS_SCALE = 8;
91
92 // All the pixels with a less distance difference than below
93 // are taken into account for circle fitting.
94 const float RHT_FITTING_DIST_DIFF = 15.0f;
95
96 // The following constant is used for the size of the hollow window in the ROI.
97 const float ROI_HOLLOW_RATE = 0.70f;
98
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;
103
104 // First, find all the pixels on the edges,
105 // and store them in the 'pixels' vector.
106 // NEW: excluding the hollow window
107 unsigned char *line_start = buffer;
108
109 gettimeofday(&start, NULL);
110 end.tv_usec = start.tv_usec;
111
112 // top "1/3"
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)) {
116 upoint_t pt = {x, y};
117 pixels.push_back(pt);
118 }
119 // NOTE: this assumes roi->pixel_step == 1
120 ++buffer;
121 }
122 line_start += roi->line_step;
123 buffer = line_start;
124 }
125 // middle "1/3"
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)) {
129 upoint_t pt = {x, y};
130 pixels.push_back(pt);
131 }
132 // NOTE: this assumes roi->pixel_step == 1
133 ++buffer;
134 }
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)) {
138 upoint_t pt = {x, y};
139 pixels.push_back(pt);
140 }
141 // NOTE: this assumes roi->pixel_step == 1
142 ++buffer;
143 }
144 line_start += roi->line_step;
145 buffer = line_start;
146 }
147 // bottom "1/3"
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)) {
151 upoint_t pt = {x, y};
152 pixels.push_back(pt);
153 }
154 // NOTE: this assumes roi->pixel_step == 1
155 ++buffer;
156 }
157 line_start += roi->line_step;
158 buffer = line_start;
159 }
160
161 // Then perform the RHT algorithm
162 upoint_t p[3];
163 center_in_roi_t center;
164 float radius;
165 vector<upoint_t>::iterator pos;
166 int num_iter = 0;
167 int num_points = (int)pixels.size();
168 if (num_points == 0) {
169 // No pixels found => no edge => no circle
170 return 0;
171 }
172
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))
176 // this only works if time constraint smaller than 500ms
177 ) {
178 // Pick three points, and move them to the remove_list.
179 for (int i = 0; i < 3; ++i) {
180 int ri = rand() % num_points;
181 pos = pixels.begin() + ri;
182 p[i] = *pos; // use * operator of iterator
183 }
184
185 // Now calculate the center and radius
186 // based on the three points.
187 calcCircle(p[0], p[1], p[2], center, radius);
188
189 // Accumulate this circle to the 3-D space...
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));
194 }
195
196 gettimeofday(&end, NULL);
197 }
198
199 // Find the most dense region, and decide on the ball.
200 int max, x_max, y_max, r_max;
201 max = accumulator.getMax(x_max, y_max, r_max);
202
203 cout << "Max vote is with " << max << " of the " << num_iter << " ones." << endl;
204
205 // Only when votes exceeds a ratio will it be considered a real circle
206 if (max > num_iter * RHT_MIN_VOTE_RATE) {
207 center_in_roi_t center;
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);
211
212 // With circle fitting
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) {
215 pixels.erase(pos);
216 } else {
217 pos++;
218 }
219 }
220
221 Circle c;
222 c.fitCircle(pixels);
223 c.count = max;
224 m_Circles.push_back(c);
225
226 /*
227 // Without circle fitting
228 m_Circles.push_back(Circle(center, c_r, max));
229 */
230
231 return 0;
232 }
233
234 // Return... here in this algorithm, we only find at most one most likely circle,
235 // (if none found, return-ed above)
236 // so the return value here is always 1
237 return 1;
238}
239
240int
242{
243 return m_Circles.size();
244}
245
246Circle *
248{
249 if (id < 0 || (unsigned int)id >= m_Circles.size()) {
250 return NULL;
251 } else {
252 return const_cast<Circle *>(&m_Circles[id]); // or use const Shape* def?!...
253 }
254}
255
256Circle *
258{
259 if (m_Circles.size() == 0) {
260 return NULL;
261 } else if (m_Circles.size() == 1) {
262 return const_cast<Circle *>(&m_Circles[0]); // or use const Shape* def?!...
263 } else {
264 int cur = 0;
265 for (unsigned int i = 1; i < m_Circles.size(); ++i) {
266 if (m_Circles[i].count > m_Circles[cur].count) {
267 cur = i;
268 }
269 }
270 return const_cast<Circle *>(&m_Circles[cur]); // or use const Shape* definition?!...
271 }
272}
273
274void
275RhtCircleModel::calcCircle(const upoint_t & p1,
276 const upoint_t & p2,
277 const upoint_t & p3,
278 center_in_roi_t &center,
279 float & radius)
280// Given three points p1, p2, p3,
281// this function calculates the center and radius
282// of the circle that is determined
283{
284 const int &x1 = p1.x, &y1 = p1.y, &x2 = p2.x, &y2 = p2.y, &x3 = p3.x, &y3 = p3.y;
285 float dx, dy;
286 int div = 2 * ((x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1));
287
288 if (div == 0) {
289 // p1, p2 and p3 are in a straight line.
290 radius = -1.0;
291 return;
292 }
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))
295 / div);
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))
298 / div);
299 dx = center.x - x1;
300 dy = center.y - y1;
301 radius = (float)sqrt(dx * dx + dy * dy);
302}
303
304} // 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
int getMax(int &x, int &y, int &r) const
Get maximum.
Definition: ht_accum.cpp:491
void reset(void)
Reset.
Definition: ht_accum.cpp:451
int accumulate(int x, int y, int r)
Accumulate new candidate.
Definition: ht_accum.cpp:468
Circle * getShape(int id) const
Get specific shape.
Definition: rht_circle.cpp:247
int getShapeCount(void) const
Get number of shapes.
Definition: rht_circle.cpp:241
Circle * getMostLikelyShape(void) const
Get best candidate.
Definition: rht_circle.cpp:257
int parseImage(unsigned char *buffer, ROI *roi)
Parse image for given ROI.
Definition: rht_circle.cpp:67
virtual ~RhtCircleModel(void)
Destructor.
Definition: rht_circle.cpp:56
RhtCircleModel(void)
Constructor.
Definition: rht_circle.cpp:51
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