Fawkes API  Fawkes Development Version
rht_circle.cpp
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 <models/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 <models/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