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 
31 using namespace std;
32 using namespace fawkes;
33 
34 namespace 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  */
63 RcdCircleModel::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. */
83 RcdCircleModel::~RcdCircleModel(void)
84 {
85  m_Circles.clear();
86 }
87 
88 int
89 RcdCircleModel::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 
306 int
307 RcdCircleModel::getShapeCount(void) const
308 {
309  return m_Circles.size();
310 }
311 
312 Circle *
313 RcdCircleModel::getShape(int id) const
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 
322 Circle *
323 RcdCircleModel::getMostLikelyShape(void) const
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 
337 void
338 RcdCircleModel::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
firevision::ROI::width
unsigned int width
ROI width.
Definition: roi.h:123
firevision::Circle
Definition: circle.h:49
fawkes::upoint_t
Point with cartesian coordinates as unsigned integers.
Definition: types.h:34
firevision::Circle::fitCircle
void fitCircle(std::vector< fawkes::upoint_t > &points)
Fit circle.
Definition: circle.cpp:73
firevision::ROI
Definition: roi.h:60
firevision::ROI::height
unsigned int height
ROI height.
Definition: roi.h:125
fawkes
fawkes::upoint_t::y
unsigned int y
y coordinate
Definition: types.h:37
firevision::Circle::count
int count
Number of pixels.
Definition: circle.h:68
firevision::center_in_roi_t::x
float x
x in pixels
Definition: types.h:53
firevision::center_in_roi_t::y
float y
y in pixels
Definition: types.h:54
firevision::center_in_roi_t
Center in ROI.
Definition: types.h:44
firevision::ROI::line_step
unsigned int line_step
line step
Definition: roi.h:131
fawkes::upoint_t::x
unsigned int x
x coordinate
Definition: types.h:36
firevision::ROI::get_roi_buffer_start
unsigned char * get_roi_buffer_start(unsigned char *buffer) const
Get ROI buffer start.
Definition: roi.cpp:526