vcs.maemo.org Git - opencv/blob - cv/src/cvhough.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 "_cv.h"
  43 #include "_cvlist.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     int *accum = 0;
  89     int *sort_buf=0;
  90     float *tabSin = 0;
  91     float *tabCos = 0;
  92
  93     CV_FUNCNAME( "icvHoughLinesStandard" );
  94
  95     __BEGIN__;
  96
  97     const uchar* image;
  98     int step, width, height;
  99     int numangle, numrho;
 100     int total = 0;
 101     float ang;
 102     int r, n;
 103     int i, j;
 104     float irho = 1 / rho;
 105     double scale;
 106
 107     CV_ASSERT( CV_IS_MAT(img) && CV_MAT_TYPE(img->type) == CV_8UC1 );
 108
 109     image = img->data.ptr;
 110     step = img->step;
 111     width = img->cols;
 112     height = img->rows;
 113
 114     numangle = cvRound(CV_PI / theta);
 115     numrho = cvRound(((width + height) * 2 + 1) / rho);
 116
 117     CV_CALL( accum = (int*)cvAlloc( sizeof(accum[0]) * (numangle+2) * (numrho+2) ));
 118     CV_CALL( sort_buf = (int*)cvAlloc( sizeof(accum[0]) * numangle * numrho ));
 119     CV_CALL( tabSin = (float*)cvAlloc( sizeof(tabSin[0]) * numangle ));
 120     CV_CALL( tabCos = (float*)cvAlloc( sizeof(tabCos[0]) * numangle ));
 121     memset( accum, 0, sizeof(accum[0]) * (numangle+2) * (numrho+2) );
 122
 123     for( ang = 0, n = 0; n < numangle; ang += theta, n++ )
 124     {
 125         tabSin[n] = (float)(sin(ang) * irho);
 126         tabCos[n] = (float)(cos(ang) * irho);
 127     }
 128
 129     // stage 1. fill accumulator
 130     for( i = 0; i < height; i++ )
 131         for( j = 0; j < width; j++ )
 132         {
 133             if( image[i * step + j] != 0 )
 134                 for( n = 0; n < numangle; n++ )
 135                 {
 136                     r = cvRound( j * tabCos[n] + i * tabSin[n] );
 137                     r += (numrho - 1) / 2;
 138                     accum[(n+1) * (numrho+2) + r+1]++;
 139                 }
 140         }
 141
 142     // stage 2. find local maximums
 143     for( r = 0; r < numrho; r++ )
 144         for( n = 0; n < numangle; n++ )
 145         {
 146             int base = (n+1) * (numrho+2) + r+1;
 147             if( accum[base] > threshold &&
 148                 accum[base] > accum[base - 1] && accum[base] >= accum[base + 1] &&
 149                 accum[base] > accum[base - numrho - 2] && accum[base] >= accum[base + numrho + 2] )
 150                 sort_buf[total++] = base;
 151         }
 152
 153     // stage 3. sort the detected lines by accumulator value
 154     icvHoughSortDescent32s( sort_buf, total, accum );
 155
 156     // stage 4. store the first min(total,linesMax) lines to the output buffer
 157     linesMax = MIN(linesMax, total);
 158     scale = 1./(numrho+2);
 159     for( i = 0; i < linesMax; i++ )
 160     {
 161         CvLinePolar line;
 162         int idx = sort_buf[i];
 163         int n = cvFloor(idx*scale) - 1;
 164         int r = idx - (n+1)*(numrho+2) - 1;
 165         line.rho = (r - (numrho - 1)*0.5f) * rho;
 166         line.angle = n * theta;
 167         cvSeqPush( lines, &line );
 168     }
 169
 170     __END__;
 171
 172     cvFree( &sort_buf );
 173     cvFree( &tabSin );
 174     cvFree( &tabCos );
 175     cvFree( &accum );
 176 }
 177
 178
 179 /****************************************************************************************\
 180 *                     Multi-Scale variant of Classical Hough Transform                   *
 181 \****************************************************************************************/
 182
 183 #if defined _MSC_VER && _MSC_VER >= 1200
 184 #pragma warning( disable: 4714 )
 185 #endif
 186
 187 //DECLARE_AND_IMPLEMENT_LIST( _index, h_ );
 188 IMPLEMENT_LIST( _index, h_ )
 189
 190 static void
 191 icvHoughLinesSDiv( const CvMat* img,
 192                    float rho, float theta, int threshold,
 193                    int srn, int stn,
 194                    CvSeq* lines, int linesMax )
 195 {
 196     uchar *caccum = 0;
 197     uchar *buffer = 0;
 198     float *sinTable = 0;
 199     int *x = 0;
 200     int *y = 0;
 201     _CVLIST *list = 0;
 202
 203     CV_FUNCNAME( "icvHoughLinesSDiv" );
 204
 205     __BEGIN__;
 206
 207 #define _POINT(row, column)\
 208     (image_src[(row)*step+(column)])
 209
 210     uchar *mcaccum = 0;
 211     int rn, tn;                 /* number of rho and theta discrete values */
 212     int index, i;
 213     int ri, ti, ti1, ti0;
 214     int row, col;
 215     float r, t;                 /* Current rho and theta */
 216     float rv;                   /* Some temporary rho value */
 217     float irho;
 218     float itheta;
 219     float srho, stheta;
 220     float isrho, istheta;
 221
 222     const uchar* image_src;
 223     int w, h, step;
 224     int fn = 0;
 225     float xc, yc;
 226
 227     const float d2r = (float)(Pi / 180);
 228     int sfn = srn * stn;
 229     int fi;
 230     int count;
 231     int cmax = 0;
 232
 233     CVPOS pos;
 234     _index *pindex;
 235     _index vi;
 236
 237     CV_ASSERT( CV_IS_MAT(img) && CV_MAT_TYPE(img->type) == CV_8UC1 );
 238     CV_ASSERT( linesMax > 0 && rho > 0 && theta > 0 );
 239
 240     threshold = MIN( threshold, 255 );
 241
 242     image_src = img->data.ptr;
 243     step = img->step;
 244     w = img->cols;
 245     h = img->rows;
 246
 247     irho = 1 / rho;
 248     itheta = 1 / theta;
 249     srho = rho / srn;
 250     stheta = theta / stn;
 251     isrho = 1 / srho;
 252     istheta = 1 / stheta;
 253
 254     rn = cvFloor( sqrt( (double)w * w + (double)h * h ) * irho );
 255     tn = cvFloor( 2 * Pi * itheta );
 256
 257     list = h_create_list__index( linesMax < 1000 ? linesMax : 1000 );
 258     vi.value = threshold;
 259     vi.rho = -1;
 260     h_add_head__index( list, &vi );
 261
 262     /* Precalculating sin */
 263     CV_CALL( sinTable = (float*)cvAlloc( 5 * tn * stn * sizeof( float )));
 264
 265     for( index = 0; index < 5 * tn * stn; index++ )
 266     {
 267         sinTable[index] = (float)cos( stheta * index * 0.2f );
 268     }
 269
 270     CV_CALL( caccum = (uchar*)cvAlloc( rn * tn * sizeof( caccum[0] )));
 271     memset( caccum, 0, rn * tn * sizeof( caccum[0] ));
 272
 273     /* Counting all feature pixels */
 274     for( row = 0; row < h; row++ )
 275         for( col = 0; col < w; col++ )
 276             fn += _POINT( row, col ) != 0;
 277
 278     CV_CALL( x = (int*)cvAlloc( fn * sizeof(x[0])));
 279     CV_CALL( y = (int*)cvAlloc( fn * sizeof(y[0])));
 280
 281     /* Full Hough Transform (it's accumulator update part) */
 282     fi = 0;
 283     for( row = 0; row < h; row++ )
 284     {
 285         for( col = 0; col < w; col++ )
 286         {
 287             if( _POINT( row, col ))
 288             {
 289                 int halftn;
 290                 float r0;
 291                 float scale_factor;
 292                 int iprev = -1;
 293                 float phi, phi1;
 294                 float theta_it;     /* Value of theta for iterating */
 295
 296                 /* Remember the feature point */
 297                 x[fi] = col;
 298                 y[fi] = row;
 299                 fi++;
 300
 301                 yc = (float) row + 0.5f;
 302                 xc = (float) col + 0.5f;
 303
 304                 /* Update the accumulator */
 305                 t = (float) fabs( cvFastArctan( yc, xc ) * d2r );
 306                 r = (float) sqrt( (double)xc * xc + (double)yc * yc );
 307                 r0 = r * irho;
 308                 ti0 = cvFloor( (t + Pi / 2) * itheta );
 309
 310                 caccum[ti0]++;
 311
 312                 theta_it = rho / r;
 313                 theta_it = theta_it < theta ? theta_it : theta;
 314                 scale_factor = theta_it * itheta;
 315                 halftn = cvFloor( Pi / theta_it );
 316                 for( ti1 = 1, phi = theta_it - halfPi, phi1 = (theta_it + t) * itheta;
 317                      ti1 < halftn; ti1++, phi += theta_it, phi1 += scale_factor )
 318                 {
 319                     rv = r0 * _cos( phi );
 320                     i = cvFloor( rv ) * tn;
 321                     i += cvFloor( phi1 );
 322                     assert( i >= 0 );
 323                     assert( i < rn * tn );
 324                     caccum[i] = (uchar) (caccum[i] + ((i ^ iprev) != 0));
 325                     iprev = i;
 326                     if( cmax < caccum[i] )
 327                         cmax = caccum[i];
 328                 }
 329             }
 330         }
 331     }
 332
 333     /* Starting additional analysis */
 334     count = 0;
 335     for( ri = 0; ri < rn; ri++ )
 336     {
 337         for( ti = 0; ti < tn; ti++ )
 338         {
 339             if( caccum[ri * tn + ti > threshold] )
 340             {
 341                 count++;
 342             }
 343         }
 344     }
 345
 346     if( count * 100 > rn * tn )
 347     {
 348         icvHoughLinesStandard( img, rho, theta, threshold, lines, linesMax );
 349         EXIT;
 350     }
 351
 352     CV_CALL( buffer = (uchar *) cvAlloc(srn * stn + 2));
 353     mcaccum = buffer + 1;
 354
 355     count = 0;
 356     for( ri = 0; ri < rn; ri++ )
 357     {
 358         for( ti = 0; ti < tn; ti++ )
 359         {
 360             if( caccum[ri * tn + ti] > threshold )
 361             {
 362                 count++;
 363                 memset( mcaccum, 0, sfn * sizeof( uchar ));
 364
 365                 for( index = 0; index < fn; index++ )
 366                 {
 367                     int ti2;
 368                     float r0;
 369
 370                     yc = (float) y[index] + 0.5f;
 371                     xc = (float) x[index] + 0.5f;
 372
 373                     /* Update the accumulator */
 374                     t = (float) fabs( cvFastArctan( yc, xc ) * d2r );
 375                     r = (float) sqrt( (double)xc * xc + (double)yc * yc ) * isrho;
 376                     ti0 = cvFloor( (t + Pi * 0.5f) * istheta );
 377                     ti2 = (ti * stn - ti0) * 5;
 378                     r0 = (float) ri *srn;
 379
 380                     for( ti1 = 0 /*, phi = ti*theta - Pi/2 - t */ ; ti1 < stn; ti1++, ti2 += 5
 381                          /*phi += stheta */  )
 382                     {
 383                         /*rv = r*_cos(phi) - r0; */
 384                         rv = r * sinTable[(int) (abs( ti2 ))] - r0;
 385                         i = cvFloor( rv ) * stn + ti1;
 386
 387                         i = CV_IMAX( i, -1 );
 388                         i = CV_IMIN( i, sfn );
 389                         mcaccum[i]++;
 390                         assert( i >= -1 );
 391                         assert( i <= sfn );
 392                     }
 393                 }
 394
 395                 /* Find peaks in maccum... */
 396                 for( index = 0; index < sfn; index++ )
 397                 {
 398                     i = 0;
 399                     pos = h_get_tail_pos__index( list );
 400                     if( h_get_prev__index( &pos )->value < mcaccum[index] )
 401                     {
 402                         vi.value = mcaccum[index];
 403                         vi.rho = index / stn * srho + ri * rho;
 404                         vi.theta = index % stn * stheta + ti * theta - halfPi;
 405                         while( h_is_pos__index( pos ))
 406                         {
 407                             if( h_get__index( pos )->value > mcaccum[index] )
 408                             {
 409                                 h_insert_after__index( list, pos, &vi );
 410                                 if( h_get_count__index( list ) > linesMax )
 411                                 {
 412                                     h_remove_tail__index( list );
 413                                 }
 414                                 break;
 415                             }
 416                             h_get_prev__index( &pos );
 417                         }
 418                         if( !h_is_pos__index( pos ))
 419                         {
 420                             h_add_head__index( list, &vi );
 421                             if( h_get_count__index( list ) > linesMax )
 422                             {
 423                                 h_remove_tail__index( list );
 424                             }
 425                         }
 426                     }
 427                 }
 428             }
 429         }
 430     }
 431
 432     pos = h_get_head_pos__index( list );
 433     if( h_get_count__index( list ) == 1 )
 434     {
 435         if( h_get__index( pos )->rho < 0 )
 436         {
 437             h_clear_list__index( list );
 438         }
 439     }
 440     else
 441     {
 442         while( h_is_pos__index( pos ))
 443         {
 444             CvLinePolar line;
 445             pindex = h_get__index( pos );
 446             if( pindex->rho < 0 )
 447             {
 448                 /* This should be the last element... */
 449                 h_get_next__index( &pos );
 450                 assert( !h_is_pos__index( pos ));
 451                 break;
 452             }
 453             line.rho = pindex->rho;
 454             line.angle = pindex->theta;
 455             cvSeqPush( lines, &line );
 456
 457             if( lines->total >= linesMax )
 458                 EXIT;
 459             h_get_next__index( &pos );
 460         }
 461     }
 462
 463     __END__;
 464
 465     h_destroy_list__index( list );
 466     cvFree( &sinTable );
 467     cvFree( &x );
 468     cvFree( &y );
 469     cvFree( &caccum );
 470     cvFree( &buffer );
 471 }
 472
 473
 474 /****************************************************************************************\
 475 *                              Probabilistic Hough Transform                             *
 476 \****************************************************************************************/
 477
 478 #if defined WIN64 && defined EM64T && _MSC_VER == 1400 && !defined CV_ICC
 479 #pragma optimize("",off)
 480 #endif
 481
 482 static void
 483 icvHoughLinesProbabalistic( CvMat* image,
 484                             float rho, float theta, int threshold,
 485                             int lineLength, int lineGap,
 486                             CvSeq *lines, int linesMax )
 487 {
 488     CvMat* accum = 0;
 489     CvMat* mask = 0;
 490     CvMat* trigtab = 0;
 491     CvMemStorage* storage = 0;
 492
 493     CV_FUNCNAME( "icvHoughLinesProbalistic" );
 494
 495     __BEGIN__;
 496
 497     CvSeq* seq;
 498     CvSeqWriter writer;
 499     int width, height;
 500     int numangle, numrho;
 501     float ang;
 502     int r, n, count;
 503     CvPoint pt;
 504     float irho = 1 / rho;
 505     CvRNG rng = cvRNG(-1);
 506     const float* ttab;
 507     uchar* mdata0;
 508
 509     CV_ASSERT( CV_IS_MAT(image) && CV_MAT_TYPE(image->type) == CV_8UC1 );
 510
 511     width = image->cols;
 512     height = image->rows;
 513
 514     numangle = cvRound(CV_PI / theta);
 515     numrho = cvRound(((width + height) * 2 + 1) / rho);
 516
 517     CV_CALL( accum = cvCreateMat( numangle, numrho, CV_32SC1 ));
 518     CV_CALL( mask = cvCreateMat( height, width, CV_8UC1 ));
 519     CV_CALL( trigtab = cvCreateMat( 1, numangle, CV_32FC2 ));
 520     cvZero( accum );
 521
 522     CV_CALL( storage = cvCreateMemStorage(0) );
 523
 524     for( ang = 0, n = 0; n < numangle; ang += theta, n++ )
 525     {
 526         trigtab->data.fl[n*2] = (float)(cos(ang) * irho);
 527         trigtab->data.fl[n*2+1] = (float)(sin(ang) * irho);
 528     }
 529     ttab = trigtab->data.fl;
 530     mdata0 = mask->data.ptr;
 531
 532     CV_CALL( cvStartWriteSeq( CV_32SC2, sizeof(CvSeq), sizeof(CvPoint), storage, &writer ));
 533
 534     // stage 1. collect non-zero image points
 535     for( pt.y = 0, count = 0; pt.y < height; pt.y++ )
 536     {
 537         const uchar* data = image->data.ptr + pt.y*image->step;
 538         uchar* mdata = mdata0 + pt.y*width;
 539         for( pt.x = 0; pt.x < width; pt.x++ )
 540         {
 541             if( data[pt.x] )
 542             {
 543                 mdata[pt.x] = (uchar)1;
 544                 CV_WRITE_SEQ_ELEM( pt, writer );
 545             }
 546             else
 547                 mdata[pt.x] = 0;
 548         }
 549     }
 550
 551     seq = cvEndWriteSeq( &writer );
 552     count = seq->total;
 553
 554     // stage 2. process all the points in random order
 555     for( ; count > 0; count-- )
 556     {
 557         // choose random point out of the remaining ones
 558         int idx = cvRandInt(&rng) % count;
 559         int max_val = threshold-1, max_n = 0;
 560         CvPoint* pt = (CvPoint*)cvGetSeqElem( seq, idx );
 561         CvPoint line_end[2] = {{0,0}, {0,0}};
 562         float a, b;
 563         int* adata = accum->data.i;
 564         int i, j, k, x0, y0, dx0, dy0, xflag;
 565         int good_line;
 566         const int shift = 16;
 567
 568         i = pt->y;
 569         j = pt->x;
 570
 571         // "remove" it by overriding it with the last element
 572         *pt = *(CvPoint*)cvGetSeqElem( seq, count-1 );
 573
 574         // check if it has been excluded already (i.e. belongs to some other line)
 575         if( !mdata0[i*width + j] )
 576             continue;
 577
 578         // update accumulator, find the most probable line
 579         for( n = 0; n < numangle; n++, adata += numrho )
 580         {
 581             r = cvRound( j * ttab[n*2] + i * ttab[n*2+1] );
 582             r += (numrho - 1) / 2;
 583             int val = ++adata[r];
 584             if( max_val < val )
 585             {
 586                 max_val = val;
 587                 max_n = n;
 588             }
 589         }
 590
 591         // if it is too "weak" candidate, continue with another point
 592         if( max_val < threshold )
 593             continue;
 594
 595         // from the current point walk in each direction
 596         // along the found line and extract the line segment
 597         a = -ttab[max_n*2+1];
 598         b = ttab[max_n*2];
 599         x0 = j;
 600         y0 = i;
 601         if( fabs(a) > fabs(b) )
 602         {
 603             xflag = 1;
 604             dx0 = a > 0 ? 1 : -1;
 605             dy0 = cvRound( b*(1 << shift)/fabs(a) );
 606             y0 = (y0 << shift) + (1 << (shift-1));
 607         }
 608         else
 609         {
 610             xflag = 0;
 611             dy0 = b > 0 ? 1 : -1;
 612             dx0 = cvRound( a*(1 << shift)/fabs(b) );
 613             x0 = (x0 << shift) + (1 << (shift-1));
 614         }
 615
 616         for( k = 0; k < 2; k++ )
 617         {
 618             int gap = 0, x = x0, y = y0, dx = dx0, dy = dy0;
 619
 620             if( k > 0 )
 621                 dx = -dx, dy = -dy;
 622
 623             // walk along the line using fixed-point arithmetics,
 624             // stop at the image border or in case of too big gap
 625             for( ;; x += dx, y += dy )
 626             {
 627                 uchar* mdata;
 628                 int i1, j1;
 629
 630                 if( xflag )
 631                 {
 632                     j1 = x;
 633                     i1 = y >> shift;
 634                 }
 635                 else
 636                 {
 637                     j1 = x >> shift;
 638                     i1 = y;
 639                 }
 640
 641                 if( j1 < 0 || j1 >= width || i1 < 0 || i1 >= height )
 642                     break;
 643
 644                 mdata = mdata0 + i1*width + j1;
 645
 646                 // for each non-zero point:
 647                 //    update line end,
 648                 //    clear the mask element
 649                 //    reset the gap
 650                 if( *mdata )
 651                 {
 652                     gap = 0;
 653                     line_end[k].y = i1;
 654                     line_end[k].x = j1;
 655                 }
 656                 else if( ++gap > lineGap )
 657                     break;
 658             }
 659         }
 660
 661         good_line = abs(line_end[1].x - line_end[0].x) >= lineLength ||
 662                     abs(line_end[1].y - line_end[0].y) >= lineLength;
 663
 664         for( k = 0; k < 2; k++ )
 665         {
 666             int x = x0, y = y0, dx = dx0, dy = dy0;
 667
 668             if( k > 0 )
 669                 dx = -dx, dy = -dy;
 670
 671             // walk along the line using fixed-point arithmetics,
 672             // stop at the image border or in case of too big gap
 673             for( ;; x += dx, y += dy )
 674             {
 675                 uchar* mdata;
 676                 int i1, j1;
 677
 678                 if( xflag )
 679                 {
 680                     j1 = x;
 681                     i1 = y >> shift;
 682                 }
 683                 else
 684                 {
 685                     j1 = x >> shift;
 686                     i1 = y;
 687                 }
 688
 689                 mdata = mdata0 + i1*width + j1;
 690
 691                 // for each non-zero point:
 692                 //    update line end,
 693                 //    clear the mask element
 694                 //    reset the gap
 695                 if( *mdata )
 696                 {
 697                     if( good_line )
 698                     {
 699                         adata = accum->data.i;
 700                         for( n = 0; n < numangle; n++, adata += numrho )
 701                         {
 702                             r = cvRound( j1 * ttab[n*2] + i1 * ttab[n*2+1] );
 703                             r += (numrho - 1) / 2;
 704                             adata[r]--;
 705                         }
 706                     }
 707                     *mdata = 0;
 708                 }
 709
 710                 if( i1 == line_end[k].y && j1 == line_end[k].x )
 711                     break;
 712             }
 713         }
 714
 715         if( good_line )
 716         {
 717             CvRect lr = { line_end[0].x, line_end[0].y, line_end[1].x, line_end[1].y };
 718             cvSeqPush( lines, &lr );
 719             if( lines->total >= linesMax )
 720                 EXIT;
 721         }
 722     }
 723
 724     __END__;
 725
 726     cvReleaseMat( &accum );
 727     cvReleaseMat( &mask );
 728     cvReleaseMat( &trigtab );
 729     cvReleaseMemStorage( &storage );
 730 }
 731
 732
 733 #if defined WIN64 && defined EM64T && _MSC_VER == 1400 && !defined CV_ICC
 734 #pragma optimize("",on)
 735 #endif
 736
 737
 738 /* Wrapper function for standard hough transform */
 739 CV_IMPL CvSeq*
 740 cvHoughLines2( CvArr* src_image, void* lineStorage, int method,
 741                double rho, double theta, int threshold,
 742                double param1, double param2 )
 743 {
 744     CvSeq* result = 0;
 745
 746     CV_FUNCNAME( "cvHoughLines" );
 747
 748     __BEGIN__;
 749
 750     CvMat stub, *img = (CvMat*)src_image;
 751     CvMat* mat = 0;
 752     CvSeq* lines = 0;
 753     CvSeq lines_header;
 754     CvSeqBlock lines_block;
 755     int lineType, elemSize;
 756     int linesMax = INT_MAX;
 757     int iparam1, iparam2;
 758
 759     CV_CALL( img = cvGetMat( img, &stub ));
 760
 761     if( !CV_IS_MASK_ARR(img))
 762         CV_ERROR( CV_StsBadArg, "The source image must be 8-bit, single-channel" );
 763
 764     if( !lineStorage )
 765         CV_ERROR( CV_StsNullPtr, "NULL destination" );
 766
 767     if( rho <= 0 || theta <= 0 || threshold <= 0 )
 768         CV_ERROR( CV_StsOutOfRange, "rho, theta and threshold must be positive" );
 769
 770     if( method != CV_HOUGH_PROBABILISTIC )
 771     {
 772         lineType = CV_32FC2;
 773         elemSize = sizeof(float)*2;
 774     }
 775     else
 776     {
 777         lineType = CV_32SC4;
 778         elemSize = sizeof(int)*4;
 779     }
 780
 781     if( CV_IS_STORAGE( lineStorage ))
 782     {
 783         CV_CALL( lines = cvCreateSeq( lineType, sizeof(CvSeq), elemSize, (CvMemStorage*)lineStorage ));
 784     }
 785     else if( CV_IS_MAT( lineStorage ))
 786     {
 787         mat = (CvMat*)lineStorage;
 788
 789         if( !CV_IS_MAT_CONT( mat->type ) || mat->rows != 1 && mat->cols != 1 )
 790             CV_ERROR( CV_StsBadArg,
 791             "The destination matrix should be continuous and have a single row or a single column" );
 792
 793         if( CV_MAT_TYPE( mat->type ) != lineType )
 794             CV_ERROR( CV_StsBadArg,
 795             "The destination matrix data type is inappropriate, see the manual" );
 796
 797         CV_CALL( lines = cvMakeSeqHeaderForArray( lineType, sizeof(CvSeq), elemSize, mat->data.ptr,
 798                                                   mat->rows + mat->cols - 1, &lines_header, &lines_block ));
 799         linesMax = lines->total;
 800         CV_CALL( cvClearSeq( lines ));
 801     }
 802     else
 803     {
 804         CV_ERROR( CV_StsBadArg, "Destination is not CvMemStorage* nor CvMat*" );
 805     }
 806
 807     iparam1 = cvRound(param1);
 808     iparam2 = cvRound(param2);
 809
 810     switch( method )
 811     {
 812     case CV_HOUGH_STANDARD:
 813           CV_CALL( icvHoughLinesStandard( img, (float)rho,
 814                 (float)theta, threshold, lines, linesMax ));
 815           break;
 816     case CV_HOUGH_MULTI_SCALE:
 817           CV_CALL( icvHoughLinesSDiv( img, (float)rho, (float)theta,
 818                 threshold, iparam1, iparam2, lines, linesMax ));
 819           break;
 820     case CV_HOUGH_PROBABILISTIC:
 821           CV_CALL( icvHoughLinesProbabalistic( img, (float)rho, (float)theta,
 822                 threshold, iparam1, iparam2, lines, linesMax ));
 823           break;
 824     default:
 825         CV_ERROR( CV_StsBadArg, "Unrecognized method id" );
 826     }
 827
 828     if( mat )
 829     {
 830         if( mat->cols > mat->rows )
 831             mat->cols = lines->total;
 832         else
 833             mat->rows = lines->total;
 834     }
 835     else
 836     {
 837         result = lines;
 838     }
 839
 840     __END__;
 841
 842     return result;
 843 }
 844
 845
 846 /****************************************************************************************\
 847 *                                     Circle Detection                                   *
 848 \****************************************************************************************/
 849
 850 static void
 851 icvHoughCirclesGradient( CvMat* img, float dp, float min_dist,
 852                          int min_radius, int max_radius,
 853                          int canny_threshold, int acc_threshold,
 854                          CvSeq* circles, int circles_max )
 855 {
 856     const int SHIFT = 10, ONE = 1 << SHIFT, R_THRESH = 30;
 857     CvMat *dx = 0, *dy = 0;
 858     CvMat *edges = 0;
 859     CvMat *accum = 0;
 860     int* sort_buf = 0;
 861     CvMat* dist_buf = 0;
 862     CvMemStorage* storage = 0;
 863
 864     CV_FUNCNAME( "icvHoughCirclesGradient" );
 865
 866     __BEGIN__;
 867
 868     int x, y, i, j, center_count, nz_count;
 869     int rows, cols, arows, acols;
 870     int astep, *adata;
 871     float* ddata;
 872     CvSeq *nz, *centers;
 873     float idp, dr;
 874     CvSeqReader reader;
 875
 876     CV_CALL( edges = cvCreateMat( img->rows, img->cols, CV_8UC1 ));
 877     CV_CALL( cvCanny( img, edges, MAX(canny_threshold/2,1), canny_threshold, 3 ));
 878
 879     CV_CALL( dx = cvCreateMat( img->rows, img->cols, CV_16SC1 ));
 880     CV_CALL( dy = cvCreateMat( img->rows, img->cols, CV_16SC1 ));
 881     CV_CALL( cvSobel( img, dx, 1, 0, 3 ));
 882     CV_CALL( cvSobel( img, dy, 0, 1, 3 ));
 883
 884     if( dp < 1.f )
 885         dp = 1.f;
 886     idp = 1.f/dp;
 887     CV_CALL( accum = cvCreateMat( cvCeil(img->rows*idp)+2, cvCeil(img->cols*idp)+2, CV_32SC1 ));
 888     CV_CALL( cvZero(accum));
 889
 890     CV_CALL( storage = cvCreateMemStorage() );
 891     CV_CALL( nz = cvCreateSeq( CV_32SC2, sizeof(CvSeq), sizeof(CvPoint), storage ));
 892     CV_CALL( centers = cvCreateSeq( CV_32SC1, sizeof(CvSeq), sizeof(int), storage ));
 893
 894     rows = img->rows;
 895     cols = img->cols;
 896     arows = accum->rows - 2;
 897     acols = accum->cols - 2;
 898     adata = accum->data.i;
 899     astep = accum->step/sizeof(adata[0]);
 900
 901     for( y = 0; y < rows; y++ )
 902     {
 903         const uchar* edges_row = edges->data.ptr + y*edges->step;
 904         const short* dx_row = (const short*)(dx->data.ptr + y*dx->step);
 905         const short* dy_row = (const short*)(dy->data.ptr + y*dy->step);
 906
 907         for( x = 0; x < cols; x++ )
 908         {
 909             float vx, vy;
 910             int sx, sy, x0, y0, x1, y1, r, k;
 911             CvPoint pt;
 912
 913             vx = dx_row[x];
 914             vy = dy_row[x];
 915
 916             if( !edges_row[x] || vx == 0 && vy == 0 )
 917                 continue;
 918
 919             if( fabs(vx) < fabs(vy) )
 920             {
 921                 sx = cvRound(vx*ONE/fabs(vy));
 922                 sy = vy < 0 ? -ONE : ONE;
 923             }
 924             else
 925             {
 926                 assert( vx != 0 );
 927                 sy = cvRound(vy*ONE/fabs(vx));
 928                 sx = vx < 0 ? -ONE : ONE;
 929             }
 930
 931             x0 = cvRound((x*idp)*ONE) + ONE + (ONE/2);
 932             y0 = cvRound((y*idp)*ONE) + ONE + (ONE/2);
 933
 934             for( k = 0; k < 2; k++ )
 935             {
 936                 x0 += min_radius * sx;
 937                 y0 += min_radius * sy;
 938
 939                 for( x1 = x0, y1 = y0, r = min_radius; r <= max_radius; x1 += sx, y1 += sy, r++ )
 940                 {
 941                     int x2 = x1 >> SHIFT, y2 = y1 >> SHIFT;
 942                     if( (unsigned)x2 >= (unsigned)acols ||
 943                         (unsigned)y2 >= (unsigned)arows )
 944                         break;
 945                     adata[y2*astep + x2]++;
 946                 }
 947
 948                 x0 -= min_radius * sx;
 949                 y0 -= min_radius * sy;
 950                 sx = -sx; sy = -sy;
 951             }
 952
 953             pt.x = x; pt.y = y;
 954             cvSeqPush( nz, &pt );
 955         }
 956     }
 957
 958     nz_count = nz->total;
 959     if( !nz_count )
 960         EXIT;
 961
 962     for( y = 1; y < arows - 1; y++ )
 963     {
 964         for( x = 1; x < acols - 1; x++ )
 965         {
 966             int base = y*(acols+2) + x;
 967             if( adata[base] > acc_threshold &&
 968                 adata[base] > adata[base-1] && adata[base] > adata[base+1] &&
 969                 adata[base] > adata[base-acols-2] && adata[base] > adata[base+acols+2] )
 970                 cvSeqPush(centers, &base);
 971         }
 972     }
 973
 974     center_count = centers->total;
 975     if( !center_count )
 976         EXIT;
 977
 978     CV_CALL( sort_buf = (int*)cvAlloc( MAX(center_count,nz_count)*sizeof(sort_buf[0]) ));
 979     cvCvtSeqToArray( centers, sort_buf );
 980
 981     icvHoughSortDescent32s( sort_buf, center_count, adata );
 982     cvClearSeq( centers );
 983     cvSeqPushMulti( centers, sort_buf, center_count );
 984
 985     CV_CALL( dist_buf = cvCreateMat( 1, nz_count, CV_32FC1 ));
 986     ddata = dist_buf->data.fl;
 987
 988     dr = dp;
 989     min_dist = MAX( min_dist, dp );
 990     min_dist *= min_dist;
 991
 992     for( i = 0; i < centers->total; i++ )
 993     {
 994         int ofs = *(int*)cvGetSeqElem( centers, i );
 995         y = ofs/(acols+2) - 1;
 996         x = ofs - (y+1)*(acols+2) - 1;
 997         float cx = (float)(x*dp), cy = (float)(y*dp);
 998         int start_idx = nz_count - 1;
 999         float start_dist, dist_sum;
1000         float r_best = 0, c[3];
1001         int max_count = R_THRESH;
1002
1003         for( j = 0; j < circles->total; j++ )
1004         {
1005             float* c = (float*)cvGetSeqElem( circles, j );
1006             if( (c[0] - cx)*(c[0] - cx) + (c[1] - cy)*(c[1] - cy) < min_dist )
1007                 break;
1008         }
1009
1010         if( j < circles->total )
1011             continue;
1012
1013         cvStartReadSeq( nz, &reader );
1014         for( j = 0; j < nz_count; j++ )
1015         {
1016             CvPoint pt;
1017             float _dx, _dy;
1018             CV_READ_SEQ_ELEM( pt, reader );
1019             _dx = cx - pt.x; _dy = cy - pt.y;
1020             ddata[j] = _dx*_dx + _dy*_dy;
1021             sort_buf[j] = j;
1022         }
1023
1024         cvPow( dist_buf, dist_buf, 0.5 );
1025         icvHoughSortDescent32s( sort_buf, nz_count, (int*)ddata );
1026
1027         dist_sum = start_dist = ddata[sort_buf[nz_count-1]];
1028         for( j = nz_count - 2; j >= 0; j-- )
1029         {
1030             float d = ddata[sort_buf[j]];
1031
1032             if( d > max_radius )
1033                 break;
1034
1035             if( d - start_dist > dr )
1036             {
1037                 float r_cur = ddata[sort_buf[(j + start_idx)/2]];
1038                 if( (start_idx - j)*r_best >= max_count*r_cur ||
1039                     r_best < FLT_EPSILON && start_idx - j >= max_count )
1040                 {
1041                     r_best = r_cur;
1042                     max_count = start_idx - j;
1043                 }
1044                 start_dist = d;
1045                 start_idx = j;
1046                 dist_sum = 0;
1047             }
1048             dist_sum += d;
1049         }
1050
1051         if( max_count > R_THRESH )
1052         {
1053             c[0] = cx;
1054             c[1] = cy;
1055             c[2] = (float)r_best;
1056             cvSeqPush( circles, c );
1057             if( circles->total > circles_max )
1058                 EXIT;
1059         }
1060     }
1061
1062     __END__;
1063
1064     cvReleaseMat( &dist_buf );
1065     cvFree( &sort_buf );
1066     cvReleaseMemStorage( &storage );
1067     cvReleaseMat( &edges );
1068     cvReleaseMat( &dx );
1069     cvReleaseMat( &dy );
1070     cvReleaseMat( &accum );
1071 }
1072
1073 CV_IMPL CvSeq*
1074 cvHoughCircles( CvArr* src_image, void* circle_storage,
1075                 int method, double dp, double min_dist,
1076                 double param1, double param2,
1077                 int min_radius, int max_radius )
1078 {
1079     CvSeq* result = 0;
1080
1081     CV_FUNCNAME( "cvHoughCircles" );
1082
1083     __BEGIN__;
1084
1085     CvMat stub, *img = (CvMat*)src_image;
1086     CvMat* mat = 0;
1087     CvSeq* circles = 0;
1088     CvSeq circles_header;
1089     CvSeqBlock circles_block;
1090     int circles_max = INT_MAX;
1091     int canny_threshold = cvRound(param1);
1092     int acc_threshold = cvRound(param2);
1093
1094     CV_CALL( img = cvGetMat( img, &stub ));
1095
1096     if( !CV_IS_MASK_ARR(img))
1097         CV_ERROR( CV_StsBadArg, "The source image must be 8-bit, single-channel" );
1098
1099     if( !circle_storage )
1100         CV_ERROR( CV_StsNullPtr, "NULL destination" );
1101
1102     if( dp <= 0 || min_dist <= 0 || canny_threshold <= 0 || acc_threshold <= 0 )
1103         CV_ERROR( CV_StsOutOfRange, "dp, min_dist, canny_threshold and acc_threshold must be all positive numbers" );
1104
1105     min_radius = MAX( min_radius, 0 );
1106     if( max_radius <= 0 )
1107         max_radius = MAX( img->rows, img->cols );
1108     else if( max_radius <= min_radius )
1109         max_radius = min_radius + 2;
1110
1111     if( CV_IS_STORAGE( circle_storage ))
1112     {
1113         CV_CALL( circles = cvCreateSeq( CV_32FC3, sizeof(CvSeq),
1114             sizeof(float)*3, (CvMemStorage*)circle_storage ));
1115     }
1116     else if( CV_IS_MAT( circle_storage ))
1117     {
1118         mat = (CvMat*)circle_storage;
1119
1120         if( !CV_IS_MAT_CONT( mat->type ) || mat->rows != 1 && mat->cols != 1 ||
1121             CV_MAT_TYPE(mat->type) != CV_32FC3 )
1122             CV_ERROR( CV_StsBadArg,
1123             "The destination matrix should be continuous and have a single row or a single column" );
1124
1125         CV_CALL( circles = cvMakeSeqHeaderForArray( CV_32FC3, sizeof(CvSeq), sizeof(float)*3,
1126                 mat->data.ptr, mat->rows + mat->cols - 1, &circles_header, &circles_block ));
1127         circles_max = circles->total;
1128         CV_CALL( cvClearSeq( circles ));
1129     }
1130     else
1131     {
1132         CV_ERROR( CV_StsBadArg, "Destination is not CvMemStorage* nor CvMat*" );
1133     }
1134
1135     switch( method )
1136     {
1137     case CV_HOUGH_GRADIENT:
1138           CV_CALL( icvHoughCirclesGradient( img, (float)dp, (float)min_dist,
1139                                     min_radius, max_radius, canny_threshold,
1140                                     acc_threshold, circles, circles_max ));
1141           break;
1142     default:
1143         CV_ERROR( CV_StsBadArg, "Unrecognized method id" );
1144     }
1145
1146     if( mat )
1147     {
1148         if( mat->cols > mat->rows )
1149             mat->cols = circles->total;
1150         else
1151             mat->rows = circles->total;
1152     }
1153     else
1154         result = circles;
1155
1156     __END__;
1157
1158     return result;
1159 }
1160
1161 /* End of file. */