Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * rcd_circle.cpp - Implementation of a circle shape finder 00004 * 00005 * Created: Thu May 16 00:00:00 2005 00006 * Copyright 2005 Tim Niemueller [www.niemueller.de] 00007 * Hu Yuxiao <Yuxiao.Hu@rwth-aachen.de> 00008 * 00009 ****************************************************************************/ 00010 00011 /* This program is free software; you can redistribute it and/or modify 00012 * it under the terms of the GNU General Public License as published by 00013 * the Free Software Foundation; either version 2 of the License, or 00014 * (at your option) any later version. A runtime exception applies to 00015 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00016 * 00017 * This program is distributed in the hope that it will be useful, 00018 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00019 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00020 * GNU Library General Public License for more details. 00021 * 00022 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00023 */ 00024 00025 #include <models/shape/rcd_circle.h> 00026 #include <cmath> 00027 #include <sys/time.h> 00028 #include <cstdlib> 00029 00030 using namespace std; 00031 using namespace fawkes; 00032 00033 namespace firevision { 00034 #if 0 /* just to make Emacs auto-indent happy */ 00035 } 00036 #endif 00037 00038 #define TBY_GRAYSCALE 00039 #ifdef TBY_GRAYSCALE 00040 #define TEST_IF_IS_A_PIXEL(x) ((x)>230) 00041 #else 00042 #define TEST_IF_IS_A_PIXEL(x) ((x)==0) 00043 #endif // TBY_GRAYSCALE 00044 00045 #define TBY_SQUARED_DIST(x1,y1,x2,y2) \ 00046 (((x1)-(x2))*((x1)-(x2))+((y1)-(y2))*((y1)-(y2))) 00047 #define TBY_RADIUS_DIFF(x1, y1, x2, y2, r) \ 00048 (((x1)-(x2))*((x1)-(x2))+((y1)-(y2))*((y1)-(y2))-(r)*(r)) 00049 00050 /** @class RcdCircleModel <models/shape/rcd_circle.h> 00051 * RCD circle model from the following literature 00052 * An Efficient Randomized Algorithm for Detecting Circles 00053 */ 00054 00055 /** Create a new circle model which uses RCD to detect circles 00056 * @param max_failures Max. number of failures 00057 * @param min_pixels Min number of available edge pixels 00058 * @param min_interpix_dist Min distance between chosen pixels 00059 * @param max_dist_p4 Max. distance of fourth pixel to circle 00060 * @param max_dist_a Max. distance for all other pixels to circle 00061 * @param hw_ratio Ratio height/width 00062 * @param hollow_rate size of the hollow window in the ROI. 00063 * @param max_time Maximum runtime per loop 00064 */ 00065 RcdCircleModel::RcdCircleModel(unsigned int max_failures, 00066 unsigned int min_pixels, 00067 unsigned int min_interpix_dist, 00068 unsigned int max_dist_p4, 00069 unsigned int max_dist_a, 00070 float hw_ratio, 00071 float hollow_rate, 00072 float max_time 00073 ) 00074 { 00075 00076 RCD_MAX_FAILURES = max_failures; 00077 RCD_MIN_PIXELS = min_pixels; 00078 RCD_MIN_INTERPIX_DIST = min_interpix_dist; 00079 RCD_MAX_DIST_P4 = max_dist_p4; 00080 RCD_MAX_DIST_A = max_dist_a; 00081 RCD_HW_RATIO = hw_ratio; 00082 RCD_MAX_TIME = max_time; 00083 RCD_ROI_HOLLOW_RATE = hollow_rate; 00084 00085 } 00086 00087 /** Destrcutor. */ 00088 RcdCircleModel::~RcdCircleModel(void) 00089 { 00090 m_Circles.clear(); 00091 } 00092 00093 int RcdCircleModel::parseImage( unsigned char* buf, 00094 ROI *roi ) 00095 { 00096 00097 unsigned char *buffer = roi->get_roi_buffer_start(buf); 00098 unsigned char *line_start = buffer; 00099 00100 unsigned int x, y; 00101 vector<point_t> pixels, 00102 remove_list; 00103 unsigned int f = 0; // number of failures 00104 int count; // number of pixels on the circle 00105 int num_circles = 0; 00106 struct timeval start, now; 00107 00108 // clear all the remembered circles 00109 m_Circles.clear(); 00110 00111 const unsigned int roi_hollow_top = (int)(roi->height * ((1.0f - RCD_ROI_HOLLOW_RATE) / 2)); 00112 const unsigned int roi_hollow_bottom = roi->height - roi_hollow_top; 00113 const unsigned int roi_hollow_left = (int)(roi->width * ((1.0f - RCD_ROI_HOLLOW_RATE) / 2)); 00114 const unsigned int roi_hollow_right = roi->width - roi_hollow_left; 00115 00116 // First, find all the pixels on the edges, 00117 // and store them in the 'pixels' vector. 00118 // NEW: excluding the hollow window 00119 buffer = roi->get_roi_buffer_start(buf); 00120 line_start = buffer; 00121 00122 // Find the boundary of the ball, 00123 // following used for ball pixel threshold. 00124 unsigned int boundary_right = 0; 00125 unsigned int boundary_bottom = 0; 00126 00127 gettimeofday(&start, NULL); 00128 00129 pixels.clear(); 00130 00131 // top "1/3" 00132 for (y = 0; y < roi_hollow_top; ++y) { 00133 for (x = 0; x < roi->width; ++x) { 00134 if (TEST_IF_IS_A_PIXEL(*buffer)) { 00135 point_t pt={x, y}; 00136 pixels.push_back(pt); 00137 if (x > boundary_right) boundary_right = x; 00138 boundary_bottom = y; 00139 } 00140 // NOTE: this assumes roi->pixel_step == 1 00141 ++buffer; 00142 } 00143 line_start += roi->line_step; 00144 buffer = line_start; 00145 } 00146 // middle "1/3" 00147 for (y = roi_hollow_top; y < roi_hollow_bottom; ++y) { 00148 for (x = 0; x < roi_hollow_left; ++x) { 00149 if (TEST_IF_IS_A_PIXEL(*buffer)) { 00150 point_t pt={x, y}; 00151 pixels.push_back(pt); 00152 if (x > boundary_right) boundary_right = x; 00153 boundary_bottom = y; 00154 } 00155 // NOTE: this assumes roi->pixel_step == 1 00156 ++buffer; 00157 } 00158 buffer+=(roi_hollow_right - roi_hollow_left); 00159 for (x = roi_hollow_right; x < roi->width; ++x) { 00160 if (TEST_IF_IS_A_PIXEL(*buffer)) { 00161 point_t pt={x, y}; 00162 pixels.push_back(pt); 00163 if (x > boundary_right) boundary_right = x; 00164 boundary_bottom = y; 00165 } 00166 // NOTE: this assumes roi->pixel_step == 1 00167 ++buffer; 00168 } 00169 line_start += roi->line_step; 00170 buffer = line_start; 00171 } 00172 // bottom "1/3" 00173 for (y = roi_hollow_bottom; y < roi->height; ++y) { 00174 for (x = 0; x < roi->width; ++x) { 00175 if (TEST_IF_IS_A_PIXEL(*buffer)) { 00176 point_t pt={x, y}; 00177 pixels.push_back(pt); 00178 } 00179 // NOTE: this assumes roi->pixel_step == 1 00180 ++buffer; 00181 } 00182 line_start += roi->line_step; 00183 buffer = line_start; 00184 } 00185 00186 // Then perform the RCD algorithm 00187 point_t p[4]; 00188 center_in_roi_t center; 00189 float radius; 00190 vector< point_t >::iterator pos; 00191 00192 if (pixels.size() < RCD_MIN_PIXELS) { 00193 return 0; 00194 } 00195 00196 do { 00197 00198 // Pick four points, and move them to the remove_list. 00199 for (int i=0; i < 4; ++i) { 00200 int ri = rand() % ((int)pixels.size()); 00201 pos = pixels.begin() + ri; 00202 p[i] = *pos; // use * operator of iterator 00203 pixels.erase(pos); 00204 remove_list.push_back(p[i]); 00205 } 00206 00207 if (TBY_SQUARED_DIST(p[1].x, p[1].y, p[2].x, p[2].y) < RCD_MIN_INTERPIX_DIST || 00208 TBY_SQUARED_DIST(p[2].x, p[2].y, p[0].x, p[0].y) < RCD_MIN_INTERPIX_DIST || 00209 TBY_SQUARED_DIST(p[0].x, p[0].y, p[1].x, p[1].y) < RCD_MIN_INTERPIX_DIST ) { 00210 // there are two points that are too near 00211 // to each other 00212 ++f; 00213 00214 // restore all the pixels in remove_list to pixels 00215 pixels.push_back(p[0]); 00216 pixels.push_back(p[1]); 00217 pixels.push_back(p[2]); 00218 pixels.push_back(p[3]); 00219 00220 remove_list.clear(); 00221 00222 gettimeofday(&now, NULL); 00223 continue; 00224 } 00225 00226 // Now calculate the center and radius 00227 // based on the first three points. 00228 calcCircle(p[0], p[1], p[2], center, radius); 00229 00230 // Test if the fourth point on this circle 00231 int r = (int)sqrt(TBY_SQUARED_DIST(center.x, center.y, pixels[3].x, pixels[3].y)); 00232 int dist = (int)(r - radius); 00233 dist = (dist >= 0) ? dist : -dist; 00234 if (radius <= 0 || (unsigned int)dist > RCD_MAX_DIST_P4 ) { 00235 ++f; 00236 00237 //restore all the pixels 00238 pixels.push_back(p[0]); 00239 pixels.push_back(p[1]); 00240 pixels.push_back(p[2]); 00241 pixels.push_back(p[3]); 00242 00243 remove_list.clear(); 00244 00245 gettimeofday(&now, NULL); 00246 continue; 00247 } 00248 00249 // count how many pixels are on the circle 00250 count=0; 00251 for (unsigned int i=0; i < pixels.size(); ++i) { 00252 int r = (int)sqrt(TBY_SQUARED_DIST(center.x, center.y, pixels[i].x, pixels[i].y)); 00253 int dist = (int)(r-radius); 00254 dist = (dist >= 0) ? dist : -dist; 00255 if ((unsigned int)dist <= RCD_MAX_DIST_A) { 00256 pos = pixels.begin() + i; 00257 ++count; 00258 // move this pixel to the remove_list 00259 remove_list.push_back(pixels[i]); 00260 pixels.erase(pos--); // need "--"? not sure yet! 00261 } 00262 } 00263 00264 // test if there are enough points on the circle 00265 // to convince us that this is indeed a circle 00266 if ( (float)count > 00267 ( boundary_right > boundary_bottom ? boundary_right : boundary_bottom ) * RCD_HW_RATIO ) { 00268 // this is indeed a circle 00269 if ( radius > TBY_CIRCLE_RADIUS_MIN && 00270 radius < TBY_CIRCLE_RADIUS_MAX ) { 00271 // only ball of size in the range are saved in the candidate pool. 00272 00273 // use least square fitting to reduce error 00274 Circle c; 00275 c.fitCircle(remove_list); 00276 c.count = count; 00277 00278 // save circle 00279 m_Circles.push_back(c); 00280 } 00281 remove_list.clear(); 00282 ++num_circles; 00283 } else { 00284 // not a circle, charge a failure 00285 ++f; 00286 00287 while(!remove_list.empty()) { 00288 pixels.push_back(remove_list.back()); 00289 remove_list.pop_back(); 00290 } 00291 gettimeofday(&now, NULL); 00292 continue; 00293 } 00294 00295 gettimeofday(&now, NULL); 00296 00297 diff_sec = now.tv_sec - start.tv_sec; 00298 diff_usec = now.tv_usec - start.tv_usec; 00299 if (diff_usec < 0) { 00300 diff_sec -= 1; 00301 diff_usec += 1000000; 00302 } 00303 00304 f_diff_sec = diff_sec + diff_usec / 1000000.f; 00305 00306 } while( (f < RCD_MAX_FAILURES) && (pixels.size() > RCD_MIN_PIXELS) && 00307 ( f_diff_sec < RCD_MAX_TIME) ); 00308 00309 return num_circles; 00310 } 00311 00312 int RcdCircleModel::getShapeCount(void) const 00313 { 00314 return m_Circles.size(); 00315 } 00316 00317 Circle* RcdCircleModel::getShape(int id) const 00318 { 00319 if (id < 0 || (unsigned int)id >= m_Circles.size()) 00320 { 00321 return NULL; 00322 } 00323 else 00324 { 00325 return const_cast<Circle*>(&m_Circles[id]); // or use const Shape* def?!... 00326 } 00327 } 00328 00329 Circle* RcdCircleModel::getMostLikelyShape(void) const 00330 { 00331 int cur=0; 00332 switch (m_Circles.size()) 00333 { 00334 case 0: 00335 return NULL; 00336 case 1: 00337 return const_cast<Circle*>(&m_Circles[0]); // or use const Shape* def?!... 00338 default: 00339 for (unsigned int i=1; i < m_Circles.size(); ++i) 00340 if (m_Circles[i].radius > m_Circles[cur].radius) 00341 cur = i; 00342 return const_cast<Circle*>(&m_Circles[cur]); // or use const Shape* definition?!... 00343 } 00344 } 00345 00346 void RcdCircleModel::calcCircle( 00347 const point_t& p1, 00348 const point_t& p2, 00349 const point_t& p3, 00350 center_in_roi_t& center, 00351 float& radius) 00352 // Given three points p1, p2, p3, 00353 // this function calculates the center and radius 00354 // of the circle that is determined 00355 { 00356 const int &x1=p1.x, &y1=p1.y, &x2=p2.x, &y2=p2.y, &x3=p3.x, &y3=p3.y; 00357 float dx, dy; 00358 int div = 2*((x2-x1)*(y3-y1)-(x3-x1)*(y2-y1)); 00359 00360 if (div == 0) 00361 { 00362 // p1, p2 and p3 are in a straight line. 00363 radius = -1.0; 00364 return; 00365 } 00366 center.x = ((float)((x2*x2+y2*y2-x1*x1-y1*y1)*(y3-y1) 00367 -(x3*x3+y3*y3-x1*x1-y1*y1)*(y2-y1)) 00368 /div); 00369 center.y = ((float)((x2-x1)*(x3*x3+y3*y3-x1*x1-y1*y1) 00370 -(x3-x1)*(x2*x2+y2*y2-x1*x1-y1*y1)) 00371 /div); 00372 dx = center.x - x1; 00373 dy = center.y - y1; 00374 radius = (float)sqrt(dx*dx+dy*dy); 00375 } 00376 00377 } // end namespace firevision