modules/imgproc/src/hough.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 #include "precomp.hpp"
  43 #include "_list.h"
  44
  45 #define halfPi ((float)(CV_PI*0.5))
  46 #define Pi     ((float)CV_PI)
  47 #define a0  0 /*-4.172325e-7f*/   /*(-(float)0x7)/((float)0x1000000); */
  48 #define a1 1.000025f        /*((float)0x1922253)/((float)0x1000000)*2/Pi; */
  49 #define a2 -2.652905e-4f    /*(-(float)0x2ae6)/((float)0x1000000)*4/(Pi*Pi); */
  50 #define a3 -0.165624f       /*(-(float)0xa45511)/((float)0x1000000)*8/(Pi*Pi*Pi); */
  51 #define a4 -1.964532e-3f    /*(-(float)0x30fd3)/((float)0x1000000)*16/(Pi*Pi*Pi*Pi); */
  52 #define a5 1.02575e-2f      /*((float)0x191cac)/((float)0x1000000)*32/(Pi*Pi*Pi*Pi*Pi); */
  53 #define a6 -9.580378e-4f    /*(-(float)0x3af27)/((float)0x1000000)*64/(Pi*Pi*Pi*Pi*Pi*Pi); */
  54
  55 #define _sin(x) ((((((a6*(x) + a5)*(x) + a4)*(x) + a3)*(x) + a2)*(x) + a1)*(x) + a0)
  56 #define _cos(x) _sin(halfPi - (x))
  57
  58 /****************************************************************************************\
  59 *                               Classical Hough Transform                                *
  60 \****************************************************************************************/
  61
  62 typedef struct CvLinePolar
  63 {
  64     float rho;
  65     float angle;
  66 }
  67 CvLinePolar;
  68
  69 /*=====================================================================================*/
  70
  71 #define hough_cmp_gt(l1,l2) (aux[l1] > aux[l2])
  72
  73 static CV_IMPLEMENT_QSORT_EX( icvHoughSortDescent32s, int, hough_cmp_gt, const int* )
  74
  75 /*
  76 Here image is an input raster;
  77 step is it's step; size characterizes it's ROI;
  78 rho and theta are discretization steps (in pixels and radians correspondingly).
  79 threshold is the minimum number of pixels in the feature for it
  80 to be a candidate for line. lines is the output
  81 array of (rho, theta) pairs. linesMax is the buffer size (number of pairs).
  82 Functions return the actual number of found lines.
  83 */
  84 static void
  85 icvHoughLinesStandard( const CvMat* img, float rho, float theta,
  86                        int threshold, CvSeq *lines, int linesMax )
  87 {
  88     cv::AutoBuffer<int> _accum, _sort_buf;
  89     cv::AutoBuffer<float> _tabSin, _tabCos;
  90
  91     const uchar* image;
  92     int step, width, height;
  93     int numangle, numrho;
  94     int total = 0;
  95     int i, j;
  96     float irho = 1 / rho;
  97     double scale;
  98
  99     CV_Assert( CV_IS_MAT(img) && CV_MAT_TYPE(img->type) == CV_8UC1 );
 100
 101     image = img->data.ptr;
 102     step = img->step;
 103     width = img->cols;
 104     height = img->rows;
 105
 106     numangle = cvRound(CV_PI / theta);
 107     numrho = cvRound(((width + height) * 2 + 1) / rho);
 108
 109     _accum.allocate((numangle+2) * (numrho+2));
 110     _sort_buf.allocate(numangle * numrho);
 111     _tabSin.allocate(numangle);
 112     _tabCos.allocate(numangle);
 113     int *accum = _accum, *sort_buf = _sort_buf;
 114     float *tabSin = _tabSin, *tabCos = _tabCos;
 115
 116     memset( accum, 0, sizeof(accum[0]) * (numangle+2) * (numrho+2) );
 117
 118     float ang = 0;
 119     for(int n = 0; n < numangle; ang += theta, n++ )
 120     {
 121         tabSin[n] = (float)(sin((double)ang) * irho);
 122         tabCos[n] = (float)(cos((double)ang) * irho);
 123     }
 124
 125     // stage 1. fill accumulator
 126     for( i = 0; i < height; i++ )
 127         for( j = 0; j < width; j++ )
 128         {
 129             if( image[i * step + j] != 0 )
 130                 for(int n = 0; n < numangle; n++ )
 131                 {
 132                     int r = cvRound( j * tabCos[n] + i * tabSin[n] );
 133                     r += (numrho - 1) / 2;
 134                     accum[(n+1) * (numrho+2) + r+1]++;
 135                 }
 136         }
 137
 138     // stage 2. find local maximums
 139     for(int r = 0; r < numrho; r++ )
 140         for(int n = 0; n < numangle; n++ )
 141         {
 142             int base = (n+1) * (numrho+2) + r+1;
 143             if( accum[base] > threshold &&
 144                 accum[base] > accum[base - 1] && accum[base] >= accum[base + 1] &&
 145                 accum[base] > accum[base - numrho - 2] && accum[base] >= accum[base + numrho + 2] )
 146                 sort_buf[total++] = base;
 147         }
 148
 149     // stage 3. sort the detected lines by accumulator value
 150     icvHoughSortDescent32s( sort_buf, total, accum );
 151
 152     // stage 4. store the first min(total,linesMax) lines to the output buffer
 153     linesMax = MIN(linesMax, total);
 154     scale = 1./(numrho+2);
 155     for( i = 0; i < linesMax; i++ )
 156     {
 157         CvLinePolar line;
 158         int idx = sort_buf[i];
 159         int n = cvFloor(idx*scale) - 1;
 160         int r = idx - (n+1)*(numrho+2) - 1;
 161         line.rho = (r - (numrho - 1)*0.5f) * rho;
 162         line.angle = n * theta;
 163         cvSeqPush( lines, &line );
 164     }
 165 }
 166
 167
 168 /****************************************************************************************\
 169 *                     Multi-Scale variant of Classical Hough Transform                   *
 170 \****************************************************************************************/
 171
 172 //DECLARE_AND_IMPLEMENT_LIST( _index, h_ );
 173 IMPLEMENT_LIST( _index, h_ )
 174
 175 static void
 176 icvHoughLinesSDiv( const CvMat* img,
 177                    float rho, float theta, int threshold,
 178                    int srn, int stn,
 179                    CvSeq* lines, int linesMax )
 180 {
 181     std::vector<uchar> _caccum, _buffer;
 182     std::vector<float> _sinTable;
 183     std::vector<int> _x, _y;
 184     float* sinTable;
 185     int *x, *y;
 186     uchar *caccum, *buffer;
 187     _CVLIST* list = 0;
 188
 189 #define _POINT(row, column)\
 190     (image_src[(row)*step+(column)])
 191
 192     uchar *mcaccum = 0;
 193     int rn, tn;                 /* number of rho and theta discrete values */
 194     int index, i;
 195     int ri, ti, ti1, ti0;
 196     int row, col;
 197     float r, t;                 /* Current rho and theta */
 198     float rv;                   /* Some temporary rho value */
 199     float irho;
 200     float itheta;
 201     float srho, stheta;
 202     float isrho, istheta;
 203
 204     const uchar* image_src;
 205     int w, h, step;
 206     int fn = 0;
 207     float xc, yc;
 208
 209     const float d2r = (float)(Pi / 180);
 210     int sfn = srn * stn;
 211     int fi;
 212     int count;
 213     int cmax = 0;
 214
 215     CVPOS pos;
 216     _index *pindex;
 217     _index vi;
 218
 219     CV_Assert( CV_IS_MAT(img) && CV_MAT_TYPE(img->type) == CV_8UC1 );
 220     CV_Assert( linesMax > 0 && rho > 0 && theta > 0 );
 221
 222     threshold = MIN( threshold, 255 );
 223
 224     image_src = img->data.ptr;
 225     step = img->step;
 226     w = img->cols;
 227     h = img->rows;
 228
 229     irho = 1 / rho;
 230     itheta = 1 / theta;
 231     srho = rho / srn;
 232     stheta = theta / stn;
 233     isrho = 1 / srho;
 234     istheta = 1 / stheta;
 235
 236     rn = cvFloor( sqrt( (double)w * w + (double)h * h ) * irho );
 237     tn = cvFloor( 2 * Pi * itheta );
 238
 239     list = h_create_list__index( linesMax < 1000 ? linesMax : 1000 );
 240     vi.value = threshold;
 241     vi.rho = -1;
 242     h_add_head__index( list, &vi );
 243
 244     /* Precalculating sin */
 245     _sinTable.resize( 5 * tn * stn );
 246     sinTable = &_sinTable[0];
 247
 248     for( index = 0; index < 5 * tn * stn; index++ )
 249         sinTable[index] = (float)cos( stheta * index * 0.2f );
 250
 251     _caccum.resize(rn * tn);
 252     caccum = &_caccum[0];
 253     memset( caccum, 0, rn * tn * sizeof( caccum[0] ));
 254
 255     /* Counting all feature pixels */
 256     for( row = 0; row < h; row++ )
 257         for( col = 0; col < w; col++ )
 258             fn += _POINT( row, col ) != 0;
 259
 260     _x.resize(fn);
 261     _y.resize(fn);
 262     x = &_x[0];
 263     y = &_y[0];
 264
 265     /* Full Hough Transform (it's accumulator update part) */
 266     fi = 0;
 267     for( row = 0; row < h; row++ )
 268     {
 269         for( col = 0; col < w; col++ )
 270         {
 271             if( _POINT( row, col ))
 272             {
 273                 int halftn;
 274                 float r0;
 275                 float scale_factor;
 276                 int iprev = -1;
 277                 float phi, phi1;
 278                 float theta_it;     /* Value of theta for iterating */
 279
 280                 /* Remember the feature point */
 281                 x[fi] = col;
 282                 y[fi] = row;
 283                 fi++;
 284
 285                 yc = (float) row + 0.5f;
 286                 xc = (float) col + 0.5f;
 287
 288                 /* Update the accumulator */
 289                 t = (float) fabs( cvFastArctan( yc, xc ) * d2r );
 290                 r = (float) sqrt( (double)xc * xc + (double)yc * yc );
 291                 r0 = r * irho;
 292                 ti0 = cvFloor( (t + Pi / 2) * itheta );
 293
 294                 caccum[ti0]++;
 295
 296                 theta_it = rho / r;
 297                 theta_it = theta_it < theta ? theta_it : theta;
 298                 scale_factor = theta_it * itheta;
 299                 halftn = cvFloor( Pi / theta_it );
 300                 for( ti1 = 1, phi = theta_it - halfPi, phi1 = (theta_it + t) * itheta;
 301                      ti1 < halftn; ti1++, phi += theta_it, phi1 += scale_factor )
 302                 {
 303                     rv = r0 * _cos( phi );
 304                     i = cvFloor( rv ) * tn;
 305                     i += cvFloor( phi1 );
 306                     assert( i >= 0 );
 307                     assert( i < rn * tn );
 308                     caccum[i] = (uchar) (caccum[i] + ((i ^ iprev) != 0));
 309                     iprev = i;
 310                     if( cmax < caccum[i] )
 311                         cmax = caccum[i];
 312                 }
 313             }
 314         }
 315     }
 316
 317     /* Starting additional analysis */
 318     count = 0;
 319     for( ri = 0; ri < rn; ri++ )
 320     {
 321         for( ti = 0; ti < tn; ti++ )
 322         {
 323             if( caccum[ri * tn + ti] > threshold )
 324             {
 325                 count++;
 326             }
 327         }
 328     }
 329
 330     if( count * 100 > rn * tn )
 331     {
 332         icvHoughLinesStandard( img, rho, theta, threshold, lines, linesMax );
 333         return;
 334     }
 335
 336     _buffer.resize(srn * stn + 2);
 337     buffer = &_buffer[0];
 338     mcaccum = buffer + 1;
 339
 340     count = 0;
 341     for( ri = 0; ri < rn; ri++ )
 342     {
 343         for( ti = 0; ti < tn; ti++ )
 344         {
 345             if( caccum[ri * tn + ti] > threshold )
 346             {
 347                 count++;
 348                 memset( mcaccum, 0, sfn * sizeof( uchar ));
 349
 350                 for( index = 0; index < fn; index++ )
 351                 {
 352                     int ti2;
 353                     float r0;
 354
 355                     yc = (float) y[index] + 0.5f;
 356                     xc = (float) x[index] + 0.5f;
 357
 358                     /* Update the accumulator */
 359                     t = (float) fabs( cvFastArctan( yc, xc ) * d2r );
 360                     r = (float) sqrt( (double)xc * xc + (double)yc * yc ) * isrho;
 361                     ti0 = cvFloor( (t + Pi * 0.5f) * istheta );
 362                     ti2 = (ti * stn - ti0) * 5;
 363                     r0 = (float) ri *srn;
 364
 365                     for( ti1 = 0 /*, phi = ti*theta - Pi/2 - t */ ; ti1 < stn; ti1++, ti2 += 5
 366                          /*phi += stheta */  )
 367                     {
 368                         /*rv = r*_cos(phi) - r0; */
 369                         rv = r * sinTable[(int) (abs( ti2 ))] - r0;
 370                         i = cvFloor( rv ) * stn + ti1;
 371
 372                         i = CV_IMAX( i, -1 );
 373                         i = CV_IMIN( i, sfn );
 374                         mcaccum[i]++;
 375                         assert( i >= -1 );
 376                         assert( i <= sfn );
 377                     }
 378                 }
 379
 380                 /* Find peaks in maccum... */
 381                 for( index = 0; index < sfn; index++ )
 382                 {
 383                     i = 0;
 384                     pos = h_get_tail_pos__index( list );
 385                     if( h_get_prev__index( &pos )->value < mcaccum[index] )
 386                     {
 387                         vi.value = mcaccum[index];
 388                         vi.rho = index / stn * srho + ri * rho;
 389                         vi.theta = index % stn * stheta + ti * theta - halfPi;
 390                         while( h_is_pos__index( pos ))
 391                         {
 392                             if( h_get__index( pos )->value > mcaccum[index] )
 393                             {
 394                                 h_insert_after__index( list, pos, &vi );
 395                                 if( h_get_count__index( list ) > linesMax )
 396                                 {
 397                                     h_remove_tail__index( list );
 398                                 }
 399                                 break;
 400                             }
 401                             h_get_prev__index( &pos );
 402                         }
 403                         if( !h_is_pos__index( pos ))
 404                         {
 405                             h_add_head__index( list, &vi );
 406                             if( h_get_count__index( list ) > linesMax )
 407                             {
 408                                 h_remove_tail__index( list );
 409                             }
 410                         }
 411                     }
 412                 }
 413             }
 414         }
 415     }
 416
 417     pos = h_get_head_pos__index( list );
 418     if( h_get_count__index( list ) == 1 )
 419     {
 420         if( h_get__index( pos )->rho < 0 )
 421         {
 422             h_clear_list__index( list );
 423         }
 424     }
 425     else
 426     {
 427         while( h_is_pos__index( pos ))
 428         {
 429             CvLinePolar line;
 430             pindex = h_get__index( pos );
 431             if( pindex->rho < 0 )
 432             {
 433                 /* This should be the last element... */
 434                 h_get_next__index( &pos );
 435                 assert( !h_is_pos__index( pos ));
 436                 break;
 437             }
 438             line.rho = pindex->rho;
 439             line.angle = pindex->theta;
 440             cvSeqPush( lines, &line );
 441
 442             if( lines->total >= linesMax )
 443                 break;
 444             h_get_next__index( &pos );
 445         }
 446     }
 447
 448     h_destroy_list__index(list);
 449 }
 450
 451
 452 /****************************************************************************************\
 453 *                              Probabilistic Hough Transform                             *
 454 \****************************************************************************************/
 455
 456 static void
 457 icvHoughLinesProbabilistic( CvMat* image,
 458                             float rho, float theta, int threshold,
 459                             int lineLength, int lineGap,
 460                             CvSeq *lines, int linesMax )
 461 {
 462     cv::Mat accum, mask;
 463     cv::vector<float> trigtab;
 464     cv::MemStorage storage(cvCreateMemStorage(0));
 465
 466     CvSeq* seq;
 467     CvSeqWriter writer;
 468     int width, height;
 469     int numangle, numrho;
 470     float ang;
 471     int r, n, count;
 472     CvPoint pt;
 473     float irho = 1 / rho;
 474     CvRNG rng = cvRNG(-1);
 475     const float* ttab;
 476     uchar* mdata0;
 477
 478     CV_Assert( CV_IS_MAT(image) && CV_MAT_TYPE(image->type) == CV_8UC1 );
 479
 480     width = image->cols;
 481     height = image->rows;
 482
 483     numangle = cvRound(CV_PI / theta);
 484     numrho = cvRound(((width + height) * 2 + 1) / rho);
 485
 486     accum.create( numangle, numrho, CV_32SC1 );
 487     mask.create( height, width, CV_8UC1 );
 488     trigtab.resize(numangle*2);
 489     accum = cv::Scalar(0);
 490
 491     for( ang = 0, n = 0; n < numangle; ang += theta, n++ )
 492     {
 493         trigtab[n*2] = (float)(cos(ang) * irho);
 494         trigtab[n*2+1] = (float)(sin(ang) * irho);
 495     }
 496     ttab = &trigtab[0];
 497     mdata0 = mask.data;
 498
 499     cvStartWriteSeq( CV_32SC2, sizeof(CvSeq), sizeof(CvPoint), storage, &writer );
 500
 501     // stage 1. collect non-zero image points
 502     for( pt.y = 0, count = 0; pt.y < height; pt.y++ )
 503     {
 504         const uchar* data = image->data.ptr + pt.y*image->step;
 505         uchar* mdata = mdata0 + pt.y*width;
 506         for( pt.x = 0; pt.x < width; pt.x++ )
 507         {
 508             if( data[pt.x] )
 509             {
 510                 mdata[pt.x] = (uchar)1;
 511                 CV_WRITE_SEQ_ELEM( pt, writer );
 512             }
 513             else
 514                 mdata[pt.x] = 0;
 515         }
 516     }
 517
 518     seq = cvEndWriteSeq( &writer );
 519     count = seq->total;
 520
 521     // stage 2. process all the points in random order
 522     for( ; count > 0; count-- )
 523     {
 524         // choose random point out of the remaining ones
 525         int idx = cvRandInt(&rng) % count;
 526         int max_val = threshold-1, max_n = 0;
 527         CvPoint* point = (CvPoint*)cvGetSeqElem( seq, idx );
 528         CvPoint line_end[2] = {{0,0}, {0,0}};
 529         float a, b;
 530         int* adata = (int*)accum.data;
 531         int i, j, k, x0, y0, dx0, dy0, xflag;
 532         int good_line;
 533         const int shift = 16;
 534
 535         i = point->y;
 536         j = point->x;
 537
 538         // "remove" it by overriding it with the last element
 539         *point = *(CvPoint*)cvGetSeqElem( seq, count-1 );
 540
 541         // check if it has been excluded already (i.e. belongs to some other line)
 542         if( !mdata0[i*width + j] )
 543             continue;
 544
 545         // update accumulator, find the most probable line
 546         for( n = 0; n < numangle; n++, adata += numrho )
 547         {
 548             r = cvRound( j * ttab[n*2] + i * ttab[n*2+1] );
 549             r += (numrho - 1) / 2;
 550             int val = ++adata[r];
 551             if( max_val < val )
 552             {
 553                 max_val = val;
 554                 max_n = n;
 555             }
 556         }
 557
 558         // if it is too "weak" candidate, continue with another point
 559         if( max_val < threshold )
 560             continue;
 561
 562         // from the current point walk in each direction
 563         // along the found line and extract the line segment
 564         a = -ttab[max_n*2+1];
 565         b = ttab[max_n*2];
 566         x0 = j;
 567         y0 = i;
 568         if( fabs(a) > fabs(b) )
 569         {
 570             xflag = 1;
 571             dx0 = a > 0 ? 1 : -1;
 572             dy0 = cvRound( b*(1 << shift)/fabs(a) );
 573             y0 = (y0 << shift) + (1 << (shift-1));
 574         }
 575         else
 576         {
 577             xflag = 0;
 578             dy0 = b > 0 ? 1 : -1;
 579             dx0 = cvRound( a*(1 << shift)/fabs(b) );
 580             x0 = (x0 << shift) + (1 << (shift-1));
 581         }
 582
 583         for( k = 0; k < 2; k++ )
 584         {
 585             int gap = 0, x = x0, y = y0, dx = dx0, dy = dy0;
 586
 587             if( k > 0 )
 588                 dx = -dx, dy = -dy;
 589
 590             // walk along the line using fixed-point arithmetics,
 591             // stop at the image border or in case of too big gap
 592             for( ;; x += dx, y += dy )
 593             {
 594                 uchar* mdata;
 595                 int i1, j1;
 596
 597                 if( xflag )
 598                 {
 599                     j1 = x;
 600                     i1 = y >> shift;
 601                 }
 602                 else
 603                 {
 604                     j1 = x >> shift;
 605                     i1 = y;
 606                 }
 607
 608                 if( j1 < 0 || j1 >= width || i1 < 0 || i1 >= height )
 609                     break;
 610
 611                 mdata = mdata0 + i1*width + j1;
 612
 613                 // for each non-zero point:
 614                 //    update line end,
 615                 //    clear the mask element
 616                 //    reset the gap
 617                 if( *mdata )
 618                 {
 619                     gap = 0;
 620                     line_end[k].y = i1;
 621                     line_end[k].x = j1;
 622                 }
 623                 else if( ++gap > lineGap )
 624                     break;
 625             }
 626         }
 627
 628         good_line = abs(line_end[1].x - line_end[0].x) >= lineLength ||
 629                     abs(line_end[1].y - line_end[0].y) >= lineLength;
 630
 631         for( k = 0; k < 2; k++ )
 632         {
 633             int x = x0, y = y0, dx = dx0, dy = dy0;
 634
 635             if( k > 0 )
 636                 dx = -dx, dy = -dy;
 637
 638             // walk along the line using fixed-point arithmetics,
 639             // stop at the image border or in case of too big gap
 640             for( ;; x += dx, y += dy )
 641             {
 642                 uchar* mdata;
 643                 int i1, j1;
 644
 645                 if( xflag )
 646                 {
 647                     j1 = x;
 648                     i1 = y >> shift;
 649                 }
 650                 else
 651                 {
 652                     j1 = x >> shift;
 653                     i1 = y;
 654                 }
 655
 656                 mdata = mdata0 + i1*width + j1;
 657
 658                 // for each non-zero point:
 659                 //    update line end,
 660                 //    clear the mask element
 661                 //    reset the gap
 662                 if( *mdata )
 663                 {
 664                     if( good_line )
 665                     {
 666                         adata = (int*)accum.data;
 667                         for( n = 0; n < numangle; n++, adata += numrho )
 668                         {
 669                             r = cvRound( j1 * ttab[n*2] + i1 * ttab[n*2+1] );
 670                             r += (numrho - 1) / 2;
 671                             adata[r]--;
 672                         }
 673                     }
 674                     *mdata = 0;
 675                 }
 676
 677                 if( i1 == line_end[k].y && j1 == line_end[k].x )
 678                     break;
 679             }
 680         }
 681
 682         if( good_line )
 683         {
 684             CvRect lr = { line_end[0].x, line_end[0].y, line_end[1].x, line_end[1].y };
 685             cvSeqPush( lines, &lr );
 686             if( lines->total >= linesMax )
 687                 return;
 688         }
 689     }
 690 }
 691
 692 /* Wrapper function for standard hough transform */
 693 CV_IMPL CvSeq*
 694 cvHoughLines2( CvArr* src_image, void* lineStorage, int method,
 695                double rho, double theta, int threshold,
 696                double param1, double param2 )
 697 {
 698     CvSeq* result = 0;
 699
 700     CvMat stub, *img = (CvMat*)src_image;
 701     CvMat* mat = 0;
 702     CvSeq* lines = 0;
 703     CvSeq lines_header;
 704     CvSeqBlock lines_block;
 705     int lineType, elemSize;
 706     int linesMax = INT_MAX;
 707     int iparam1, iparam2;
 708
 709     img = cvGetMat( img, &stub );
 710
 711     if( !CV_IS_MASK_ARR(img))
 712         CV_Error( CV_StsBadArg, "The source image must be 8-bit, single-channel" );
 713
 714     if( !lineStorage )
 715         CV_Error( CV_StsNullPtr, "NULL destination" );
 716
 717     if( rho <= 0 || theta <= 0 || threshold <= 0 )
 718         CV_Error( CV_StsOutOfRange, "rho, theta and threshold must be positive" );
 719
 720     if( method != CV_HOUGH_PROBABILISTIC )
 721     {
 722         lineType = CV_32FC2;
 723         elemSize = sizeof(float)*2;
 724     }
 725     else
 726     {
 727         lineType = CV_32SC4;
 728         elemSize = sizeof(int)*4;
 729     }
 730
 731     if( CV_IS_STORAGE( lineStorage ))
 732     {
 733         lines = cvCreateSeq( lineType, sizeof(CvSeq), elemSize, (CvMemStorage*)lineStorage );
 734     }
 735     else if( CV_IS_MAT( lineStorage ))
 736     {
 737         mat = (CvMat*)lineStorage;
 738
 739         if( !CV_IS_MAT_CONT( mat->type ) || (mat->rows != 1 && mat->cols != 1) )
 740             CV_Error( CV_StsBadArg,
 741             "The destination matrix should be continuous and have a single row or a single column" );
 742
 743         if( CV_MAT_TYPE( mat->type ) != lineType )
 744             CV_Error( CV_StsBadArg,
 745             "The destination matrix data type is inappropriate, see the manual" );
 746
 747         lines = cvMakeSeqHeaderForArray( lineType, sizeof(CvSeq), elemSize, mat->data.ptr,
 748                                          mat->rows + mat->cols - 1, &lines_header, &lines_block );
 749         linesMax = lines->total;
 750         cvClearSeq( lines );
 751     }
 752     else
 753         CV_Error( CV_StsBadArg, "Destination is not CvMemStorage* nor CvMat*" );
 754
 755     iparam1 = cvRound(param1);
 756     iparam2 = cvRound(param2);
 757
 758     switch( method )
 759     {
 760     case CV_HOUGH_STANDARD:
 761           icvHoughLinesStandard( img, (float)rho,
 762                 (float)theta, threshold, lines, linesMax );
 763           break;
 764     case CV_HOUGH_MULTI_SCALE:
 765           icvHoughLinesSDiv( img, (float)rho, (float)theta,
 766                 threshold, iparam1, iparam2, lines, linesMax );
 767           break;
 768     case CV_HOUGH_PROBABILISTIC:
 769           icvHoughLinesProbabilistic( img, (float)rho, (float)theta,
 770                 threshold, iparam1, iparam2, lines, linesMax );
 771           break;
 772     default:
 773         CV_Error( CV_StsBadArg, "Unrecognized method id" );
 774     }
 775
 776     if( mat )
 777     {
 778         if( mat->cols > mat->rows )
 779             mat->cols = lines->total;
 780         else
 781             mat->rows = lines->total;
 782     }
 783     else
 784         result = lines;
 785
 786     return result;
 787 }
 788
 789
 790 /****************************************************************************************\
 791 *                                     Circle Detection                                   *
 792 \****************************************************************************************/
 793
 794 static void
 795 icvHoughCirclesGradient( CvMat* img, float dp, float min_dist,
 796                          int min_radius, int max_radius,
 797                          int canny_threshold, int acc_threshold,
 798                          CvSeq* circles, int circles_max )
 799 {
 800     const int SHIFT = 10, ONE = 1 << SHIFT;
 801     cv::Ptr<CvMat> dx, dy;
 802     cv::Ptr<CvMat> edges, accum, dist_buf;
 803     std::vector<int> sort_buf;
 804     cv::Ptr<CvMemStorage> storage;
 805
 806     int x, y, i, j, k, center_count, nz_count;
 807     float min_radius2 = (float)min_radius*min_radius;
 808     float max_radius2 = (float)max_radius*max_radius;
 809     int rows, cols, arows, acols;
 810     int astep, *adata;
 811     float* ddata;
 812     CvSeq *nz, *centers;
 813     float idp, dr;
 814     CvSeqReader reader;
 815
 816     edges = cvCreateMat( img->rows, img->cols, CV_8UC1 );
 817     cvCanny( img, edges, MAX(canny_threshold/2,1), canny_threshold, 3 );
 818
 819     dx = cvCreateMat( img->rows, img->cols, CV_16SC1 );
 820     dy = cvCreateMat( img->rows, img->cols, CV_16SC1 );
 821     cvSobel( img, dx, 1, 0, 3 );
 822     cvSobel( img, dy, 0, 1, 3 );
 823
 824     if( dp < 1.f )
 825         dp = 1.f;
 826     idp = 1.f/dp;
 827     accum = cvCreateMat( cvCeil(img->rows*idp)+2, cvCeil(img->cols*idp)+2, CV_32SC1 );
 828     cvZero(accum);
 829
 830     storage = cvCreateMemStorage();
 831     nz = cvCreateSeq( CV_32SC2, sizeof(CvSeq), sizeof(CvPoint), storage );
 832     centers = cvCreateSeq( CV_32SC1, sizeof(CvSeq), sizeof(int), storage );
 833
 834     rows = img->rows;
 835     cols = img->cols;
 836     arows = accum->rows - 2;
 837     acols = accum->cols - 2;
 838     adata = accum->data.i;
 839     astep = accum->step/sizeof(adata[0]);
 840     // Accumulate circle evidence for each edge pixel
 841     for( y = 0; y < rows; y++ )
 842     {
 843         const uchar* edges_row = edges->data.ptr + y*edges->step;
 844         const short* dx_row = (const short*)(dx->data.ptr + y*dx->step);
 845         const short* dy_row = (const short*)(dy->data.ptr + y*dy->step);
 846
 847         for( x = 0; x < cols; x++ )
 848         {
 849             float vx, vy;
 850             int sx, sy, x0, y0, x1, y1, r;
 851             CvPoint pt;
 852
 853             vx = dx_row[x];
 854             vy = dy_row[x];
 855
 856             if( !edges_row[x] || (vx == 0 && vy == 0) )
 857                 continue;
 858
 859             float mag = sqrt(vx*vx+vy*vy);
 860             assert( mag >= 1 );
 861             sx = cvRound((vx*idp)*ONE/mag);
 862             sy = cvRound((vy*idp)*ONE/mag);
 863
 864             x0 = cvRound((x*idp)*ONE);
 865             y0 = cvRound((y*idp)*ONE);
 866             // Step from min_radius to max_radius in both directions of the gradient
 867             for(int k1 = 0; k1 < 2; k1++ )
 868             {
 869                 x1 = x0 + min_radius * sx;
 870                 y1 = y0 + min_radius * sy;
 871
 872                 for( r = min_radius; r <= max_radius; x1 += sx, y1 += sy, r++ )
 873                 {
 874                     int x2 = x1 >> SHIFT, y2 = y1 >> SHIFT;
 875                     if( (unsigned)x2 >= (unsigned)acols ||
 876                         (unsigned)y2 >= (unsigned)arows )
 877                         break;
 878                     adata[y2*astep + x2]++;
 879                 }
 880
 881                 sx = -sx; sy = -sy;
 882             }
 883
 884             pt.x = x; pt.y = y;
 885             cvSeqPush( nz, &pt );
 886         }
 887     }
 888
 889     nz_count = nz->total;
 890     if( !nz_count )
 891         return;
 892     //Find possible circle centers
 893     for( y = 1; y < arows - 1; y++ )
 894     {
 895         for( x = 1; x < acols - 1; x++ )
 896         {
 897             int base = y*(acols+2) + x;
 898             if( adata[base] > acc_threshold &&
 899                 adata[base] > adata[base-1] && adata[base] > adata[base+1] &&
 900                 adata[base] > adata[base-acols-2] && adata[base] > adata[base+acols+2] )
 901                 cvSeqPush(centers, &base);
 902         }
 903     }
 904
 905     center_count = centers->total;
 906     if( !center_count )
 907         return;
 908
 909     sort_buf.resize( MAX(center_count,nz_count) );
 910     cvCvtSeqToArray( centers, &sort_buf[0] );
 911
 912     icvHoughSortDescent32s( &sort_buf[0], center_count, adata );
 913     cvClearSeq( centers );
 914     cvSeqPushMulti( centers, &sort_buf[0], center_count );
 915
 916     dist_buf = cvCreateMat( 1, nz_count, CV_32FC1 );
 917     ddata = dist_buf->data.fl;
 918
 919     dr = dp;
 920     min_dist = MAX( min_dist, dp );
 921     min_dist *= min_dist;
 922     // For each found possible center
 923     // Estimate radius and check support
 924     for( i = 0; i < centers->total; i++ )
 925     {
 926         int ofs = *(int*)cvGetSeqElem( centers, i );
 927         y = ofs/(acols+2);
 928         x = ofs - (y)*(acols+2);
 929         //Calculate circle's center in pixels
 930         float cx = (float)((x + 0.5f)*dp), cy = (float)(( y + 0.5f )*dp);
 931         float start_dist, dist_sum;
 932         float r_best = 0;
 933         int max_count = 0;
 934         // Check distance with previously detected circles
 935         for( j = 0; j < circles->total; j++ )
 936         {
 937             float* c = (float*)cvGetSeqElem( circles, j );
 938             if( (c[0] - cx)*(c[0] - cx) + (c[1] - cy)*(c[1] - cy) < min_dist )
 939                 break;
 940         }
 941
 942         if( j < circles->total )
 943             continue;
 944         // Estimate best radius
 945         cvStartReadSeq( nz, &reader );
 946         for( j = k = 0; j < nz_count; j++ )
 947         {
 948             CvPoint pt;
 949             float _dx, _dy, _r2;
 950             CV_READ_SEQ_ELEM( pt, reader );
 951             _dx = cx - pt.x; _dy = cy - pt.y;
 952             _r2 = _dx*_dx + _dy*_dy;
 953             if(min_radius2 <= _r2 && _r2 <= max_radius2 )
 954             {
 955                 ddata[k] = _r2;
 956                 sort_buf[k] = k;
 957                 k++;
 958             }
 959         }
 960
 961         int nz_count1 = k, start_idx = nz_count1 - 1;
 962         if( nz_count1 == 0 )
 963             continue;
 964         dist_buf->cols = nz_count1;
 965         cvPow( dist_buf, dist_buf, 0.5 );
 966         icvHoughSortDescent32s( &sort_buf[0], nz_count1, (int*)ddata );
 967
 968         dist_sum = start_dist = ddata[sort_buf[nz_count1-1]];
 969         for( j = nz_count1 - 2; j >= 0; j-- )
 970         {
 971             float d = ddata[sort_buf[j]];
 972
 973             if( d > max_radius )
 974                 break;
 975
 976             if( d - start_dist > dr )
 977             {
 978                 float r_cur = ddata[sort_buf[(j + start_idx)/2]];
 979                 if( (start_idx - j)*r_best >= max_count*r_cur ||
 980                     (r_best < FLT_EPSILON && start_idx - j >= max_count) )
 981                 {
 982                     r_best = r_cur;
 983                     max_count = start_idx - j;
 984                 }
 985                 start_dist = d;
 986                 start_idx = j;
 987                 dist_sum = 0;
 988             }
 989             dist_sum += d;
 990         }
 991         // Check if the circle has enough support
 992         if( max_count > acc_threshold )
 993         {
 994             float c[3];
 995             c[0] = cx;
 996             c[1] = cy;
 997             c[2] = (float)r_best;
 998             cvSeqPush( circles, c );
 999             if( circles->total > circles_max )
1000                 return;
1001         }
1002     }
1003 }
1004
1005 CV_IMPL CvSeq*
1006 cvHoughCircles( CvArr* src_image, void* circle_storage,
1007                 int method, double dp, double min_dist,
1008                 double param1, double param2,
1009                 int min_radius, int max_radius )
1010 {
1011     CvSeq* result = 0;
1012
1013     CvMat stub, *img = (CvMat*)src_image;
1014     CvMat* mat = 0;
1015     CvSeq* circles = 0;
1016     CvSeq circles_header;
1017     CvSeqBlock circles_block;
1018     int circles_max = INT_MAX;
1019     int canny_threshold = cvRound(param1);
1020     int acc_threshold = cvRound(param2);
1021
1022     img = cvGetMat( img, &stub );
1023
1024     if( !CV_IS_MASK_ARR(img))
1025         CV_Error( CV_StsBadArg, "The source image must be 8-bit, single-channel" );
1026
1027     if( !circle_storage )
1028         CV_Error( CV_StsNullPtr, "NULL destination" );
1029
1030     if( dp <= 0 || min_dist <= 0 || canny_threshold <= 0 || acc_threshold <= 0 )
1031         CV_Error( CV_StsOutOfRange, "dp, min_dist, canny_threshold and acc_threshold must be all positive numbers" );
1032
1033     min_radius = MAX( min_radius, 0 );
1034     if( max_radius <= 0 )
1035         max_radius = MAX( img->rows, img->cols );
1036     else if( max_radius <= min_radius )
1037         max_radius = min_radius + 2;
1038
1039     if( CV_IS_STORAGE( circle_storage ))
1040     {
1041         circles = cvCreateSeq( CV_32FC3, sizeof(CvSeq),
1042             sizeof(float)*3, (CvMemStorage*)circle_storage );
1043     }
1044     else if( CV_IS_MAT( circle_storage ))
1045     {
1046         mat = (CvMat*)circle_storage;
1047
1048         if( !CV_IS_MAT_CONT( mat->type ) || (mat->rows != 1 && mat->cols != 1) ||
1049             CV_MAT_TYPE(mat->type) != CV_32FC3 )
1050             CV_Error( CV_StsBadArg,
1051             "The destination matrix should be continuous and have a single row or a single column" );
1052
1053         circles = cvMakeSeqHeaderForArray( CV_32FC3, sizeof(CvSeq), sizeof(float)*3,
1054                 mat->data.ptr, mat->rows + mat->cols - 1, &circles_header, &circles_block );
1055         circles_max = circles->total;
1056         cvClearSeq( circles );
1057     }
1058     else
1059         CV_Error( CV_StsBadArg, "Destination is not CvMemStorage* nor CvMat*" );
1060
1061     switch( method )
1062     {
1063     case CV_HOUGH_GRADIENT:
1064         icvHoughCirclesGradient( img, (float)dp, (float)min_dist,
1065                                 min_radius, max_radius, canny_threshold,
1066                                 acc_threshold, circles, circles_max );
1067           break;
1068     default:
1069         CV_Error( CV_StsBadArg, "Unrecognized method id" );
1070     }
1071
1072     if( mat )
1073     {
1074         if( mat->cols > mat->rows )
1075             mat->cols = circles->total;
1076         else
1077             mat->rows = circles->total;
1078     }
1079     else
1080         result = circles;
1081
1082     return result;
1083 }
1084
1085
1086 namespace cv
1087 {
1088
1089 const int STORAGE_SIZE = 1 << 12;
1090
1091 static void seqToMat(const CvSeq* seq, OutputArray _arr)
1092 {
1093     if( seq && seq->total > 0 )
1094     {
1095         _arr.create(1, seq->total, seq->flags, -1, true);
1096         Mat arr = _arr.getMat();
1097         cvCvtSeqToArray(seq, arr.data);
1098     }
1099     else
1100         _arr.release();
1101 }
1102
1103 }
1104
1105 void cv::HoughLines( InputArray _image, OutputArray _lines,
1106                      double rho, double theta, int threshold,
1107                      double srn, double stn )
1108 {
1109     Ptr<CvMemStorage> storage = cvCreateMemStorage(STORAGE_SIZE);
1110     Mat image = _image.getMat();
1111     CvMat c_image = image;
1112     CvSeq* seq = cvHoughLines2( &c_image, storage, srn == 0 && stn == 0 ?
1113                     CV_HOUGH_STANDARD : CV_HOUGH_MULTI_SCALE,
1114                     rho, theta, threshold, srn, stn );
1115     seqToMat(seq, _lines);
1116 }
1117
1118 void cv::HoughLinesP( InputArray _image, OutputArray _lines,
1119                       double rho, double theta, int threshold,
1120                       double minLineLength, double maxGap )
1121 {
1122     Ptr<CvMemStorage> storage = cvCreateMemStorage(STORAGE_SIZE);
1123     Mat image = _image.getMat();
1124     CvMat c_image = image;
1125     CvSeq* seq = cvHoughLines2( &c_image, storage, CV_HOUGH_PROBABILISTIC,
1126                     rho, theta, threshold, minLineLength, maxGap );
1127     seqToMat(seq, _lines);
1128 }
1129
1130 void cv::HoughCircles( InputArray _image, OutputArray _circles,
1131                        int method, double dp, double min_dist,
1132                        double param1, double param2,
1133                        int minRadius, int maxRadius )
1134 {
1135     Ptr<CvMemStorage> storage = cvCreateMemStorage(STORAGE_SIZE);
1136     Mat image = _image.getMat();
1137     CvMat c_image = image;
1138     CvSeq* seq = cvHoughCircles( &c_image, storage, method,
1139                     dp, min_dist, param1, param2, minRadius, maxRadius );
1140     seqToMat(seq, _circles);
1141 }
1142
1143 /* End of file. */