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