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