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