Fawkes API Fawkes Development Version

rcd_circle.cpp

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
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends