modules/calib3d/src/calibinit.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 /************************************************************************************\
  43     This is improved variant of chessboard corner detection algorithm that
  44     uses a graph of connected quads. It is based on the code contributed
  45     by Vladimir Vezhnevets and Philip Gruebele.
  46     Here is the copyright notice from the original Vladimir's code:
  47     ===============================================================
  48
  49     The algorithms developed and implemented by Vezhnevets Vldimir
  50     aka Dead Moroz (vvp@graphics.cs.msu.ru)
  51     See http://graphics.cs.msu.su/en/research/calibration/opencv.html
  52     for detailed information.
  53
  54     Reliability additions and modifications made by Philip Gruebele.
  55     <a href="mailto:pgruebele@cox.net">pgruebele@cox.net</a>
  56
  57     Some further improvements for detection of partially ocluded boards at non-ideal
  58     lighting conditions have been made by Alex Bovyrin and Kurt Kolonige
  59
  60 \************************************************************************************/
  61
  62 /************************************************************************************\
  63   This version adds a new and improved variant of chessboard corner detection
  64   that works better in poor lighting condition. It is based on work from
  65   Oliver Schreer and Stefano Masneri. This method works faster than the previous
  66   one and reverts back to the older method in case no chessboard detection is
  67   possible. Overall performance improves also because now the method avoids
  68   performing the same computation multiple times when not necessary.
  69
  70 \************************************************************************************/
  71
  72 #include "precomp.hpp"
  73 #include "opencv2/imgproc/imgproc_c.h"
  74 #include "opencv2/calib3d/calib3d_c.h"
  75 #include "circlesgrid.hpp"
  76 #include <stdarg.h>
  77 #include <vector>
  78
  79 using namespace cv;
  80 using namespace std;
  81
  82 //#define ENABLE_TRIM_COL_ROW
  83
  84 //#define DEBUG_CHESSBOARD
  85
  86 #ifdef DEBUG_CHESSBOARD
  87 static int PRINTF( const char* fmt, ... )
  88 {
  89     va_list args;
  90     va_start(args, fmt);
  91     return vprintf(fmt, args);
  92 }
  93 #else
  94 #define PRINTF(...)
  95 #endif
  96
  97 //=====================================================================================
  98 // Implementation for the enhanced calibration object detection
  99 //=====================================================================================
 100
 101 #define MAX_CONTOUR_APPROX  7
 102
 103 struct CvContourEx
 104 {
 105     CV_CONTOUR_FIELDS()
 106     int counter;
 107 };
 108
 109 //=====================================================================================
 110
 111 /// Corner info structure
 112 /** This structure stores information about the chessboard corner.*/
 113 struct CvCBCorner
 114 {
 115     CvPoint2D32f pt; // Coordinates of the corner
 116     int row;         // Board row index
 117     int count;       // Number of neighbor corners
 118     struct CvCBCorner* neighbors[4]; // Neighbor corners
 119
 120     float meanDist(int *_n) const
 121     {
 122         float sum = 0;
 123         int n = 0;
 124         for( int i = 0; i < 4; i++ )
 125         {
 126             if( neighbors[i] )
 127             {
 128                 float dx = neighbors[i]->pt.x - pt.x;
 129                 float dy = neighbors[i]->pt.y - pt.y;
 130                 sum += sqrt(dx*dx + dy*dy);
 131                 n++;
 132             }
 133         }
 134         if(_n)
 135             *_n = n;
 136         return sum/MAX(n,1);
 137     }
 138 };
 139
 140 //=====================================================================================
 141 /// Quadrangle contour info structure
 142 /** This structure stores information about the chessboard quadrange.*/
 143 struct CvCBQuad
 144 {
 145     int count;      // Number of quad neighbors
 146     int group_idx;  // quad group ID
 147     int row, col;   // row and column of this quad
 148     bool ordered;   // true if corners/neighbors are ordered counter-clockwise
 149     float edge_len; // quad edge len, in pix^2
 150     // neighbors and corners are synced, i.e., neighbor 0 shares corner 0
 151     CvCBCorner *corners[4]; // Coordinates of quad corners
 152     struct CvCBQuad *neighbors[4]; // Pointers of quad neighbors
 153 };
 154
 155 //=====================================================================================
 156
 157 #ifdef DEBUG_CHESSBOARD
 158 #include "opencv2/highgui.hpp"
 159 #include "opencv2/imgproc.hpp"
 160 static void SHOW(const std::string & name, Mat & img)
 161 {
 162     imshow(name, img);
 163     while ((uchar)waitKey(0) != 'q') {}
 164 }
 165 static void SHOW_QUADS(const std::string & name, const Mat & img_, CvCBQuad * quads, int quads_count)
 166 {
 167     Mat img = img_.clone();
 168     if (img.channels() == 1)
 169         cvtColor(img, img, COLOR_GRAY2BGR);
 170     for (int i = 0; i < quads_count; ++i)
 171     {
 172         CvCBQuad & quad = quads[i];
 173         for (int j = 0; j < 4; ++j)
 174         {
 175             line(img, quad.corners[j]->pt, quad.corners[(j + 1) % 4]->pt, Scalar(0, 240, 0), 1, LINE_AA);
 176         }
 177     }
 178     imshow(name, img);
 179     while ((uchar)waitKey(0) != 'q') {}
 180 }
 181 #else
 182 #define SHOW(...)
 183 #define SHOW_QUADS(...)
 184 #endif
 185
 186 //=====================================================================================
 187
 188 static int icvGenerateQuads( CvCBQuad **quads, CvCBCorner **corners,
 189                              CvMemStorage *storage, const Mat &image_, int flags, int *max_quad_buf_size);
 190
 191 static bool processQuads(CvCBQuad *quads, int quad_count, CvSize pattern_size, int max_quad_buf_size,
 192                          CvMemStorage * storage, CvCBCorner *corners, CvPoint2D32f *out_corners, int *out_corner_count, int & prev_sqr_size);
 193
 194 /*static int
 195 icvGenerateQuadsEx( CvCBQuad **out_quads, CvCBCorner **out_corners,
 196     CvMemStorage *storage, CvMat *image, CvMat *thresh_img, int dilation, int flags );*/
 197
 198 static void icvFindQuadNeighbors( CvCBQuad *quads, int quad_count );
 199
 200 static int icvFindConnectedQuads( CvCBQuad *quads, int quad_count,
 201                                   CvCBQuad **quad_group, int group_idx,
 202                                   CvMemStorage* storage );
 203
 204 static int icvCheckQuadGroup( CvCBQuad **quad_group, int count,
 205                               CvCBCorner **out_corners, CvSize pattern_size );
 206
 207 static int icvCleanFoundConnectedQuads( int quad_count,
 208                 CvCBQuad **quads, CvSize pattern_size );
 209
 210 static int icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,
 211            int *all_count, CvCBQuad **all_quads, CvCBCorner **corners,
 212            CvSize pattern_size, int max_quad_buf_size, CvMemStorage* storage );
 213
 214 static void icvOrderQuad(CvCBQuad *quad, CvCBCorner *corner, int common);
 215
 216 #ifdef ENABLE_TRIM_COL_ROW
 217 static int icvTrimCol(CvCBQuad **quads, int count, int col, int dir);
 218
 219 static int icvTrimRow(CvCBQuad **quads, int count, int row, int dir);
 220 #endif
 221
 222 static int icvAddOuterQuad(CvCBQuad *quad, CvCBQuad **quads, int quad_count,
 223                     CvCBQuad **all_quads, int all_count, CvCBCorner **corners, int max_quad_buf_size);
 224
 225 static void icvRemoveQuadFromGroup(CvCBQuad **quads, int count, CvCBQuad *q0);
 226
 227 static int icvCheckBoardMonotony( CvPoint2D32f* corners, CvSize pattern_size );
 228
 229 /***************************************************************************************************/
 230 //COMPUTE INTENSITY HISTOGRAM OF INPUT IMAGE
 231 static int icvGetIntensityHistogram( const Mat & img, std::vector<int>& piHist )
 232 {
 233     // sum up all pixel in row direction and divide by number of columns
 234     for ( int j=0; j<img.rows; j++ )
 235     {
 236         const uchar * row = img.ptr(j);
 237         for ( int i=0; i<img.cols; i++ )
 238         {
 239             piHist[row[i]]++;
 240         }
 241     }
 242     return 0;
 243 }
 244 /***************************************************************************************************/
 245 //SMOOTH HISTOGRAM USING WINDOW OF SIZE 2*iWidth+1
 246 static int icvSmoothHistogram( const std::vector<int>& piHist, std::vector<int>& piHistSmooth, int iWidth )
 247 {
 248     int iIdx;
 249     for ( int i=0; i<256; i++)
 250     {
 251         int iSmooth = 0;
 252         for ( int ii=-iWidth; ii<=iWidth; ii++)
 253         {
 254             iIdx = i+ii;
 255             if (iIdx > 0 && iIdx < 256)
 256             {
 257                 iSmooth += piHist[iIdx];
 258             }
 259         }
 260         piHistSmooth[i] = iSmooth/(2*iWidth+1);
 261     }
 262     return 0;
 263 }
 264 /***************************************************************************************************/
 265 //COMPUTE FAST HISTOGRAM GRADIENT
 266 static int icvGradientOfHistogram( const std::vector<int>& piHist, std::vector<int>& piHistGrad )
 267 {
 268     piHistGrad[0] = 0;
 269     for ( int i=1; i<255; i++)
 270     {
 271         piHistGrad[i] = piHist[i-1] - piHist[i+1];
 272         if ( abs(piHistGrad[i]) < 100 )
 273         {
 274             if ( piHistGrad[i-1] == 0)
 275                 piHistGrad[i] = -100;
 276             else
 277                 piHistGrad[i] = piHistGrad[i-1];
 278         }
 279     }
 280     return 0;
 281 }
 282 /***************************************************************************************************/
 283 //PERFORM SMART IMAGE THRESHOLDING BASED ON ANALYSIS OF INTENSTY HISTOGRAM
 284 static bool icvBinarizationHistogramBased( Mat & img )
 285 {
 286     CV_Assert(img.channels() == 1 && img.depth() == CV_8U);
 287     int iCols = img.cols;
 288     int iRows = img.rows;
 289     int iMaxPix = iCols*iRows;
 290     int iMaxPix1 = iMaxPix/100;
 291     const int iNumBins = 256;
 292     std::vector<int> piHistIntensity(iNumBins, 0);
 293     std::vector<int> piHistSmooth(iNumBins, 0);
 294     std::vector<int> piHistGrad(iNumBins, 0);
 295     std::vector<int> piAccumSum(iNumBins, 0);
 296     std::vector<int> piMaxPos(20, 0);
 297     int iThresh = 0;
 298     int iIdx;
 299     int iWidth = 1;
 300
 301     icvGetIntensityHistogram( img, piHistIntensity );
 302
 303     // get accumulated sum starting from bright
 304     piAccumSum[iNumBins-1] = piHistIntensity[iNumBins-1];
 305     for ( int i=iNumBins-2; i>=0; i-- )
 306     {
 307         piAccumSum[i] = piHistIntensity[i] + piAccumSum[i+1];
 308     }
 309
 310     // first smooth the distribution
 311     icvSmoothHistogram( piHistIntensity, piHistSmooth, iWidth );
 312
 313     // compute gradient
 314     icvGradientOfHistogram( piHistSmooth, piHistGrad );
 315
 316     // check for zeros
 317     int iCntMaxima = 0;
 318     for ( int i=iNumBins-2; (i>2) && (iCntMaxima<20); i--)
 319     {
 320         if ( (piHistGrad[i-1] < 0) && (piHistGrad[i] > 0) )
 321         {
 322             piMaxPos[iCntMaxima] = i;
 323             iCntMaxima++;
 324         }
 325     }
 326
 327     iIdx = 0;
 328     int iSumAroundMax = 0;
 329     for ( int i=0; i<iCntMaxima; i++ )
 330     {
 331         iIdx = piMaxPos[i];
 332         iSumAroundMax = piHistSmooth[iIdx-1] + piHistSmooth[iIdx] + piHistSmooth[iIdx+1];
 333         if ( iSumAroundMax < iMaxPix1 && iIdx < 64 )
 334         {
 335             for ( int j=i; j<iCntMaxima-1; j++ )
 336             {
 337                 piMaxPos[j] = piMaxPos[j+1];
 338             }
 339             iCntMaxima--;
 340             i--;
 341         }
 342     }
 343     if ( iCntMaxima == 1)
 344     {
 345         iThresh = piMaxPos[0]/2;
 346     }
 347     else if ( iCntMaxima == 2)
 348     {
 349         iThresh = (piMaxPos[0] + piMaxPos[1])/2;
 350     }
 351     else // iCntMaxima >= 3
 352     {
 353         // CHECKING THRESHOLD FOR WHITE
 354         int iIdxAccSum = 0, iAccum = 0;
 355         for (int i=iNumBins-1; i>0; i--)
 356         {
 357             iAccum += piHistIntensity[i];
 358             // iMaxPix/18 is about 5,5%, minimum required number of pixels required for white part of chessboard
 359             if ( iAccum > (iMaxPix/18) )
 360             {
 361                 iIdxAccSum = i;
 362                 break;
 363             }
 364         }
 365
 366         int iIdxBGMax = 0;
 367         int iBrightMax = piMaxPos[0];
 368         // printf("iBrightMax = %d\n", iBrightMax);
 369         for ( int n=0; n<iCntMaxima-1; n++)
 370         {
 371             iIdxBGMax = n+1;
 372             if ( piMaxPos[n] < iIdxAccSum )
 373             {
 374                 break;
 375             }
 376             iBrightMax = piMaxPos[n];
 377         }
 378
 379         // CHECKING THRESHOLD FOR BLACK
 380         int iMaxVal = piHistIntensity[piMaxPos[iIdxBGMax]];
 381
 382         //IF TOO CLOSE TO 255, jump to next maximum
 383         if ( piMaxPos[iIdxBGMax] >= 250 && iIdxBGMax < iCntMaxima )
 384         {
 385             iIdxBGMax++;
 386             iMaxVal = piHistIntensity[piMaxPos[iIdxBGMax]];
 387         }
 388
 389         for ( int n=iIdxBGMax + 1; n<iCntMaxima; n++)
 390         {
 391             if ( piHistIntensity[piMaxPos[n]] >= iMaxVal )
 392             {
 393                 iMaxVal = piHistIntensity[piMaxPos[n]];
 394                 iIdxBGMax = n;
 395             }
 396         }
 397
 398         //SETTING THRESHOLD FOR BINARIZATION
 399         int iDist2 = (iBrightMax - piMaxPos[iIdxBGMax])/2;
 400         iThresh = iBrightMax - iDist2;
 401         PRINTF("THRESHOLD SELECTED = %d, BRIGHTMAX = %d, DARKMAX = %d\n", iThresh, iBrightMax, piMaxPos[iIdxBGMax]);
 402     }
 403
 404
 405     if ( iThresh > 0 )
 406     {
 407         for ( int jj=0; jj<iRows; jj++)
 408         {
 409             uchar * row = img.ptr(jj);
 410             for ( int ii=0; ii<iCols; ii++)
 411             {
 412                 if ( row[ii] < iThresh )
 413                     row[ii] = 0;
 414                 else
 415                     row[ii] = 255;
 416             }
 417         }
 418     }
 419
 420     return true;
 421 }
 422
 423 CV_IMPL
 424 int cvFindChessboardCorners( const void* arr, CvSize pattern_size,
 425                              CvPoint2D32f* out_corners, int* out_corner_count,
 426                              int flags )
 427 {
 428     int found = 0;
 429     CvCBQuad *quads = 0;
 430     CvCBCorner *corners = 0;
 431
 432     cv::Ptr<CvMemStorage> storage;
 433
 434     CV_TRY
 435     {
 436     int k = 0;
 437     const int min_dilations = 0;
 438     const int max_dilations = 7;
 439
 440     if( out_corner_count )
 441         *out_corner_count = 0;
 442
 443     Mat img = cvarrToMat((CvMat*)arr).clone();
 444
 445     if( img.depth() != CV_8U || (img.channels() != 1 && img.channels() != 3 && img.channels() != 4) )
 446        CV_Error( CV_StsUnsupportedFormat, "Only 8-bit grayscale or color images are supported" );
 447
 448     if( pattern_size.width <= 2 || pattern_size.height <= 2 )
 449         CV_Error( CV_StsOutOfRange, "Both width and height of the pattern should have bigger than 2" );
 450
 451     if( !out_corners )
 452         CV_Error( CV_StsNullPtr, "Null pointer to corners" );
 453
 454     if (img.channels() != 1)
 455     {
 456         cvtColor(img, img, COLOR_BGR2GRAY);
 457     }
 458
 459
 460     Mat thresh_img_new = img.clone();
 461     icvBinarizationHistogramBased( thresh_img_new ); // process image in-place
 462     SHOW("New binarization", thresh_img_new);
 463
 464     if( flags & CV_CALIB_CB_FAST_CHECK)
 465     {
 466         //perform new method for checking chessboard using a binary image.
 467         //image is binarised using a threshold dependent on the image histogram
 468         if (checkChessboardBinary(thresh_img_new, pattern_size) <= 0) //fall back to the old method
 469         {
 470             if (checkChessboard(img, pattern_size) <= 0)
 471             {
 472                 return found;
 473             }
 474         }
 475     }
 476
 477     storage.reset(cvCreateMemStorage(0));
 478
 479     int prev_sqr_size = 0;
 480
 481     // Try our standard "1" dilation, but if the pattern is not found, iterate the whole procedure with higher dilations.
 482     // This is necessary because some squares simply do not separate properly with a single dilation.  However,
 483     // we want to use the minimum number of dilations possible since dilations cause the squares to become smaller,
 484     // making it difficult to detect smaller squares.
 485     for( int dilations = min_dilations; dilations <= max_dilations; dilations++ )
 486     {
 487         if (found)
 488             break;      // already found it
 489
 490         //USE BINARY IMAGE COMPUTED USING icvBinarizationHistogramBased METHOD
 491         dilate( thresh_img_new, thresh_img_new, Mat(), Point(-1, -1), 1 );
 492
 493         // So we can find rectangles that go to the edge, we draw a white line around the image edge.
 494         // Otherwise FindContours will miss those clipped rectangle contours.
 495         // The border color will be the image mean, because otherwise we risk screwing up filters like cvSmooth()...
 496         rectangle( thresh_img_new, Point(0,0), Point(thresh_img_new.cols-1, thresh_img_new.rows-1), Scalar(255,255,255), 3, LINE_8);
 497         int max_quad_buf_size = 0;
 498         cvFree(&quads);
 499         cvFree(&corners);
 500         int quad_count = icvGenerateQuads( &quads, &corners, storage, thresh_img_new, flags, &max_quad_buf_size );
 501         PRINTF("Quad count: %d/%d\n", quad_count, (pattern_size.width/2+1)*(pattern_size.height/2+1));
 502         SHOW_QUADS("New quads", thresh_img_new, quads, quad_count);
 503         if (processQuads(quads, quad_count, pattern_size, max_quad_buf_size, storage, corners, out_corners, out_corner_count, prev_sqr_size))
 504             found = 1;
 505     }
 506
 507     PRINTF("Chessboard detection result 0: %d\n", found);
 508
 509     // revert to old, slower, method if detection failed
 510     if (!found)
 511     {
 512         if( flags & CV_CALIB_CB_NORMALIZE_IMAGE )
 513         {
 514             equalizeHist( img, img );
 515         }
 516
 517         Mat thresh_img;
 518         prev_sqr_size = 0;
 519
 520         PRINTF("Fallback to old algorithm\n");
 521         const bool useAdaptive = flags & CV_CALIB_CB_ADAPTIVE_THRESH;
 522         if (!useAdaptive)
 523         {
 524             // empiric threshold level
 525             // thresholding performed here and not inside the cycle to save processing time
 526             double mean = cv::mean(img).val[0];
 527             int thresh_level = MAX(cvRound( mean - 10 ), 10);
 528             threshold( img, thresh_img, thresh_level, 255, THRESH_BINARY );
 529         }
 530         //if flag CV_CALIB_CB_ADAPTIVE_THRESH is not set it doesn't make sense to iterate over k
 531         int max_k = useAdaptive ? 6 : 1;
 532         for( k = 0; k < max_k; k++ )
 533         {
 534             for( int dilations = min_dilations; dilations <= max_dilations; dilations++ )
 535             {
 536                 if (found)
 537                     break;      // already found it
 538
 539                 // convert the input grayscale image to binary (black-n-white)
 540                 if (useAdaptive)
 541                 {
 542                     int block_size = cvRound(prev_sqr_size == 0
 543                                              ? MIN(img.cols, img.rows) * (k % 2 == 0 ? 0.2 : 0.1)
 544                                              : prev_sqr_size * 2);
 545                     block_size = block_size | 1;
 546                     // convert to binary
 547                     adaptiveThreshold( img, thresh_img, 255, ADAPTIVE_THRESH_MEAN_C, THRESH_BINARY, block_size, (k/2)*5 );
 548                     if (dilations > 0)
 549                         dilate( thresh_img, thresh_img, Mat(), Point(-1, -1), dilations-1 );
 550
 551                 }
 552                 else
 553                 {
 554                     dilate( thresh_img, thresh_img, Mat(), Point(-1, -1), 1 );
 555                 }
 556                 SHOW("Old binarization", thresh_img);
 557
 558                 // So we can find rectangles that go to the edge, we draw a white line around the image edge.
 559                 // Otherwise FindContours will miss those clipped rectangle contours.
 560                 // The border color will be the image mean, because otherwise we risk screwing up filters like cvSmooth()...
 561                 rectangle( thresh_img, Point(0,0), Point(thresh_img.cols-1, thresh_img.rows-1), Scalar(255,255,255), 3, LINE_8);
 562                 int max_quad_buf_size = 0;
 563                 cvFree(&quads);
 564                 cvFree(&corners);
 565                 int quad_count = icvGenerateQuads( &quads, &corners, storage, thresh_img, flags, &max_quad_buf_size);
 566                 PRINTF("Quad count: %d/%d\n", quad_count, (pattern_size.width/2+1)*(pattern_size.height/2+1));
 567                 SHOW_QUADS("Old quads", thresh_img, quads, quad_count);
 568                 if (processQuads(quads, quad_count, pattern_size, max_quad_buf_size, storage, corners, out_corners, out_corner_count, prev_sqr_size))
 569                     found = 1;
 570             }
 571         }
 572     }
 573
 574     PRINTF("Chessboard detection result 1: %d\n", found);
 575
 576     if( found )
 577         found = icvCheckBoardMonotony( out_corners, pattern_size );
 578
 579     PRINTF("Chessboard detection result 2: %d\n", found);
 580
 581     // check that none of the found corners is too close to the image boundary
 582     if( found )
 583     {
 584         const int BORDER = 8;
 585         for( k = 0; k < pattern_size.width*pattern_size.height; k++ )
 586         {
 587             if( out_corners[k].x <= BORDER || out_corners[k].x > img.cols - BORDER ||
 588                 out_corners[k].y <= BORDER || out_corners[k].y > img.rows - BORDER )
 589                 break;
 590         }
 591
 592         found = k == pattern_size.width*pattern_size.height;
 593     }
 594
 595     PRINTF("Chessboard detection result 3: %d\n", found);
 596
 597     if( found )
 598     {
 599         if ( pattern_size.height % 2 == 0 && pattern_size.width % 2 == 0 )
 600         {
 601             int last_row = (pattern_size.height-1)*pattern_size.width;
 602             double dy0 = out_corners[last_row].y - out_corners[0].y;
 603             if( dy0 < 0 )
 604             {
 605                 int n = pattern_size.width*pattern_size.height;
 606                 for(int i = 0; i < n/2; i++ )
 607                 {
 608                     CvPoint2D32f temp;
 609                     CV_SWAP(out_corners[i], out_corners[n-i-1], temp);
 610                 }
 611             }
 612         }
 613         int wsize = 2;
 614         CvMat old_img(img);
 615         cvFindCornerSubPix( &old_img, out_corners, pattern_size.width*pattern_size.height,
 616                             cvSize(wsize, wsize), cvSize(-1,-1),
 617                             cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 15, 0.1));
 618     }
 619     }
 620     CV_CATCH_ALL
 621     {
 622         cvFree(&quads);
 623         cvFree(&corners);
 624         CV_RETHROW();
 625     }
 626     cvFree(&quads);
 627     cvFree(&corners);
 628     return found;
 629 }
 630
 631 //
 632 // Checks that each board row and column is pretty much monotonous curve:
 633 // It analyzes each row and each column of the chessboard as following:
 634 //    for each corner c lying between end points in the same row/column it checks that
 635 //    the point projection to the line segment (a,b) is lying between projections
 636 //    of the neighbor corners in the same row/column.
 637 //
 638 // This function has been created as temporary workaround for the bug in current implementation
 639 // of cvFindChessboardCornes that produces absolutely unordered sets of corners.
 640 //
 641
 642 static int
 643 icvCheckBoardMonotony( CvPoint2D32f* corners, CvSize pattern_size )
 644 {
 645     int i, j, k;
 646
 647     for( k = 0; k < 2; k++ )
 648     {
 649         for( i = 0; i < (k == 0 ? pattern_size.height : pattern_size.width); i++ )
 650         {
 651             CvPoint2D32f a = k == 0 ? corners[i*pattern_size.width] : corners[i];
 652             CvPoint2D32f b = k == 0 ? corners[(i+1)*pattern_size.width-1] :
 653                 corners[(pattern_size.height-1)*pattern_size.width + i];
 654             float prevt = 0, dx0 = b.x - a.x, dy0 = b.y - a.y;
 655             if( fabs(dx0) + fabs(dy0) < FLT_EPSILON )
 656                 return 0;
 657             for( j = 1; j < (k == 0 ? pattern_size.width : pattern_size.height) - 1; j++ )
 658             {
 659                 CvPoint2D32f c = k == 0 ? corners[i*pattern_size.width + j] :
 660                     corners[j*pattern_size.width + i];
 661                 float t = ((c.x - a.x)*dx0 + (c.y - a.y)*dy0)/(dx0*dx0 + dy0*dy0);
 662                 if( t < prevt || t > 1 )
 663                     return 0;
 664                 prevt = t;
 665             }
 666         }
 667     }
 668
 669     return 1;
 670 }
 671
 672 //
 673 // order a group of connected quads
 674 // order of corners:
 675 //   0 is top left
 676 //   clockwise from there
 677 // note: "top left" is nominal, depends on initial ordering of starting quad
 678 //   but all other quads are ordered consistently
 679 //
 680 // can change the number of quads in the group
 681 // can add quads, so we need to have quad/corner arrays passed in
 682 //
 683
 684 static int
 685 icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,
 686         int *all_count, CvCBQuad **all_quads, CvCBCorner **corners,
 687         CvSize pattern_size, int max_quad_buf_size, CvMemStorage* storage )
 688 {
 689     cv::Ptr<CvMemStorage> temp_storage(cvCreateChildMemStorage( storage ));
 690     CvSeq* stack = cvCreateSeq( 0, sizeof(*stack), sizeof(void*), temp_storage );
 691
 692     // first find an interior quad
 693     CvCBQuad *start = NULL;
 694     for (int i=0; i<quad_count; i++)
 695     {
 696         if (quads[i]->count == 4)
 697         {
 698             start = quads[i];
 699             break;
 700         }
 701     }
 702
 703     if (start == NULL)
 704         return 0;   // no 4-connected quad
 705
 706     // start with first one, assign rows/cols
 707     int row_min = 0, col_min = 0, row_max=0, col_max = 0;
 708
 709     std::map<int, int> col_hist;
 710     std::map<int, int> row_hist;
 711
 712     cvSeqPush(stack, &start);
 713     start->row = 0;
 714     start->col = 0;
 715     start->ordered = true;
 716
 717     // Recursively order the quads so that all position numbers (e.g.,
 718     // 0,1,2,3) are in the at the same relative corner (e.g., lower right).
 719
 720     while( stack->total )
 721     {
 722         CvCBQuad* q;
 723         cvSeqPop( stack, &q );
 724         int col = q->col;
 725         int row = q->row;
 726         col_hist[col]++;
 727         row_hist[row]++;
 728
 729         // check min/max
 730         if (row > row_max) row_max = row;
 731         if (row < row_min) row_min = row;
 732         if (col > col_max) col_max = col;
 733         if (col < col_min) col_min = col;
 734
 735         for(int i = 0; i < 4; i++ )
 736         {
 737             CvCBQuad *neighbor = q->neighbors[i];
 738             switch(i)   // adjust col, row for this quad
 739             {           // start at top left, go clockwise
 740             case 0:
 741                 row--; col--; break;
 742             case 1:
 743                 col += 2; break;
 744             case 2:
 745                 row += 2;   break;
 746             case 3:
 747                 col -= 2; break;
 748             }
 749
 750             // just do inside quads
 751             if (neighbor && neighbor->ordered == false && neighbor->count == 4)
 752             {
 753                 PRINTF("col: %d  row: %d\n", col, row);
 754                 icvOrderQuad(neighbor, q->corners[i], (i+2)%4); // set in order
 755                 neighbor->ordered = true;
 756                 neighbor->row = row;
 757                 neighbor->col = col;
 758                 cvSeqPush( stack, &neighbor );
 759             }
 760         }
 761     }
 762
 763     for (int i=col_min; i<=col_max; i++)
 764         PRINTF("HIST[%d] = %d\n", i, col_hist[i]);
 765
 766     // analyze inner quad structure
 767     int w = pattern_size.width - 1;
 768     int h = pattern_size.height - 1;
 769     int drow = row_max - row_min + 1;
 770     int dcol = col_max - col_min + 1;
 771
 772     // normalize pattern and found quad indices
 773     if ((w > h && dcol < drow) ||
 774         (w < h && drow < dcol))
 775     {
 776         h = pattern_size.width - 1;
 777         w = pattern_size.height - 1;
 778     }
 779
 780     PRINTF("Size: %dx%d  Pattern: %dx%d\n", dcol, drow, w, h);
 781
 782     // check if there are enough inner quads
 783     if (dcol < w || drow < h)   // found enough inner quads?
 784     {
 785         PRINTF("Too few inner quad rows/cols\n");
 786         return 0;   // no, return
 787     }
 788 #ifdef ENABLE_TRIM_COL_ROW
 789     // too many columns, not very common
 790     if (dcol == w+1)    // too many, trim
 791     {
 792         PRINTF("Trimming cols\n");
 793         if (col_hist[col_max] > col_hist[col_min])
 794         {
 795             PRINTF("Trimming left col\n");
 796             quad_count = icvTrimCol(quads,quad_count,col_min,-1);
 797         }
 798         else
 799         {
 800             PRINTF("Trimming right col\n");
 801             quad_count = icvTrimCol(quads,quad_count,col_max,+1);
 802         }
 803     }
 804
 805     // too many rows, not very common
 806     if (drow == h+1)    // too many, trim
 807     {
 808         PRINTF("Trimming rows\n");
 809         if (row_hist[row_max] > row_hist[row_min])
 810         {
 811             PRINTF("Trimming top row\n");
 812             quad_count = icvTrimRow(quads,quad_count,row_min,-1);
 813         }
 814         else
 815         {
 816             PRINTF("Trimming bottom row\n");
 817             quad_count = icvTrimRow(quads,quad_count,row_max,+1);
 818         }
 819     }
 820 #endif
 821
 822     // check edges of inner quads
 823     // if there is an outer quad missing, fill it in
 824     // first order all inner quads
 825     int found = 0;
 826     for (int i=0; i<quad_count; i++)
 827     {
 828         if (quads[i]->count == 4)
 829         {   // ok, look at neighbors
 830             int col = quads[i]->col;
 831             int row = quads[i]->row;
 832             for (int j=0; j<4; j++)
 833             {
 834                 switch(j)   // adjust col, row for this quad
 835                 {       // start at top left, go clockwise
 836                 case 0:
 837                     row--; col--; break;
 838                 case 1:
 839                     col += 2; break;
 840                 case 2:
 841                     row += 2;   break;
 842                 case 3:
 843                     col -= 2; break;
 844                 }
 845                 CvCBQuad *neighbor = quads[i]->neighbors[j];
 846                 if (neighbor && !neighbor->ordered && // is it an inner quad?
 847                     col <= col_max && col >= col_min &&
 848                     row <= row_max && row >= row_min)
 849                 {
 850                     // if so, set in order
 851                     PRINTF("Adding inner: col: %d  row: %d\n", col, row);
 852                     found++;
 853                     icvOrderQuad(neighbor, quads[i]->corners[j], (j+2)%4);
 854                     neighbor->ordered = true;
 855                     neighbor->row = row;
 856                     neighbor->col = col;
 857                 }
 858             }
 859         }
 860     }
 861
 862     // if we have found inner quads, add corresponding outer quads,
 863     //   which are missing
 864     if (found > 0)
 865     {
 866         PRINTF("Found %d inner quads not connected to outer quads, repairing\n", found);
 867         for (int i=0; i<quad_count && *all_count < max_quad_buf_size; i++)
 868         {
 869             if (quads[i]->count < 4 && quads[i]->ordered)
 870             {
 871                 int added = icvAddOuterQuad(quads[i],quads,quad_count,all_quads,*all_count,corners, max_quad_buf_size);
 872                 *all_count += added;
 873                 quad_count += added;
 874             }
 875         }
 876
 877         if (*all_count >= max_quad_buf_size)
 878             return 0;
 879     }
 880
 881
 882     // final trimming of outer quads
 883     if (dcol == w && drow == h) // found correct inner quads
 884     {
 885         PRINTF("Inner bounds ok, check outer quads\n");
 886         int rcount = quad_count;
 887         for (int i=quad_count-1; i>=0; i--) // eliminate any quad not connected to
 888             // an ordered quad
 889         {
 890             if (quads[i]->ordered == false)
 891             {
 892                 bool outer = false;
 893                 for (int j=0; j<4; j++) // any neighbors that are ordered?
 894                 {
 895                     if (quads[i]->neighbors[j] && quads[i]->neighbors[j]->ordered)
 896                         outer = true;
 897                 }
 898                 if (!outer) // not an outer quad, eliminate
 899                 {
 900                     PRINTF("Removing quad %d\n", i);
 901                     icvRemoveQuadFromGroup(quads,rcount,quads[i]);
 902                     rcount--;
 903                 }
 904             }
 905
 906         }
 907         return rcount;
 908     }
 909
 910     return 0;
 911 }
 912
 913
 914 // add an outer quad
 915 // looks for the neighbor of <quad> that isn't present,
 916 //   tries to add it in.
 917 // <quad> is ordered
 918
 919 static int
 920 icvAddOuterQuad( CvCBQuad *quad, CvCBQuad **quads, int quad_count,
 921         CvCBQuad **all_quads, int all_count, CvCBCorner **corners, int max_quad_buf_size )
 922
 923 {
 924     int added = 0;
 925     for (int i=0; i<4 && all_count < max_quad_buf_size; i++) // find no-neighbor corners
 926     {
 927         if (!quad->neighbors[i])    // ok, create and add neighbor
 928         {
 929             int j = (i+2)%4;
 930             PRINTF("Adding quad as neighbor 2\n");
 931             CvCBQuad *q = &(*all_quads)[all_count];
 932             memset( q, 0, sizeof(*q) );
 933             added++;
 934             quads[quad_count] = q;
 935             quad_count++;
 936
 937             // set neighbor and group id
 938             quad->neighbors[i] = q;
 939             quad->count += 1;
 940             q->neighbors[j] = quad;
 941             q->group_idx = quad->group_idx;
 942             q->count = 1;   // number of neighbors
 943             q->ordered = false;
 944             q->edge_len = quad->edge_len;
 945
 946             // make corners of new quad
 947             // same as neighbor quad, but offset
 948             CvPoint2D32f pt = quad->corners[i]->pt;
 949             CvCBCorner* corner;
 950             float dx = pt.x - quad->corners[j]->pt.x;
 951             float dy = pt.y - quad->corners[j]->pt.y;
 952             for (int k=0; k<4; k++)
 953             {
 954                 corner = &(*corners)[all_count*4+k];
 955                 pt = quad->corners[k]->pt;
 956                 memset( corner, 0, sizeof(*corner) );
 957                 corner->pt = pt;
 958                 q->corners[k] = corner;
 959                 corner->pt.x += dx;
 960                 corner->pt.y += dy;
 961             }
 962             // have to set exact corner
 963             q->corners[j] = quad->corners[i];
 964
 965             // now find other neighbor and add it, if possible
 966             if (quad->neighbors[(i+3)%4] &&
 967                 quad->neighbors[(i+3)%4]->ordered &&
 968                 quad->neighbors[(i+3)%4]->neighbors[i] &&
 969                 quad->neighbors[(i+3)%4]->neighbors[i]->ordered )
 970             {
 971                 CvCBQuad *qn = quad->neighbors[(i+3)%4]->neighbors[i];
 972                 q->count = 2;
 973                 q->neighbors[(j+1)%4] = qn;
 974                 qn->neighbors[(i+1)%4] = q;
 975                 qn->count += 1;
 976                 // have to set exact corner
 977                 q->corners[(j+1)%4] = qn->corners[(i+1)%4];
 978             }
 979
 980             all_count++;
 981         }
 982     }
 983     return added;
 984 }
 985
 986
 987 // trimming routines
 988 #ifdef ENABLE_TRIM_COL_ROW
 989 static int
 990 icvTrimCol(CvCBQuad **quads, int count, int col, int dir)
 991 {
 992     int rcount = count;
 993     // find the right quad(s)
 994     for (int i=0; i<count; i++)
 995     {
 996 #ifdef DEBUG_CHESSBOARD
 997         if (quads[i]->ordered)
 998             PRINTF("index: %d  cur: %d\n", col, quads[i]->col);
 999 #endif
1000         if (quads[i]->ordered && quads[i]->col == col)
1001         {
1002             if (dir == 1)
1003             {
1004                 if (quads[i]->neighbors[1])
1005                 {
1006                     icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[1]);
1007                     rcount--;
1008                 }
1009                 if (quads[i]->neighbors[2])
1010                 {
1011                     icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[2]);
1012                     rcount--;
1013                 }
1014             }
1015             else
1016             {
1017                 if (quads[i]->neighbors[0])
1018                 {
1019                     icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[0]);
1020                     rcount--;
1021                 }
1022                 if (quads[i]->neighbors[3])
1023                 {
1024                     icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[3]);
1025                     rcount--;
1026                 }
1027             }
1028
1029         }
1030     }
1031     return rcount;
1032 }
1033
1034 static int
1035 icvTrimRow(CvCBQuad **quads, int count, int row, int dir)
1036 {
1037     int i, rcount = count;
1038     // find the right quad(s)
1039     for (i=0; i<count; i++)
1040     {
1041 #ifdef DEBUG_CHESSBOARD
1042         if (quads[i]->ordered)
1043             PRINTF("index: %d  cur: %d\n", row, quads[i]->row);
1044 #endif
1045         if (quads[i]->ordered && quads[i]->row == row)
1046         {
1047             if (dir == 1)   // remove from bottom
1048             {
1049                 if (quads[i]->neighbors[2])
1050                 {
1051                     icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[2]);
1052                     rcount--;
1053                 }
1054                 if (quads[i]->neighbors[3])
1055                 {
1056                     icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[3]);
1057                     rcount--;
1058                 }
1059             }
1060             else    // remove from top
1061             {
1062                 if (quads[i]->neighbors[0])
1063                 {
1064                     icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[0]);
1065                     rcount--;
1066                 }
1067                 if (quads[i]->neighbors[1])
1068                 {
1069                     icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[1]);
1070                     rcount--;
1071                 }
1072             }
1073
1074         }
1075     }
1076     return rcount;
1077 }
1078 #endif
1079
1080 //
1081 // remove quad from quad group
1082 //
1083
1084 static void
1085 icvRemoveQuadFromGroup(CvCBQuad **quads, int count, CvCBQuad *q0)
1086 {
1087     int i, j;
1088     // remove any references to this quad as a neighbor
1089     for(i = 0; i < count; i++ )
1090     {
1091         CvCBQuad *q = quads[i];
1092         for(j = 0; j < 4; j++ )
1093         {
1094             if( q->neighbors[j] == q0 )
1095             {
1096                 q->neighbors[j] = 0;
1097                 q->count--;
1098                 for(int k = 0; k < 4; k++ )
1099                     if( q0->neighbors[k] == q )
1100                     {
1101                         q0->neighbors[k] = 0;
1102                         q0->count--;
1103                         break;
1104                     }
1105                 break;
1106             }
1107         }
1108     }
1109
1110     // remove the quad
1111     for(i = 0; i < count; i++ )
1112     {
1113         CvCBQuad *q = quads[i];
1114         if (q == q0)
1115         {
1116             quads[i] = quads[count-1];
1117             break;
1118         }
1119     }
1120 }
1121
1122 //
1123 // put quad into correct order, where <corner> has value <common>
1124 //
1125
1126 static void
1127 icvOrderQuad(CvCBQuad *quad, CvCBCorner *corner, int common)
1128 {
1129     // find the corner
1130     int tc;
1131     for (tc=0; tc<4; tc++)
1132         if (quad->corners[tc]->pt.x == corner->pt.x &&
1133             quad->corners[tc]->pt.y == corner->pt.y)
1134             break;
1135
1136     // set corner order
1137     // shift
1138     while (tc != common)
1139     {
1140         // shift by one
1141         CvCBCorner *tempc;
1142         CvCBQuad *tempq;
1143         tempc = quad->corners[3];
1144         tempq = quad->neighbors[3];
1145         for (int i=3; i>0; i--)
1146         {
1147             quad->corners[i] = quad->corners[i-1];
1148             quad->neighbors[i] = quad->neighbors[i-1];
1149         }
1150         quad->corners[0] = tempc;
1151         quad->neighbors[0] = tempq;
1152         tc++;
1153         tc = tc%4;
1154     }
1155 }
1156
1157
1158 // if we found too many connect quads, remove those which probably do not belong.
1159 static int
1160 icvCleanFoundConnectedQuads( int quad_count, CvCBQuad **quad_group, CvSize pattern_size )
1161 {
1162     CvPoint2D32f center;
1163     int i, j, k;
1164     // number of quads this pattern should contain
1165     int count = ((pattern_size.width + 1)*(pattern_size.height + 1) + 1)/2;
1166
1167     // if we have more quadrangles than we should,
1168     // try to eliminate duplicates or ones which don't belong to the pattern rectangle...
1169     if( quad_count <= count )
1170         return quad_count;
1171
1172     // create an array of quadrangle centers
1173     cv::AutoBuffer<CvPoint2D32f> centers( quad_count );
1174     cv::Ptr<CvMemStorage> temp_storage(cvCreateMemStorage(0));
1175
1176     for( i = 0; i < quad_count; i++ )
1177     {
1178         CvPoint2D32f ci;
1179         CvCBQuad* q = quad_group[i];
1180
1181         for( j = 0; j < 4; j++ )
1182         {
1183             CvPoint2D32f pt = q->corners[j]->pt;
1184             ci.x += pt.x;
1185             ci.y += pt.y;
1186         }
1187
1188         ci.x *= 0.25f;
1189         ci.y *= 0.25f;
1190
1191         centers[i] = ci;
1192         center.x += ci.x;
1193         center.y += ci.y;
1194     }
1195     center.x /= quad_count;
1196     center.y /= quad_count;
1197
1198     // If we still have more quadrangles than we should,
1199     // we try to eliminate bad ones based on minimizing the bounding box.
1200     // We iteratively remove the point which reduces the size of
1201     // the bounding box of the blobs the most
1202     // (since we want the rectangle to be as small as possible)
1203     // remove the quadrange that causes the biggest reduction
1204     // in pattern size until we have the correct number
1205     for( ; quad_count > count; quad_count-- )
1206     {
1207         double min_box_area = DBL_MAX;
1208         int skip, min_box_area_index = -1;
1209
1210         // For each point, calculate box area without that point
1211         for( skip = 0; skip < quad_count; skip++ )
1212         {
1213             // get bounding rectangle
1214             CvPoint2D32f temp = centers[skip]; // temporarily make index 'skip' the same as
1215             centers[skip] = center;            // pattern center (so it is not counted for convex hull)
1216             CvMat pointMat = cvMat(1, quad_count, CV_32FC2, centers);
1217             CvSeq *hull = cvConvexHull2( &pointMat, temp_storage, CV_CLOCKWISE, 1 );
1218             centers[skip] = temp;
1219             double hull_area = fabs(cvContourArea(hull, CV_WHOLE_SEQ));
1220
1221             // remember smallest box area
1222             if( hull_area < min_box_area )
1223             {
1224                 min_box_area = hull_area;
1225                 min_box_area_index = skip;
1226             }
1227             cvClearMemStorage( temp_storage );
1228         }
1229
1230         CvCBQuad *q0 = quad_group[min_box_area_index];
1231
1232         // remove any references to this quad as a neighbor
1233         for( i = 0; i < quad_count; i++ )
1234         {
1235             CvCBQuad *q = quad_group[i];
1236             for( j = 0; j < 4; j++ )
1237             {
1238                 if( q->neighbors[j] == q0 )
1239                 {
1240                     q->neighbors[j] = 0;
1241                     q->count--;
1242                     for( k = 0; k < 4; k++ )
1243                         if( q0->neighbors[k] == q )
1244                         {
1245                             q0->neighbors[k] = 0;
1246                             q0->count--;
1247                             break;
1248                         }
1249                     break;
1250                 }
1251             }
1252         }
1253
1254         // remove the quad
1255         quad_count--;
1256         quad_group[min_box_area_index] = quad_group[quad_count];
1257         centers[min_box_area_index] = centers[quad_count];
1258     }
1259
1260     return quad_count;
1261 }
1262
1263 //=====================================================================================
1264
1265 static int
1266 icvFindConnectedQuads( CvCBQuad *quad, int quad_count, CvCBQuad **out_group,
1267                        int group_idx, CvMemStorage* storage )
1268 {
1269     cv::Ptr<CvMemStorage> temp_storage(cvCreateChildMemStorage( storage ));
1270     CvSeq* stack = cvCreateSeq( 0, sizeof(*stack), sizeof(void*), temp_storage );
1271     int i, count = 0;
1272
1273     // Scan the array for a first unlabeled quad
1274     for( i = 0; i < quad_count; i++ )
1275     {
1276         if( quad[i].count > 0 && quad[i].group_idx < 0)
1277             break;
1278     }
1279
1280     // Recursively find a group of connected quads starting from the seed quad[i]
1281     if( i < quad_count )
1282     {
1283         CvCBQuad* q = &quad[i];
1284         cvSeqPush( stack, &q );
1285         out_group[count++] = q;
1286         q->group_idx = group_idx;
1287         q->ordered = false;
1288
1289         while( stack->total )
1290         {
1291             cvSeqPop( stack, &q );
1292             for( i = 0; i < 4; i++ )
1293             {
1294                 CvCBQuad *neighbor = q->neighbors[i];
1295                 if( neighbor && neighbor->count > 0 && neighbor->group_idx < 0 )
1296                 {
1297                     cvSeqPush( stack, &neighbor );
1298                     out_group[count++] = neighbor;
1299                     neighbor->group_idx = group_idx;
1300                     neighbor->ordered = false;
1301                 }
1302             }
1303         }
1304     }
1305
1306     return count;
1307 }
1308
1309
1310 //=====================================================================================
1311
1312 static int
1313 icvCheckQuadGroup( CvCBQuad **quad_group, int quad_count,
1314                    CvCBCorner **out_corners, CvSize pattern_size )
1315 {
1316     const int ROW1 = 1000000;
1317     const int ROW2 = 2000000;
1318     const int ROW_ = 3000000;
1319     int result = 0;
1320     int i, out_corner_count = 0, corner_count = 0;
1321     std::vector<CvCBCorner*> corners(quad_count*4);
1322
1323     int j, k, kk;
1324     int width = 0, height = 0;
1325     int hist[5] = {0,0,0,0,0};
1326     CvCBCorner* first = 0, *first2 = 0, *right, *cur, *below, *c;
1327
1328     // build dual graph, which vertices are internal quad corners
1329     // and two vertices are connected iff they lie on the same quad edge
1330     for( i = 0; i < quad_count; i++ )
1331     {
1332         CvCBQuad* q = quad_group[i];
1333         /*CvScalar color = q->count == 0 ? cvScalar(0,255,255) :
1334                          q->count == 1 ? cvScalar(0,0,255) :
1335                          q->count == 2 ? cvScalar(0,255,0) :
1336                          q->count == 3 ? cvScalar(255,255,0) :
1337                                          cvScalar(255,0,0);*/
1338
1339         for( j = 0; j < 4; j++ )
1340         {
1341             //cvLine( debug_img, cvPointFrom32f(q->corners[j]->pt), cvPointFrom32f(q->corners[(j+1)&3]->pt), color, 1, CV_AA, 0 );
1342             if( q->neighbors[j] )
1343             {
1344                 CvCBCorner *a = q->corners[j], *b = q->corners[(j+1)&3];
1345                 // mark internal corners that belong to:
1346                 //   - a quad with a single neighbor - with ROW1,
1347                 //   - a quad with two neighbors     - with ROW2
1348                 // make the rest of internal corners with ROW_
1349                 int row_flag = q->count == 1 ? ROW1 : q->count == 2 ? ROW2 : ROW_;
1350
1351                 if( a->row == 0 )
1352                 {
1353                     corners[corner_count++] = a;
1354                     a->row = row_flag;
1355                 }
1356                 else if( a->row > row_flag )
1357                     a->row = row_flag;
1358
1359                 if( q->neighbors[(j+1)&3] )
1360                 {
1361                     if( a->count >= 4 || b->count >= 4 )
1362                         goto finalize;
1363                     for( k = 0; k < 4; k++ )
1364                     {
1365                         if( a->neighbors[k] == b )
1366                             goto finalize;
1367                         if( b->neighbors[k] == a )
1368                             goto finalize;
1369                     }
1370                     a->neighbors[a->count++] = b;
1371                     b->neighbors[b->count++] = a;
1372                 }
1373             }
1374         }
1375     }
1376
1377     if( corner_count != pattern_size.width*pattern_size.height )
1378         goto finalize;
1379
1380     for( i = 0; i < corner_count; i++ )
1381     {
1382         int n = corners[i]->count;
1383         assert( 0 <= n && n <= 4 );
1384         hist[n]++;
1385         if( !first && n == 2 )
1386         {
1387             if( corners[i]->row == ROW1 )
1388                 first = corners[i];
1389             else if( !first2 && corners[i]->row == ROW2 )
1390                 first2 = corners[i];
1391         }
1392     }
1393
1394     // start with a corner that belongs to a quad with a single neighbor.
1395     // if we do not have such, start with a corner of a quad with two neighbors.
1396     if( !first )
1397         first = first2;
1398
1399     if( !first || hist[0] != 0 || hist[1] != 0 || hist[2] != 4 ||
1400         hist[3] != (pattern_size.width + pattern_size.height)*2 - 8 )
1401         goto finalize;
1402
1403     cur = first;
1404     right = below = 0;
1405     out_corners[out_corner_count++] = cur;
1406
1407     for( k = 0; k < 4; k++ )
1408     {
1409         c = cur->neighbors[k];
1410         if( c )
1411         {
1412             if( !right )
1413                 right = c;
1414             else if( !below )
1415                 below = c;
1416         }
1417     }
1418
1419     if( !right || (right->count != 2 && right->count != 3) ||
1420         !below || (below->count != 2 && below->count != 3) )
1421         goto finalize;
1422
1423     cur->row = 0;
1424     //cvCircle( debug_img, cvPointFrom32f(cur->pt), 3, cvScalar(0,255,0), -1, 8, 0 );
1425
1426     first = below; // remember the first corner in the next row
1427     // find and store the first row (or column)
1428     for(j=1;;j++)
1429     {
1430         right->row = 0;
1431         out_corners[out_corner_count++] = right;
1432         //cvCircle( debug_img, cvPointFrom32f(right->pt), 3, cvScalar(0,255-j*10,0), -1, 8, 0 );
1433         if( right->count == 2 )
1434             break;
1435         if( right->count != 3 || out_corner_count >= MAX(pattern_size.width,pattern_size.height) )
1436             goto finalize;
1437         cur = right;
1438         for( k = 0; k < 4; k++ )
1439         {
1440             c = cur->neighbors[k];
1441             if( c && c->row > 0 )
1442             {
1443                 for( kk = 0; kk < 4; kk++ )
1444                 {
1445                     if( c->neighbors[kk] == below )
1446                         break;
1447                 }
1448                 if( kk < 4 )
1449                     below = c;
1450                 else
1451                     right = c;
1452             }
1453         }
1454     }
1455
1456     width = out_corner_count;
1457     if( width == pattern_size.width )
1458         height = pattern_size.height;
1459     else if( width == pattern_size.height )
1460         height = pattern_size.width;
1461     else
1462         goto finalize;
1463
1464     // find and store all the other rows
1465     for( i = 1; ; i++ )
1466     {
1467         if( !first )
1468             break;
1469         cur = first;
1470         first = 0;
1471         for( j = 0;; j++ )
1472         {
1473             cur->row = i;
1474             out_corners[out_corner_count++] = cur;
1475             //cvCircle( debug_img, cvPointFrom32f(cur->pt), 3, cvScalar(0,0,255-j*10), -1, 8, 0 );
1476             if( cur->count == 2 + (i < height-1) && j > 0 )
1477                 break;
1478
1479             right = 0;
1480
1481             // find a neighbor that has not been processed yet
1482             // and that has a neighbor from the previous row
1483             for( k = 0; k < 4; k++ )
1484             {
1485                 c = cur->neighbors[k];
1486                 if( c && c->row > i )
1487                 {
1488                     for( kk = 0; kk < 4; kk++ )
1489                     {
1490                         if( c->neighbors[kk] && c->neighbors[kk]->row == i-1 )
1491                             break;
1492                     }
1493                     if( kk < 4 )
1494                     {
1495                         right = c;
1496                         if( j > 0 )
1497                             break;
1498                     }
1499                     else if( j == 0 )
1500                         first = c;
1501                 }
1502             }
1503             if( !right )
1504                 goto finalize;
1505             cur = right;
1506         }
1507
1508         if( j != width - 1 )
1509             goto finalize;
1510     }
1511
1512     if( out_corner_count != corner_count )
1513         goto finalize;
1514
1515     // check if we need to transpose the board
1516     if( width != pattern_size.width )
1517     {
1518         CV_SWAP( width, height, k );
1519
1520         memcpy( &corners[0], out_corners, corner_count*sizeof(corners[0]) );
1521         for( i = 0; i < height; i++ )
1522             for( j = 0; j < width; j++ )
1523                 out_corners[i*width + j] = corners[j*height + i];
1524     }
1525
1526     // check if we need to revert the order in each row
1527     {
1528         CvPoint2D32f p0 = out_corners[0]->pt, p1 = out_corners[pattern_size.width-1]->pt,
1529                      p2 = out_corners[pattern_size.width]->pt;
1530         if( (p1.x - p0.x)*(p2.y - p1.y) - (p1.y - p0.y)*(p2.x - p1.x) < 0 )
1531         {
1532             if( width % 2 == 0 )
1533             {
1534                 for( i = 0; i < height; i++ )
1535                     for( j = 0; j < width/2; j++ )
1536                         CV_SWAP( out_corners[i*width+j], out_corners[i*width+width-j-1], c );
1537             }
1538             else
1539             {
1540                 for( j = 0; j < width; j++ )
1541                     for( i = 0; i < height/2; i++ )
1542                         CV_SWAP( out_corners[i*width+j], out_corners[(height - i - 1)*width+j], c );
1543             }
1544         }
1545     }
1546
1547     result = corner_count;
1548
1549 finalize:
1550
1551     if( result <= 0 )
1552     {
1553         corner_count = MIN( corner_count, pattern_size.width*pattern_size.height );
1554         for( i = 0; i < corner_count; i++ )
1555             out_corners[i] = corners[i];
1556         result = -corner_count;
1557
1558         if (result == -pattern_size.width*pattern_size.height)
1559             result = -result;
1560     }
1561
1562     return result;
1563 }
1564
1565
1566
1567
1568 //=====================================================================================
1569
1570 static void icvFindQuadNeighbors( CvCBQuad *quads, int quad_count )
1571 {
1572     const float thresh_scale = 1.f;
1573     int idx, i, k, j;
1574     float dx, dy, dist;
1575
1576     // find quad neighbors
1577     for( idx = 0; idx < quad_count; idx++ )
1578     {
1579         CvCBQuad* cur_quad = &quads[idx];
1580
1581         // choose the points of the current quadrangle that are close to
1582         // some points of the other quadrangles
1583         // (it can happen for split corners (due to dilation) of the
1584         // checker board). Search only in other quadrangles!
1585
1586         // for each corner of this quadrangle
1587         for( i = 0; i < 4; i++ )
1588         {
1589             CvPoint2D32f pt;
1590             float min_dist = FLT_MAX;
1591             int closest_corner_idx = -1;
1592             CvCBQuad *closest_quad = 0;
1593             CvCBCorner *closest_corner = 0;
1594
1595             if( cur_quad->neighbors[i] )
1596                 continue;
1597
1598             pt = cur_quad->corners[i]->pt;
1599
1600             // find the closest corner in all other quadrangles
1601             for( k = 0; k < quad_count; k++ )
1602             {
1603                 if( k == idx )
1604                     continue;
1605
1606                 for( j = 0; j < 4; j++ )
1607                 {
1608                     if( quads[k].neighbors[j] )
1609                         continue;
1610
1611                     dx = pt.x - quads[k].corners[j]->pt.x;
1612                     dy = pt.y - quads[k].corners[j]->pt.y;
1613                     dist = dx * dx + dy * dy;
1614
1615                     if( dist < min_dist &&
1616                         dist <= cur_quad->edge_len*thresh_scale &&
1617                         dist <= quads[k].edge_len*thresh_scale )
1618                     {
1619                         // check edge lengths, make sure they're compatible
1620                         // edges that are different by more than 1:4 are rejected
1621                         float ediff = cur_quad->edge_len - quads[k].edge_len;
1622                         if (ediff > 32*cur_quad->edge_len ||
1623                             ediff > 32*quads[k].edge_len)
1624                         {
1625                             PRINTF("Incompatible edge lengths\n");
1626                             continue;
1627                         }
1628                         closest_corner_idx = j;
1629                         closest_quad = &quads[k];
1630                         min_dist = dist;
1631                     }
1632                 }
1633             }
1634
1635             // we found a matching corner point?
1636             if( closest_corner_idx >= 0 && min_dist < FLT_MAX )
1637             {
1638                 // If another point from our current quad is closer to the found corner
1639                 // than the current one, then we don't count this one after all.
1640                 // This is necessary to support small squares where otherwise the wrong
1641                 // corner will get matched to closest_quad;
1642                 closest_corner = closest_quad->corners[closest_corner_idx];
1643
1644                 for( j = 0; j < 4; j++ )
1645                 {
1646                     if( cur_quad->neighbors[j] == closest_quad )
1647                         break;
1648
1649                     dx = closest_corner->pt.x - cur_quad->corners[j]->pt.x;
1650                     dy = closest_corner->pt.y - cur_quad->corners[j]->pt.y;
1651
1652                     if( dx * dx + dy * dy < min_dist )
1653                         break;
1654                 }
1655
1656                 if( j < 4 || cur_quad->count >= 4 || closest_quad->count >= 4 )
1657                     continue;
1658
1659                 // Check that each corner is a neighbor of different quads
1660                 for( j = 0; j < closest_quad->count; j++ )
1661                 {
1662                     if( closest_quad->neighbors[j] == cur_quad )
1663                         break;
1664                 }
1665                 if( j < closest_quad->count )
1666                     continue;
1667
1668                 // check whether the closest corner to closest_corner
1669                 // is different from cur_quad->corners[i]->pt
1670                 for( k = 0; k < quad_count; k++ )
1671                 {
1672                     CvCBQuad* q = &quads[k];
1673                     if( k == idx || q == closest_quad )
1674                         continue;
1675
1676                     for( j = 0; j < 4; j++ )
1677                         if( !q->neighbors[j] )
1678                         {
1679                             dx = closest_corner->pt.x - q->corners[j]->pt.x;
1680                             dy = closest_corner->pt.y - q->corners[j]->pt.y;
1681                             dist = dx*dx + dy*dy;
1682                             if( dist < min_dist )
1683                                 break;
1684                         }
1685                     if( j < 4 )
1686                         break;
1687                 }
1688
1689                 if( k < quad_count )
1690                     continue;
1691
1692                 closest_corner->pt.x = (pt.x + closest_corner->pt.x) * 0.5f;
1693                 closest_corner->pt.y = (pt.y + closest_corner->pt.y) * 0.5f;
1694
1695                 // We've found one more corner - remember it
1696                 cur_quad->count++;
1697                 cur_quad->neighbors[i] = closest_quad;
1698                 cur_quad->corners[i] = closest_corner;
1699
1700                 closest_quad->count++;
1701                 closest_quad->neighbors[closest_corner_idx] = cur_quad;
1702             }
1703         }
1704     }
1705 }
1706
1707 //=====================================================================================
1708
1709 // returns corners in clockwise order
1710 // corners don't necessarily start at same position on quad (e.g.,
1711 //   top left corner)
1712
1713 static int
1714 icvGenerateQuads( CvCBQuad **out_quads, CvCBCorner **out_corners,
1715                   CvMemStorage *storage, const cv::Mat & image_, int flags, int *max_quad_buf_size )
1716 {
1717     CvMat image_old(image_), *image = &image_old;
1718     int quad_count = 0;
1719     cv::Ptr<CvMemStorage> temp_storage;
1720
1721     if( out_quads )
1722         *out_quads = 0;
1723
1724     if( out_corners )
1725         *out_corners = 0;
1726
1727     CvSeq *src_contour = 0;
1728     CvSeq *root;
1729     CvContourEx* board = 0;
1730     CvContourScanner scanner;
1731     int i, idx, min_size;
1732
1733     CV_Assert( out_corners && out_quads );
1734
1735     // empiric bound for minimal allowed perimeter for squares
1736     min_size = 25; //cvRound( image->cols * image->rows * .03 * 0.01 * 0.92 );
1737
1738     // create temporary storage for contours and the sequence of pointers to found quadrangles
1739     temp_storage.reset(cvCreateChildMemStorage( storage ));
1740     root = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSeq*), temp_storage );
1741
1742     // initialize contour retrieving routine
1743     scanner = cvStartFindContours( image, temp_storage, sizeof(CvContourEx),
1744                                    CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
1745
1746     // get all the contours one by one
1747     while( (src_contour = cvFindNextContour( scanner )) != 0 )
1748     {
1749         CvSeq *dst_contour = 0;
1750         CvRect rect = ((CvContour*)src_contour)->rect;
1751
1752         // reject contours with too small perimeter
1753         if( CV_IS_SEQ_HOLE(src_contour) && rect.width*rect.height >= min_size )
1754         {
1755             const int min_approx_level = 1, max_approx_level = MAX_CONTOUR_APPROX;
1756             int approx_level;
1757             for( approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++ )
1758             {
1759                 dst_contour = cvApproxPoly( src_contour, sizeof(CvContour), temp_storage,
1760                                             CV_POLY_APPROX_DP, (float)approx_level );
1761                 if( dst_contour->total == 4 )
1762                     break;
1763
1764                 // we call this again on its own output, because sometimes
1765                 // cvApproxPoly() does not simplify as much as it should.
1766                 dst_contour = cvApproxPoly( dst_contour, sizeof(CvContour), temp_storage,
1767                                             CV_POLY_APPROX_DP, (float)approx_level );
1768
1769                 if( dst_contour->total == 4 )
1770                     break;
1771             }
1772
1773             // reject non-quadrangles
1774             if( dst_contour->total == 4 && cvCheckContourConvexity(dst_contour) )
1775             {
1776                 CvPoint pt[4];
1777                 double d1, d2, p = cvContourPerimeter(dst_contour);
1778                 double area = fabs(cvContourArea(dst_contour, CV_WHOLE_SEQ));
1779                 double dx, dy;
1780
1781                 for( i = 0; i < 4; i++ )
1782                     pt[i] = *(CvPoint*)cvGetSeqElem(dst_contour, i);
1783
1784                 dx = pt[0].x - pt[2].x;
1785                 dy = pt[0].y - pt[2].y;
1786                 d1 = sqrt(dx*dx + dy*dy);
1787
1788                 dx = pt[1].x - pt[3].x;
1789                 dy = pt[1].y - pt[3].y;
1790                 d2 = sqrt(dx*dx + dy*dy);
1791
1792                 // philipg.  Only accept those quadrangles which are more square
1793                 // than rectangular and which are big enough
1794                 double d3, d4;
1795                 dx = pt[0].x - pt[1].x;
1796                 dy = pt[0].y - pt[1].y;
1797                 d3 = sqrt(dx*dx + dy*dy);
1798                 dx = pt[1].x - pt[2].x;
1799                 dy = pt[1].y - pt[2].y;
1800                 d4 = sqrt(dx*dx + dy*dy);
1801                 if( !(flags & CV_CALIB_CB_FILTER_QUADS) ||
1802                     (d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > min_size &&
1803                     d1 >= 0.15 * p && d2 >= 0.15 * p) )
1804                 {
1805                     CvContourEx* parent = (CvContourEx*)(src_contour->v_prev);
1806                     parent->counter++;
1807                     if( !board || board->counter < parent->counter )
1808                         board = parent;
1809                     dst_contour->v_prev = (CvSeq*)parent;
1810                     //for( i = 0; i < 4; i++ ) cvLine( debug_img, pt[i], pt[(i+1)&3], cvScalar(200,255,255), 1, CV_AA, 0 );
1811                     cvSeqPush( root, &dst_contour );
1812                 }
1813             }
1814         }
1815     }
1816
1817     // finish contour retrieving
1818     cvEndFindContours( &scanner );
1819
1820     // allocate quad & corner buffers
1821     *max_quad_buf_size = MAX(1, (root->total+root->total / 2)) * 2;
1822     *out_quads = (CvCBQuad*)cvAlloc(*max_quad_buf_size * sizeof((*out_quads)[0]));
1823     *out_corners = (CvCBCorner*)cvAlloc(*max_quad_buf_size * 4 * sizeof((*out_corners)[0]));
1824
1825     // Create array of quads structures
1826     for( idx = 0; idx < root->total; idx++ )
1827     {
1828         CvCBQuad* q = &(*out_quads)[quad_count];
1829         src_contour = *(CvSeq**)cvGetSeqElem( root, idx );
1830         if( (flags & CV_CALIB_CB_FILTER_QUADS) && src_contour->v_prev != (CvSeq*)board )
1831             continue;
1832
1833         // reset group ID
1834         memset( q, 0, sizeof(*q) );
1835         q->group_idx = -1;
1836         assert( src_contour->total == 4 );
1837         for( i = 0; i < 4; i++ )
1838         {
1839             CvPoint * onePoint = (CvPoint*)cvGetSeqElem(src_contour, i);
1840             CV_Assert(onePoint != NULL);
1841             CvPoint2D32f pt = cvPointTo32f(*onePoint);
1842             CvCBCorner* corner = &(*out_corners)[quad_count*4 + i];
1843
1844             memset( corner, 0, sizeof(*corner) );
1845             corner->pt = pt;
1846             q->corners[i] = corner;
1847         }
1848         q->edge_len = FLT_MAX;
1849         for( i = 0; i < 4; i++ )
1850         {
1851             float dx = q->corners[i]->pt.x - q->corners[(i+1)&3]->pt.x;
1852             float dy = q->corners[i]->pt.y - q->corners[(i+1)&3]->pt.y;
1853             float d = dx*dx + dy*dy;
1854             if( q->edge_len > d )
1855                 q->edge_len = d;
1856         }
1857         quad_count++;
1858     }
1859
1860     return quad_count;
1861 }
1862
1863 static bool processQuads(CvCBQuad *quads, int quad_count, CvSize pattern_size, int max_quad_buf_size,
1864                          CvMemStorage * storage, CvCBCorner *corners, CvPoint2D32f *out_corners, int *out_corner_count, int & prev_sqr_size)
1865 {
1866     if( quad_count <= 0 )
1867         return false;
1868
1869     bool found = false;
1870
1871     // Find quad's neighbors
1872     icvFindQuadNeighbors( quads, quad_count );
1873
1874     // allocate extra for adding in icvOrderFoundQuads
1875     CvCBQuad **quad_group = 0;
1876     CvCBCorner **corner_group = 0;
1877
1878     quad_group = (CvCBQuad**)cvAlloc( sizeof(quad_group[0]) * max_quad_buf_size);
1879     corner_group = (CvCBCorner**)cvAlloc( sizeof(corner_group[0]) * max_quad_buf_size * 4 );
1880
1881     for( int group_idx = 0; ; group_idx++ )
1882     {
1883         int count = icvFindConnectedQuads( quads, quad_count, quad_group, group_idx, storage );
1884
1885         if( count == 0 )
1886             break;
1887
1888         // order the quad corners globally
1889         // maybe delete or add some
1890         PRINTF("Starting ordering of inner quads (%d)\n", count);
1891         count = icvOrderFoundConnectedQuads(count, quad_group, &quad_count, &quads, &corners,
1892                                             pattern_size, max_quad_buf_size, storage );
1893         PRINTF("Finished ordering of inner quads (%d)\n", count);
1894
1895         if (count == 0)
1896             continue;       // haven't found inner quads
1897
1898         // If count is more than it should be, this will remove those quads
1899         // which cause maximum deviation from a nice square pattern.
1900         count = icvCleanFoundConnectedQuads( count, quad_group, pattern_size );
1901         PRINTF("Connected group: %d, count: %d\n", group_idx, count);
1902
1903         count = icvCheckQuadGroup( quad_group, count, corner_group, pattern_size );
1904         PRINTF("Connected group: %d, count: %d\n", group_idx, count);
1905
1906         int n = count > 0 ? pattern_size.width * pattern_size.height : -count;
1907         n = MIN( n, pattern_size.width * pattern_size.height );
1908         float sum_dist = 0;
1909         int total = 0;
1910
1911         for(int i = 0; i < n; i++ )
1912         {
1913             int ni = 0;
1914             float avgi = corner_group[i]->meanDist(&ni);
1915             sum_dist += avgi*ni;
1916             total += ni;
1917         }
1918         prev_sqr_size = cvRound(sum_dist/MAX(total, 1));
1919
1920         if( count > 0 || (out_corner_count && -count > *out_corner_count) )
1921         {
1922             // copy corners to output array
1923             for(int i = 0; i < n; i++ )
1924                 out_corners[i] = corner_group[i]->pt;
1925
1926             if( out_corner_count )
1927                 *out_corner_count = n;
1928
1929             if( count == pattern_size.width*pattern_size.height
1930                     && icvCheckBoardMonotony( out_corners, pattern_size ))
1931             {
1932                 found = true;
1933                 break;
1934             }
1935         }
1936     }
1937
1938     cvFree(&quad_group);
1939     cvFree(&corner_group);
1940
1941     return found;
1942 }
1943
1944 //==================================================================================================
1945
1946 CV_IMPL void
1947 cvDrawChessboardCorners( CvArr* _image, CvSize pattern_size,
1948                          CvPoint2D32f* corners, int count, int found )
1949 {
1950     const int shift = 0;
1951     const int radius = 4;
1952     const int r = radius*(1 << shift);
1953     int i;
1954     CvMat stub, *image;
1955     double scale = 1;
1956     int type, cn, line_type;
1957
1958     image = cvGetMat( _image, &stub );
1959
1960     type = CV_MAT_TYPE(image->type);
1961     cn = CV_MAT_CN(type);
1962     if( cn != 1 && cn != 3 && cn != 4 )
1963         CV_Error( CV_StsUnsupportedFormat, "Number of channels must be 1, 3 or 4" );
1964
1965     switch( CV_MAT_DEPTH(image->type) )
1966     {
1967     case CV_8U:
1968         scale = 1;
1969         break;
1970     case CV_16U:
1971         scale = 256;
1972         break;
1973     case CV_32F:
1974         scale = 1./255;
1975         break;
1976     default:
1977         CV_Error( CV_StsUnsupportedFormat,
1978             "Only 8-bit, 16-bit or floating-point 32-bit images are supported" );
1979     }
1980
1981     line_type = type == CV_8UC1 || type == CV_8UC3 ? CV_AA : 8;
1982
1983     if( !found )
1984     {
1985         CvScalar color(0,0,255,0);
1986         if( cn == 1 )
1987             color = cvScalarAll(200);
1988         color.val[0] *= scale;
1989         color.val[1] *= scale;
1990         color.val[2] *= scale;
1991         color.val[3] *= scale;
1992
1993         for( i = 0; i < count; i++ )
1994         {
1995             CvPoint pt;
1996             pt.x = cvRound(corners[i].x*(1 << shift));
1997             pt.y = cvRound(corners[i].y*(1 << shift));
1998             cvLine( image, cvPoint( pt.x - r, pt.y - r ),
1999                     cvPoint( pt.x + r, pt.y + r ), color, 1, line_type, shift );
2000             cvLine( image, cvPoint( pt.x - r, pt.y + r),
2001                     cvPoint( pt.x + r, pt.y - r), color, 1, line_type, shift );
2002             cvCircle( image, pt, r+(1<<shift), color, 1, line_type, shift );
2003         }
2004     }
2005     else
2006     {
2007         int x, y;
2008         CvPoint prev_pt;
2009         const int line_max = 7;
2010         static const CvScalar line_colors[line_max] =
2011         {
2012             CvScalar(0,0,255),
2013             CvScalar(0,128,255),
2014             CvScalar(0,200,200),
2015             CvScalar(0,255,0),
2016             CvScalar(200,200,0),
2017             CvScalar(255,0,0),
2018             CvScalar(255,0,255)
2019         };
2020
2021         for( y = 0, i = 0; y < pattern_size.height; y++ )
2022         {
2023             CvScalar color = line_colors[y % line_max];
2024             if( cn == 1 )
2025                 color = cvScalarAll(200);
2026             color.val[0] *= scale;
2027             color.val[1] *= scale;
2028             color.val[2] *= scale;
2029             color.val[3] *= scale;
2030
2031             for( x = 0; x < pattern_size.width; x++, i++ )
2032             {
2033                 CvPoint pt;
2034                 pt.x = cvRound(corners[i].x*(1 << shift));
2035                 pt.y = cvRound(corners[i].y*(1 << shift));
2036
2037                 if( i != 0 )
2038                     cvLine( image, prev_pt, pt, color, 1, line_type, shift );
2039
2040                 cvLine( image, cvPoint(pt.x - r, pt.y - r),
2041                         cvPoint(pt.x + r, pt.y + r), color, 1, line_type, shift );
2042                 cvLine( image, cvPoint(pt.x - r, pt.y + r),
2043                         cvPoint(pt.x + r, pt.y - r), color, 1, line_type, shift );
2044                 cvCircle( image, pt, r+(1<<shift), color, 1, line_type, shift );
2045                 prev_pt = pt;
2046             }
2047         }
2048     }
2049 }
2050
2051 bool cv::findChessboardCorners( InputArray _image, Size patternSize,
2052                             OutputArray corners, int flags )
2053 {
2054     CV_INSTRUMENT_REGION()
2055
2056     int count = patternSize.area()*2;
2057     std::vector<Point2f> tmpcorners(count+1);
2058     Mat image = _image.getMat(); CvMat c_image = image;
2059     bool ok = cvFindChessboardCorners(&c_image, patternSize,
2060         (CvPoint2D32f*)&tmpcorners[0], &count, flags ) > 0;
2061     if( count > 0 )
2062     {
2063         tmpcorners.resize(count);
2064         Mat(tmpcorners).copyTo(corners);
2065     }
2066     else
2067         corners.release();
2068     return ok;
2069 }
2070
2071 namespace
2072 {
2073 int quiet_error(int /*status*/, const char* /*func_name*/,
2074                                        const char* /*err_msg*/, const char* /*file_name*/,
2075                                        int /*line*/, void* /*userdata*/ )
2076 {
2077   return 0;
2078 }
2079 }
2080
2081 void cv::drawChessboardCorners( InputOutputArray _image, Size patternSize,
2082                             InputArray _corners,
2083                             bool patternWasFound )
2084 {
2085     CV_INSTRUMENT_REGION()
2086
2087     Mat corners = _corners.getMat();
2088     if( corners.empty() )
2089         return;
2090     Mat image = _image.getMat(); CvMat c_image = image;
2091     int nelems = corners.checkVector(2, CV_32F, true);
2092     CV_Assert(nelems >= 0);
2093     cvDrawChessboardCorners( &c_image, patternSize, corners.ptr<CvPoint2D32f>(),
2094                              nelems, patternWasFound );
2095 }
2096
2097 bool cv::findCirclesGrid( InputArray _image, Size patternSize,
2098                           OutputArray _centers, int flags, const Ptr<FeatureDetector> &blobDetector,
2099                           const CirclesGridFinderParameters& parameters_)
2100 {
2101     CV_INSTRUMENT_REGION()
2102
2103     CirclesGridFinderParameters parameters = parameters_; // parameters.gridType is amended below
2104
2105     bool isAsymmetricGrid = (flags & CALIB_CB_ASYMMETRIC_GRID) ? true : false;
2106     bool isSymmetricGrid  = (flags & CALIB_CB_SYMMETRIC_GRID ) ? true : false;
2107     CV_Assert(isAsymmetricGrid ^ isSymmetricGrid);
2108
2109     Mat image = _image.getMat();
2110     std::vector<Point2f> centers;
2111
2112     std::vector<KeyPoint> keypoints;
2113     blobDetector->detect(image, keypoints);
2114     std::vector<Point2f> points;
2115     for (size_t i = 0; i < keypoints.size(); i++)
2116     {
2117       points.push_back (keypoints[i].pt);
2118     }
2119
2120     if(flags & CALIB_CB_ASYMMETRIC_GRID)
2121       parameters.gridType = CirclesGridFinderParameters::ASYMMETRIC_GRID;
2122     if(flags & CALIB_CB_SYMMETRIC_GRID)
2123       parameters.gridType = CirclesGridFinderParameters::SYMMETRIC_GRID;
2124
2125     if(flags & CALIB_CB_CLUSTERING)
2126     {
2127       CirclesGridClusterFinder circlesGridClusterFinder(parameters);
2128       circlesGridClusterFinder.findGrid(points, patternSize, centers);
2129       Mat(centers).copyTo(_centers);
2130       return !centers.empty();
2131     }
2132
2133     const int attempts = 2;
2134     const size_t minHomographyPoints = 4;
2135     Mat H;
2136     for (int i = 0; i < attempts; i++)
2137     {
2138       centers.clear();
2139       CirclesGridFinder boxFinder(patternSize, points, parameters);
2140       bool isFound = false;
2141 #define BE_QUIET 1
2142 #if BE_QUIET
2143       void* oldCbkData;
2144       ErrorCallback oldCbk = redirectError(quiet_error, 0, &oldCbkData);
2145 #endif
2146       CV_TRY
2147       {
2148         isFound = boxFinder.findHoles();
2149       }
2150       CV_CATCH(Exception, e)
2151       {
2152           CV_UNUSED(e);
2153       }
2154 #if BE_QUIET
2155       redirectError(oldCbk, oldCbkData);
2156 #endif
2157       if (isFound)
2158       {
2159         switch(parameters.gridType)
2160         {
2161           case CirclesGridFinderParameters::SYMMETRIC_GRID:
2162             boxFinder.getHoles(centers);
2163             break;
2164           case CirclesGridFinderParameters::ASYMMETRIC_GRID:
2165         boxFinder.getAsymmetricHoles(centers);
2166         break;
2167           default:
2168             CV_Error(CV_StsBadArg, "Unknown pattern type");
2169         }
2170
2171         if (i != 0)
2172         {
2173           Mat orgPointsMat;
2174           transform(centers, orgPointsMat, H.inv());
2175           convertPointsFromHomogeneous(orgPointsMat, centers);
2176         }
2177         Mat(centers).copyTo(_centers);
2178         return true;
2179       }
2180
2181       boxFinder.getHoles(centers);
2182       if (i != attempts - 1)
2183       {
2184         if (centers.size() < minHomographyPoints)
2185           break;
2186         H = CirclesGridFinder::rectifyGrid(boxFinder.getDetectedGridSize(), centers, points, points);
2187       }
2188     }
2189     Mat(centers).copyTo(_centers);
2190     return false;
2191 }
2192
2193 bool cv::findCirclesGrid( InputArray _image, Size patternSize,
2194                           OutputArray _centers, int flags, const Ptr<FeatureDetector> &blobDetector)
2195 {
2196     return cv::findCirclesGrid(_image, patternSize, _centers, flags, blobDetector, CirclesGridFinderParameters());
2197 }
2198
2199 /* End of file. */