Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * rht_circle.cpp - Implementation of a circle shape finder 00004 * with Randomized Hough Transform 00005 * 00006 * Created: Tue Jun 28 00:00:00 2005 00007 * Copyright 2005 Hu Yuxiao <Yuxiao.Hu@rwth-aachen.de> 00008 * Tim Niemueller [www.niemueller.de] 00009 * 00010 ****************************************************************************/ 00011 00012 /* This program is free software; you can redistribute it and/or modify 00013 * it under the terms of the GNU General Public License as published by 00014 * the Free Software Foundation; either version 2 of the License, or 00015 * (at your option) any later version. A runtime exception applies to 00016 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00017 * 00018 * This program is distributed in the hope that it will be useful, 00019 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00020 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00021 * GNU Library General Public License for more details. 00022 * 00023 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00024 */ 00025 00026 #include <fvmodels/shape/rht_circle.h> 00027 00028 #include <cmath> 00029 #include <sys/time.h> 00030 00031 using namespace std; 00032 using namespace fawkes; 00033 00034 #define TEST_IF_IS_A_PIXEL(x) ((x)>230) 00035 00036 #define TBY_SQUARED_DIST(x1,y1,x2,y2) \ 00037 (((x1)-(x2))*((x1)-(x2))+((y1)-(y2))*((y1)-(y2))) 00038 #define TBY_RADIUS_DIFF(x1, y1, x2, y2, r) \ 00039 (sqrt(((x1)-(x2))*((x1)-(x2))+((y1)-(y2))*((y1)-(y2))-(r)*(r))) 00040 00041 00042 namespace firevision { 00043 #if 0 /* just to make Emacs auto-indent happy */ 00044 } 00045 #endif 00046 00047 const float RhtCircleModel::RHT_MIN_RADIUS = 40.0; 00048 const float RhtCircleModel::RHT_MAX_RADIUS = 500.0; 00049 00050 /** @class RhtCircleModel <fvmodels/shape/rht_circle.h> 00051 * Randomized Hough-Transform circle model. 00052 */ 00053 00054 /** Constructor. */ 00055 RhtCircleModel::RhtCircleModel(void) 00056 { 00057 } 00058 00059 00060 /** Destructor. */ 00061 RhtCircleModel::~RhtCircleModel(void) 00062 { 00063 m_Circles.clear(); 00064 } 00065 00066 /************************************************************** 00067 * In this function I implement the circle detection algorithm 00068 * from the following literature 00069 * Randomized Hough Transform 00070 **************************************************************/ 00071 int 00072 RhtCircleModel::parseImage( unsigned char* buf, 00073 ROI *roi ) 00074 { 00075 00076 unsigned char *buffer = roi->get_roi_buffer_start(buf); 00077 00078 unsigned int x, y; 00079 vector<point_t> pixels; 00080 struct timeval start, end; 00081 00082 // clear the accumulator 00083 accumulator.reset(); 00084 00085 // clear all the remembered circles 00086 m_Circles.clear(); 00087 00088 // The following constants are used as stopping criteria 00089 const int RHT_MAX_TIME = 10000; // = 10ms (is given in microseconds) 00090 const int RHT_MAX_ITER = 1000; // Maximal number of iterations. 00091 00092 // The following constant is used for eliminating circles with too few votes 00093 const float RHT_MIN_VOTE_RATE = 0.0f; 00094 00095 // The following constants are used for RHT accumulator precision 00096 const int RHT_XY_SCALE = 8; 00097 const int RHT_RADIUS_SCALE= 8; 00098 00099 // All the pixels with a less distance difference than below 00100 // are taken into account for circle fitting. 00101 const float RHT_FITTING_DIST_DIFF = 15.0f; 00102 00103 // The following constant is used for the size of the hollow window in the ROI. 00104 const float ROI_HOLLOW_RATE = 0.70f; 00105 00106 const unsigned int roi_hollow_top = (int)(roi->height * ((1.0f - ROI_HOLLOW_RATE) / 2)); 00107 const unsigned int roi_hollow_bottom = roi->height - roi_hollow_top; 00108 const unsigned int roi_hollow_left = (int)(roi->width * ((1.0f - ROI_HOLLOW_RATE) / 2)); 00109 const unsigned int roi_hollow_right = roi->width - roi_hollow_left; 00110 00111 // First, find all the pixels on the edges, 00112 // and store them in the 'pixels' vector. 00113 // NEW: excluding the hollow window 00114 unsigned char *line_start = buffer; 00115 00116 gettimeofday(&start, NULL); 00117 end.tv_usec = start.tv_usec; 00118 00119 // top "1/3" 00120 for (y = 0; y < roi_hollow_top; ++y) { 00121 for (x = 0; x < roi->width; ++x) { 00122 if (TEST_IF_IS_A_PIXEL(*buffer)) { 00123 point_t pt={x, y}; 00124 pixels.push_back(pt); 00125 } 00126 // NOTE: this assumes roi->pixel_step == 1 00127 ++buffer; 00128 } 00129 line_start += roi->line_step; 00130 buffer = line_start; 00131 } 00132 // middle "1/3" 00133 for (y = roi_hollow_top; y < roi_hollow_bottom; ++y) { 00134 for (x = 0; x < roi_hollow_left; ++x) { 00135 if (TEST_IF_IS_A_PIXEL(*buffer)) { 00136 point_t pt={x, y}; 00137 pixels.push_back(pt); 00138 } 00139 // NOTE: this assumes roi->pixel_step == 1 00140 ++buffer; 00141 } 00142 buffer+=(roi_hollow_right - roi_hollow_left); 00143 for (x = roi_hollow_right; x < roi->width; ++x) { 00144 if (TEST_IF_IS_A_PIXEL(*buffer)) { 00145 point_t pt={x, y}; 00146 pixels.push_back(pt); 00147 } 00148 // NOTE: this assumes roi->pixel_step == 1 00149 ++buffer; 00150 } 00151 line_start += roi->line_step; 00152 buffer = line_start; 00153 } 00154 // bottom "1/3" 00155 for (y = roi_hollow_bottom; y < roi->height; ++y) { 00156 for (x = 0; x < roi->width; ++x) { 00157 if (TEST_IF_IS_A_PIXEL(*buffer)) { 00158 point_t pt={x, y}; 00159 pixels.push_back(pt); 00160 } 00161 // NOTE: this assumes roi->pixel_step == 1 00162 ++buffer; 00163 } 00164 line_start += roi->line_step; 00165 buffer = line_start; 00166 } 00167 00168 // Then perform the RHT algorithm 00169 point_t p[3]; 00170 center_in_roi_t center; 00171 float radius; 00172 vector< point_t >::iterator pos; 00173 int num_iter = 0; 00174 int num_points = (int) pixels.size(); 00175 if (num_points == 0) { 00176 // No pixels found => no edge => no circle 00177 return 0; 00178 } 00179 00180 while( (num_iter++ < RHT_MAX_ITER) && 00181 ( ((end.tv_usec - start.tv_usec) < RHT_MAX_TIME) || 00182 ((end.tv_usec + 1000000 - start.tv_usec) < RHT_MAX_TIME) ) 00183 // this only works if time constraint smaller than 500ms 00184 ) { 00185 00186 // Pick three points, and move them to the remove_list. 00187 for (int i=0; i < 3; ++i) { 00188 int ri = rand() % num_points; 00189 pos = pixels.begin() + ri; 00190 p[i] = *pos; // use * operator of iterator 00191 } 00192 00193 // Now calculate the center and radius 00194 // based on the three points. 00195 calcCircle(p[0], p[1], p[2], center, radius); 00196 00197 // Accumulate this circle to the 3-D space... 00198 if (radius > RHT_MIN_RADIUS && radius < RHT_MAX_RADIUS) { 00199 accumulator.accumulate((int)(center.x / RHT_XY_SCALE), 00200 (int)(center.y / RHT_XY_SCALE), 00201 (int)(radius / RHT_RADIUS_SCALE)); 00202 } 00203 00204 gettimeofday(&end, NULL); 00205 } 00206 00207 // Find the most dense region, and decide on the ball. 00208 int max, x_max, y_max, r_max; 00209 max = accumulator.getMax(x_max, y_max, r_max); 00210 00211 cout << "Max vote is with " << max << " of the " << num_iter << " ones." << endl; 00212 00213 // Only when votes exceeds a ratio will it be considered a real circle 00214 if (max > num_iter * RHT_MIN_VOTE_RATE) 00215 { 00216 center_in_roi_t center; 00217 center.x = (float)(x_max * RHT_XY_SCALE + RHT_XY_SCALE/2); 00218 center.y = (float)(y_max * RHT_XY_SCALE + RHT_XY_SCALE/2); 00219 float c_r = (float)(r_max * RHT_RADIUS_SCALE + RHT_RADIUS_SCALE/2); 00220 00221 // With circle fitting 00222 for(vector< point_t >::iterator pos = pixels.begin(); pos != pixels.end(); ) 00223 { 00224 if (TBY_RADIUS_DIFF(pos->x, pos->y, center.x, center.y, c_r) 00225 > RHT_FITTING_DIST_DIFF) 00226 { 00227 pixels.erase(pos); 00228 } 00229 else 00230 { 00231 pos++; 00232 } 00233 } 00234 00235 Circle c; 00236 c.fitCircle(pixels); 00237 c.count = max; 00238 m_Circles.push_back(c); 00239 00240 /* 00241 // Without circle fitting 00242 m_Circles.push_back(Circle(center, c_r, max)); 00243 */ 00244 00245 return 0; 00246 } 00247 00248 // Return... here in this algorithm, we only find at most one most likely circle, 00249 // (if none found, return-ed above) 00250 // so the return value here is always 1 00251 return 1; 00252 } 00253 00254 00255 int 00256 RhtCircleModel::getShapeCount(void) const 00257 { 00258 return m_Circles.size(); 00259 } 00260 00261 Circle * 00262 RhtCircleModel::getShape(int id) const 00263 { 00264 if (id < 0 || (unsigned int)id >= m_Circles.size()) { 00265 return NULL; 00266 } else { 00267 return const_cast<Circle*>(&m_Circles[id]); // or use const Shape* def?!... 00268 } 00269 } 00270 00271 00272 Circle * 00273 RhtCircleModel::getMostLikelyShape(void) const 00274 { 00275 if (m_Circles.size() == 0) { 00276 return NULL; 00277 } else if (m_Circles.size() == 1) { 00278 return const_cast<Circle*>(&m_Circles[0]); // or use const Shape* def?!... 00279 } else { 00280 int cur=0; 00281 for (unsigned int i=1; i < m_Circles.size(); ++i) { 00282 if (m_Circles[i].count > m_Circles[cur].count) { 00283 cur = i; 00284 } 00285 } 00286 return const_cast<Circle*>(&m_Circles[cur]); // or use const Shape* definition?!... 00287 } 00288 } 00289 00290 00291 void 00292 RhtCircleModel::calcCircle( 00293 const point_t& p1, 00294 const point_t& p2, 00295 const point_t& p3, 00296 center_in_roi_t& center, 00297 float& radius) 00298 // Given three points p1, p2, p3, 00299 // this function calculates the center and radius 00300 // of the circle that is determined 00301 { 00302 const int &x1=p1.x, &y1=p1.y, &x2=p2.x, &y2=p2.y, &x3=p3.x, &y3=p3.y; 00303 float dx, dy; 00304 int div = 2*((x2-x1)*(y3-y1)-(x3-x1)*(y2-y1)); 00305 00306 if (div == 0) 00307 { 00308 // p1, p2 and p3 are in a straight line. 00309 radius = -1.0; 00310 return; 00311 } 00312 center.x = ((float)((x2*x2+y2*y2-x1*x1-y1*y1)*(y3-y1) 00313 -(x3*x3+y3*y3-x1*x1-y1*y1)*(y2-y1)) 00314 /div); 00315 center.y = ((float)((x2-x1)*(x3*x3+y3*y3-x1*x1-y1*y1) 00316 -(x3-x1)*(x2*x2+y2*y2-x1*x1-y1*y1)) 00317 /div); 00318 dx = center.x - x1; 00319 dy = center.y - y1; 00320 radius = (float)sqrt(dx*dx+dy*dy); 00321 } 00322 00323 } // end namespace firevision