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         Mat binarized_img = thresh_img_new.clone(); // make clone because cvFindContours modifies the source image
 501         int quad_count = icvGenerateQuads( &quads, &corners, storage, binarized_img, flags, &max_quad_buf_size );
 502         PRINTF("Quad count: %d/%d\n", quad_count, (pattern_size.width/2+1)*(pattern_size.height/2+1));
 503         SHOW_QUADS("New quads", thresh_img_new, quads, quad_count);
 504         if (processQuads(quads, quad_count, pattern_size, max_quad_buf_size, storage, corners, out_corners, out_corner_count, prev_sqr_size))
 505             found = 1;
 506     }
 507
 508     PRINTF("Chessboard detection result 0: %d\n", found);
 509
 510     // revert to old, slower, method if detection failed
 511     if (!found)
 512     {
 513         if( flags & CV_CALIB_CB_NORMALIZE_IMAGE )
 514         {
 515             equalizeHist( img, img );
 516         }
 517
 518         Mat thresh_img;
 519         prev_sqr_size = 0;
 520
 521         PRINTF("Fallback to old algorithm\n");
 522         const bool useAdaptive = flags & CV_CALIB_CB_ADAPTIVE_THRESH;
 523         if (!useAdaptive)
 524         {
 525             // empiric threshold level
 526             // thresholding performed here and not inside the cycle to save processing time
 527             double mean = cv::mean(img).val[0];
 528             int thresh_level = MAX(cvRound( mean - 10 ), 10);
 529             threshold( img, thresh_img, thresh_level, 255, THRESH_BINARY );
 530         }
 531         //if flag CV_CALIB_CB_ADAPTIVE_THRESH is not set it doesn't make sense to iterate over k
 532         int max_k = useAdaptive ? 6 : 1;
 533         for( k = 0; k < max_k; k++ )
 534         {
 535             for( int dilations = min_dilations; dilations <= max_dilations; dilations++ )
 536             {
 537                 if (found)
 538                     break;      // already found it
 539
 540                 // convert the input grayscale image to binary (black-n-white)
 541                 if (useAdaptive)
 542                 {
 543                     int block_size = cvRound(prev_sqr_size == 0
 544                                              ? MIN(img.cols, img.rows) * (k % 2 == 0 ? 0.2 : 0.1)
 545                                              : prev_sqr_size * 2);
 546                     block_size = block_size | 1;
 547                     // convert to binary
 548                     adaptiveThreshold( img, thresh_img, 255, ADAPTIVE_THRESH_MEAN_C, THRESH_BINARY, block_size, (k/2)*5 );
 549                     if (dilations > 0)
 550                         dilate( thresh_img, thresh_img, Mat(), Point(-1, -1), dilations-1 );
 551
 552                 }
 553                 else
 554                 {
 555                     dilate( thresh_img, thresh_img, Mat(), Point(-1, -1), 1 );
 556                 }
 557                 SHOW("Old binarization", thresh_img);
 558
 559                 // So we can find rectangles that go to the edge, we draw a white line around the image edge.
 560                 // Otherwise FindContours will miss those clipped rectangle contours.
 561                 // The border color will be the image mean, because otherwise we risk screwing up filters like cvSmooth()...
 562                 rectangle( thresh_img, Point(0,0), Point(thresh_img.cols-1, thresh_img.rows-1), Scalar(255,255,255), 3, LINE_8);
 563                 int max_quad_buf_size = 0;
 564                 cvFree(&quads);
 565                 cvFree(&corners);
 566                 Mat binarized_img = (useAdaptive) ? thresh_img : thresh_img.clone(); // make clone because cvFindContours modifies the source image
 567                 int quad_count = icvGenerateQuads( &quads, &corners, storage, binarized_img, flags, &max_quad_buf_size);
 568                 PRINTF("Quad count: %d/%d\n", quad_count, (pattern_size.width/2+1)*(pattern_size.height/2+1));
 569                 SHOW_QUADS("Old quads", thresh_img, quads, quad_count);
 570                 if (processQuads(quads, quad_count, pattern_size, max_quad_buf_size, storage, corners, out_corners, out_corner_count, prev_sqr_size))
 571                     found = 1;
 572             }
 573         }
 574     }
 575
 576     PRINTF("Chessboard detection result 1: %d\n", found);
 577
 578     if( found )
 579         found = icvCheckBoardMonotony( out_corners, pattern_size );
 580
 581     PRINTF("Chessboard detection result 2: %d\n", found);
 582
 583     // check that none of the found corners is too close to the image boundary
 584     if( found )
 585     {
 586         const int BORDER = 8;
 587         for( k = 0; k < pattern_size.width*pattern_size.height; k++ )
 588         {
 589             if( out_corners[k].x <= BORDER || out_corners[k].x > img.cols - BORDER ||
 590                 out_corners[k].y <= BORDER || out_corners[k].y > img.rows - BORDER )
 591                 break;
 592         }
 593
 594         found = k == pattern_size.width*pattern_size.height;
 595     }
 596
 597     PRINTF("Chessboard detection result 3: %d\n", found);
 598
 599     if( found )
 600     {
 601         if ( pattern_size.height % 2 == 0 && pattern_size.width % 2 == 0 )
 602         {
 603             int last_row = (pattern_size.height-1)*pattern_size.width;
 604             double dy0 = out_corners[last_row].y - out_corners[0].y;
 605             if( dy0 < 0 )
 606             {
 607                 int n = pattern_size.width*pattern_size.height;
 608                 for(int i = 0; i < n/2; i++ )
 609                 {
 610                     CvPoint2D32f temp;
 611                     CV_SWAP(out_corners[i], out_corners[n-i-1], temp);
 612                 }
 613             }
 614         }
 615         int wsize = 2;
 616         CvMat old_img(img);
 617         cvFindCornerSubPix( &old_img, out_corners, pattern_size.width*pattern_size.height,
 618                             cvSize(wsize, wsize), cvSize(-1,-1),
 619                             cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 15, 0.1));
 620     }
 621     }
 622     CV_CATCH_ALL
 623     {
 624         cvFree(&quads);
 625         cvFree(&corners);
 626         CV_RETHROW();
 627     }
 628     cvFree(&quads);
 629     cvFree(&corners);
 630     return found;
 631 }
 632
 633 //
 634 // Checks that each board row and column is pretty much monotonous curve:
 635 // It analyzes each row and each column of the chessboard as following:
 636 //    for each corner c lying between end points in the same row/column it checks that
 637 //    the point projection to the line segment (a,b) is lying between projections
 638 //    of the neighbor corners in the same row/column.
 639 //
 640 // This function has been created as temporary workaround for the bug in current implementation
 641 // of cvFindChessboardCornes that produces absolutely unordered sets of corners.
 642 //
 643
 644 static int
 645 icvCheckBoardMonotony( CvPoint2D32f* corners, CvSize pattern_size )
 646 {
 647     int i, j, k;
 648
 649     for( k = 0; k < 2; k++ )
 650     {
 651         for( i = 0; i < (k == 0 ? pattern_size.height : pattern_size.width); i++ )
 652         {
 653             CvPoint2D32f a = k == 0 ? corners[i*pattern_size.width] : corners[i];
 654             CvPoint2D32f b = k == 0 ? corners[(i+1)*pattern_size.width-1] :
 655                 corners[(pattern_size.height-1)*pattern_size.width + i];
 656             float prevt = 0, dx0 = b.x - a.x, dy0 = b.y - a.y;
 657             if( fabs(dx0) + fabs(dy0) < FLT_EPSILON )
 658                 return 0;
 659             for( j = 1; j < (k == 0 ? pattern_size.width : pattern_size.height) - 1; j++ )
 660             {
 661                 CvPoint2D32f c = k == 0 ? corners[i*pattern_size.width + j] :
 662                     corners[j*pattern_size.width + i];
 663                 float t = ((c.x - a.x)*dx0 + (c.y - a.y)*dy0)/(dx0*dx0 + dy0*dy0);
 664                 if( t < prevt || t > 1 )
 665                     return 0;
 666                 prevt = t;
 667             }
 668         }
 669     }
 670
 671     return 1;
 672 }
 673
 674 //
 675 // order a group of connected quads
 676 // order of corners:
 677 //   0 is top left
 678 //   clockwise from there
 679 // note: "top left" is nominal, depends on initial ordering of starting quad
 680 //   but all other quads are ordered consistently
 681 //
 682 // can change the number of quads in the group
 683 // can add quads, so we need to have quad/corner arrays passed in
 684 //
 685
 686 static int
 687 icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,
 688         int *all_count, CvCBQuad **all_quads, CvCBCorner **corners,
 689         CvSize pattern_size, int max_quad_buf_size, CvMemStorage* storage )
 690 {
 691     cv::Ptr<CvMemStorage> temp_storage(cvCreateChildMemStorage( storage ));
 692     CvSeq* stack = cvCreateSeq( 0, sizeof(*stack), sizeof(void*), temp_storage );
 693
 694     // first find an interior quad
 695     CvCBQuad *start = NULL;
 696     for (int i=0; i<quad_count; i++)
 697     {
 698         if (quads[i]->count == 4)
 699         {
 700             start = quads[i];
 701             break;
 702         }
 703     }
 704
 705     if (start == NULL)
 706         return 0;   // no 4-connected quad
 707
 708     // start with first one, assign rows/cols
 709     int row_min = 0, col_min = 0, row_max=0, col_max = 0;
 710
 711     std::map<int, int> col_hist;
 712     std::map<int, int> row_hist;
 713
 714     cvSeqPush(stack, &start);
 715     start->row = 0;
 716     start->col = 0;
 717     start->ordered = true;
 718
 719     // Recursively order the quads so that all position numbers (e.g.,
 720     // 0,1,2,3) are in the at the same relative corner (e.g., lower right).
 721
 722     while( stack->total )
 723     {
 724         CvCBQuad* q;
 725         cvSeqPop( stack, &q );
 726         int col = q->col;
 727         int row = q->row;
 728         col_hist[col]++;
 729         row_hist[row]++;
 730
 731         // check min/max
 732         if (row > row_max) row_max = row;
 733         if (row < row_min) row_min = row;
 734         if (col > col_max) col_max = col;
 735         if (col < col_min) col_min = col;
 736
 737         for(int i = 0; i < 4; i++ )
 738         {
 739             CvCBQuad *neighbor = q->neighbors[i];
 740             switch(i)   // adjust col, row for this quad
 741             {           // start at top left, go clockwise
 742             case 0:
 743                 row--; col--; break;
 744             case 1:
 745                 col += 2; break;
 746             case 2:
 747                 row += 2;   break;
 748             case 3:
 749                 col -= 2; break;
 750             }
 751
 752             // just do inside quads
 753             if (neighbor && neighbor->ordered == false && neighbor->count == 4)
 754             {
 755                 PRINTF("col: %d  row: %d\n", col, row);
 756                 icvOrderQuad(neighbor, q->corners[i], (i+2)%4); // set in order
 757                 neighbor->ordered = true;
 758                 neighbor->row = row;
 759                 neighbor->col = col;
 760                 cvSeqPush( stack, &neighbor );
 761             }
 762         }
 763     }
 764
 765     for (int i=col_min; i<=col_max; i++)
 766         PRINTF("HIST[%d] = %d\n", i, col_hist[i]);
 767
 768     // analyze inner quad structure
 769     int w = pattern_size.width - 1;
 770     int h = pattern_size.height - 1;
 771     int drow = row_max - row_min + 1;
 772     int dcol = col_max - col_min + 1;
 773
 774     // normalize pattern and found quad indices
 775     if ((w > h && dcol < drow) ||
 776         (w < h && drow < dcol))
 777     {
 778         h = pattern_size.width - 1;
 779         w = pattern_size.height - 1;
 780     }
 781
 782     PRINTF("Size: %dx%d  Pattern: %dx%d\n", dcol, drow, w, h);
 783
 784     // check if there are enough inner quads
 785     if (dcol < w || drow < h)   // found enough inner quads?
 786     {
 787         PRINTF("Too few inner quad rows/cols\n");
 788         return 0;   // no, return
 789     }
 790 #ifdef ENABLE_TRIM_COL_ROW
 791     // too many columns, not very common
 792     if (dcol == w+1)    // too many, trim
 793     {
 794         PRINTF("Trimming cols\n");
 795         if (col_hist[col_max] > col_hist[col_min])
 796         {
 797             PRINTF("Trimming left col\n");
 798             quad_count = icvTrimCol(quads,quad_count,col_min,-1);
 799         }
 800         else
 801         {
 802             PRINTF("Trimming right col\n");
 803             quad_count = icvTrimCol(quads,quad_count,col_max,+1);
 804         }
 805     }
 806
 807     // too many rows, not very common
 808     if (drow == h+1)    // too many, trim
 809     {
 810         PRINTF("Trimming rows\n");
 811         if (row_hist[row_max] > row_hist[row_min])
 812         {
 813             PRINTF("Trimming top row\n");
 814             quad_count = icvTrimRow(quads,quad_count,row_min,-1);
 815         }
 816         else
 817         {
 818             PRINTF("Trimming bottom row\n");
 819             quad_count = icvTrimRow(quads,quad_count,row_max,+1);
 820         }
 821     }
 822 #endif
 823
 824     // check edges of inner quads
 825     // if there is an outer quad missing, fill it in
 826     // first order all inner quads
 827     int found = 0;
 828     for (int i=0; i<quad_count; i++)
 829     {
 830         if (quads[i]->count == 4)
 831         {   // ok, look at neighbors
 832             int col = quads[i]->col;
 833             int row = quads[i]->row;
 834             for (int j=0; j<4; j++)
 835             {
 836                 switch(j)   // adjust col, row for this quad
 837                 {       // start at top left, go clockwise
 838                 case 0:
 839                     row--; col--; break;
 840                 case 1:
 841                     col += 2; break;
 842                 case 2:
 843                     row += 2;   break;
 844                 case 3:
 845                     col -= 2; break;
 846                 }
 847                 CvCBQuad *neighbor = quads[i]->neighbors[j];
 848                 if (neighbor && !neighbor->ordered && // is it an inner quad?
 849                     col <= col_max && col >= col_min &&
 850                     row <= row_max && row >= row_min)
 851                 {
 852                     // if so, set in order
 853                     PRINTF("Adding inner: col: %d  row: %d\n", col, row);
 854                     found++;
 855                     icvOrderQuad(neighbor, quads[i]->corners[j], (j+2)%4);
 856                     neighbor->ordered = true;
 857                     neighbor->row = row;
 858                     neighbor->col = col;
 859                 }
 860             }
 861         }
 862     }
 863
 864     // if we have found inner quads, add corresponding outer quads,
 865     //   which are missing
 866     if (found > 0)
 867     {
 868         PRINTF("Found %d inner quads not connected to outer quads, repairing\n", found);
 869         for (int i=0; i<quad_count && *all_count < max_quad_buf_size; i++)
 870         {
 871             if (quads[i]->count < 4 && quads[i]->ordered)
 872             {
 873                 int added = icvAddOuterQuad(quads[i],quads,quad_count,all_quads,*all_count,corners, max_quad_buf_size);
 874                 *all_count += added;
 875                 quad_count += added;
 876             }
 877         }
 878
 879         if (*all_count >= max_quad_buf_size)
 880             return 0;
 881     }
 882
 883
 884     // final trimming of outer quads
 885     if (dcol == w && drow == h) // found correct inner quads
 886     {
 887         PRINTF("Inner bounds ok, check outer quads\n");
 888         int rcount = quad_count;
 889         for (int i=quad_count-1; i>=0; i--) // eliminate any quad not connected to
 890             // an ordered quad
 891         {
 892             if (quads[i]->ordered == false)
 893             {
 894                 bool outer = false;
 895                 for (int j=0; j<4; j++) // any neighbors that are ordered?
 896                 {
 897                     if (quads[i]->neighbors[j] && quads[i]->neighbors[j]->ordered)
 898                         outer = true;
 899                 }
 900                 if (!outer) // not an outer quad, eliminate
 901                 {
 902                     PRINTF("Removing quad %d\n", i);
 903                     icvRemoveQuadFromGroup(quads,rcount,quads[i]);
 904                     rcount--;
 905                 }
 906             }
 907
 908         }
 909         return rcount;
 910     }
 911
 912     return 0;
 913 }
 914
 915
 916 // add an outer quad
 917 // looks for the neighbor of <quad> that isn't present,
 918 //   tries to add it in.
 919 // <quad> is ordered
 920
 921 static int
 922 icvAddOuterQuad( CvCBQuad *quad, CvCBQuad **quads, int quad_count,
 923         CvCBQuad **all_quads, int all_count, CvCBCorner **corners, int max_quad_buf_size )
 924
 925 {
 926     int added = 0;
 927     for (int i=0; i<4 && all_count < max_quad_buf_size; i++) // find no-neighbor corners
 928     {
 929         if (!quad->neighbors[i])    // ok, create and add neighbor
 930         {
 931             int j = (i+2)%4;
 932             PRINTF("Adding quad as neighbor 2\n");
 933             CvCBQuad *q = &(*all_quads)[all_count];
 934             memset( q, 0, sizeof(*q) );
 935             added++;
 936             quads[quad_count] = q;
 937             quad_count++;
 938
 939             // set neighbor and group id
 940             quad->neighbors[i] = q;
 941             quad->count += 1;
 942             q->neighbors[j] = quad;
 943             q->group_idx = quad->group_idx;
 944             q->count = 1;   // number of neighbors
 945             q->ordered = false;
 946             q->edge_len = quad->edge_len;
 947
 948             // make corners of new quad
 949             // same as neighbor quad, but offset
 950             CvPoint2D32f pt = quad->corners[i]->pt;
 951             CvCBCorner* corner;
 952             float dx = pt.x - quad->corners[j]->pt.x;
 953             float dy = pt.y - quad->corners[j]->pt.y;
 954             for (int k=0; k<4; k++)
 955             {
 956                 corner = &(*corners)[all_count*4+k];
 957                 pt = quad->corners[k]->pt;
 958                 memset( corner, 0, sizeof(*corner) );
 959                 corner->pt = pt;
 960                 q->corners[k] = corner;
 961                 corner->pt.x += dx;
 962                 corner->pt.y += dy;
 963             }
 964             // have to set exact corner
 965             q->corners[j] = quad->corners[i];
 966
 967             // now find other neighbor and add it, if possible
 968             if (quad->neighbors[(i+3)%4] &&
 969                 quad->neighbors[(i+3)%4]->ordered &&
 970                 quad->neighbors[(i+3)%4]->neighbors[i] &&
 971                 quad->neighbors[(i+3)%4]->neighbors[i]->ordered )
 972             {
 973                 CvCBQuad *qn = quad->neighbors[(i+3)%4]->neighbors[i];
 974                 q->count = 2;
 975                 q->neighbors[(j+1)%4] = qn;
 976                 qn->neighbors[(i+1)%4] = q;
 977                 qn->count += 1;
 978                 // have to set exact corner
 979                 q->corners[(j+1)%4] = qn->corners[(i+1)%4];
 980             }
 981
 982             all_count++;
 983         }
 984     }
 985     return added;
 986 }
 987
 988
 989 // trimming routines
 990 #ifdef ENABLE_TRIM_COL_ROW
 991 static int
 992 icvTrimCol(CvCBQuad **quads, int count, int col, int dir)
 993 {
 994     int rcount = count;
 995     // find the right quad(s)
 996     for (int i=0; i<count; i++)
 997     {
 998 #ifdef DEBUG_CHESSBOARD
 999         if (quads[i]->ordered)
1000             PRINTF("index: %d  cur: %d\n", col, quads[i]->col);
1001 #endif
1002         if (quads[i]->ordered && quads[i]->col == col)
1003         {
1004             if (dir == 1)
1005             {
1006                 if (quads[i]->neighbors[1])
1007                 {
1008                     icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[1]);
1009                     rcount--;
1010                 }
1011                 if (quads[i]->neighbors[2])
1012                 {
1013                     icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[2]);
1014                     rcount--;
1015                 }
1016             }
1017             else
1018             {
1019                 if (quads[i]->neighbors[0])
1020                 {
1021                     icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[0]);
1022                     rcount--;
1023                 }
1024                 if (quads[i]->neighbors[3])
1025                 {
1026                     icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[3]);
1027                     rcount--;
1028                 }
1029             }
1030
1031         }
1032     }
1033     return rcount;
1034 }
1035
1036 static int
1037 icvTrimRow(CvCBQuad **quads, int count, int row, int dir)
1038 {
1039     int i, rcount = count;
1040     // find the right quad(s)
1041     for (i=0; i<count; i++)
1042     {
1043 #ifdef DEBUG_CHESSBOARD
1044         if (quads[i]->ordered)
1045             PRINTF("index: %d  cur: %d\n", row, quads[i]->row);
1046 #endif
1047         if (quads[i]->ordered && quads[i]->row == row)
1048         {
1049             if (dir == 1)   // remove from bottom
1050             {
1051                 if (quads[i]->neighbors[2])
1052                 {
1053                     icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[2]);
1054                     rcount--;
1055                 }
1056                 if (quads[i]->neighbors[3])
1057                 {
1058                     icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[3]);
1059                     rcount--;
1060                 }
1061             }
1062             else    // remove from top
1063             {
1064                 if (quads[i]->neighbors[0])
1065                 {
1066                     icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[0]);
1067                     rcount--;
1068                 }
1069                 if (quads[i]->neighbors[1])
1070                 {
1071                     icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[1]);
1072                     rcount--;
1073                 }
1074             }
1075
1076         }
1077     }
1078     return rcount;
1079 }
1080 #endif
1081
1082 //
1083 // remove quad from quad group
1084 //
1085
1086 static void
1087 icvRemoveQuadFromGroup(CvCBQuad **quads, int count, CvCBQuad *q0)
1088 {
1089     int i, j;
1090     // remove any references to this quad as a neighbor
1091     for(i = 0; i < count; i++ )
1092     {
1093         CvCBQuad *q = quads[i];
1094         for(j = 0; j < 4; j++ )
1095         {
1096             if( q->neighbors[j] == q0 )
1097             {
1098                 q->neighbors[j] = 0;
1099                 q->count--;
1100                 for(int k = 0; k < 4; k++ )
1101                     if( q0->neighbors[k] == q )
1102                     {
1103                         q0->neighbors[k] = 0;
1104                         q0->count--;
1105                         break;
1106                     }
1107                 break;
1108             }
1109         }
1110     }
1111
1112     // remove the quad
1113     for(i = 0; i < count; i++ )
1114     {
1115         CvCBQuad *q = quads[i];
1116         if (q == q0)
1117         {
1118             quads[i] = quads[count-1];
1119             break;
1120         }
1121     }
1122 }
1123
1124 //
1125 // put quad into correct order, where <corner> has value <common>
1126 //
1127
1128 static void
1129 icvOrderQuad(CvCBQuad *quad, CvCBCorner *corner, int common)
1130 {
1131     // find the corner
1132     int tc;
1133     for (tc=0; tc<4; tc++)
1134         if (quad->corners[tc]->pt.x == corner->pt.x &&
1135             quad->corners[tc]->pt.y == corner->pt.y)
1136             break;
1137
1138     // set corner order
1139     // shift
1140     while (tc != common)
1141     {
1142         // shift by one
1143         CvCBCorner *tempc;
1144         CvCBQuad *tempq;
1145         tempc = quad->corners[3];
1146         tempq = quad->neighbors[3];
1147         for (int i=3; i>0; i--)
1148         {
1149             quad->corners[i] = quad->corners[i-1];
1150             quad->neighbors[i] = quad->neighbors[i-1];
1151         }
1152         quad->corners[0] = tempc;
1153         quad->neighbors[0] = tempq;
1154         tc++;
1155         tc = tc%4;
1156     }
1157 }
1158
1159
1160 // if we found too many connect quads, remove those which probably do not belong.
1161 static int
1162 icvCleanFoundConnectedQuads( int quad_count, CvCBQuad **quad_group, CvSize pattern_size )
1163 {
1164     CvPoint2D32f center;
1165     int i, j, k;
1166     // number of quads this pattern should contain
1167     int count = ((pattern_size.width + 1)*(pattern_size.height + 1) + 1)/2;
1168
1169     // if we have more quadrangles than we should,
1170     // try to eliminate duplicates or ones which don't belong to the pattern rectangle...
1171     if( quad_count <= count )
1172         return quad_count;
1173
1174     // create an array of quadrangle centers
1175     cv::AutoBuffer<CvPoint2D32f> centers( quad_count );
1176     cv::Ptr<CvMemStorage> temp_storage(cvCreateMemStorage(0));
1177
1178     for( i = 0; i < quad_count; i++ )
1179     {
1180         CvPoint2D32f ci;
1181         CvCBQuad* q = quad_group[i];
1182
1183         for( j = 0; j < 4; j++ )
1184         {
1185             CvPoint2D32f pt = q->corners[j]->pt;
1186             ci.x += pt.x;
1187             ci.y += pt.y;
1188         }
1189
1190         ci.x *= 0.25f;
1191         ci.y *= 0.25f;
1192
1193         centers[i] = ci;
1194         center.x += ci.x;
1195         center.y += ci.y;
1196     }
1197     center.x /= quad_count;
1198     center.y /= quad_count;
1199
1200     // If we still have more quadrangles than we should,
1201     // we try to eliminate bad ones based on minimizing the bounding box.
1202     // We iteratively remove the point which reduces the size of
1203     // the bounding box of the blobs the most
1204     // (since we want the rectangle to be as small as possible)
1205     // remove the quadrange that causes the biggest reduction
1206     // in pattern size until we have the correct number
1207     for( ; quad_count > count; quad_count-- )
1208     {
1209         double min_box_area = DBL_MAX;
1210         int skip, min_box_area_index = -1;
1211
1212         // For each point, calculate box area without that point
1213         for( skip = 0; skip < quad_count; skip++ )
1214         {
1215             // get bounding rectangle
1216             CvPoint2D32f temp = centers[skip]; // temporarily make index 'skip' the same as
1217             centers[skip] = center;            // pattern center (so it is not counted for convex hull)
1218             CvMat pointMat = cvMat(1, quad_count, CV_32FC2, centers);
1219             CvSeq *hull = cvConvexHull2( &pointMat, temp_storage, CV_CLOCKWISE, 1 );
1220             centers[skip] = temp;
1221             double hull_area = fabs(cvContourArea(hull, CV_WHOLE_SEQ));
1222
1223             // remember smallest box area
1224             if( hull_area < min_box_area )
1225             {
1226                 min_box_area = hull_area;
1227                 min_box_area_index = skip;
1228             }
1229             cvClearMemStorage( temp_storage );
1230         }
1231
1232         CvCBQuad *q0 = quad_group[min_box_area_index];
1233
1234         // remove any references to this quad as a neighbor
1235         for( i = 0; i < quad_count; i++ )
1236         {
1237             CvCBQuad *q = quad_group[i];
1238             for( j = 0; j < 4; j++ )
1239             {
1240                 if( q->neighbors[j] == q0 )
1241                 {
1242                     q->neighbors[j] = 0;
1243                     q->count--;
1244                     for( k = 0; k < 4; k++ )
1245                         if( q0->neighbors[k] == q )
1246                         {
1247                             q0->neighbors[k] = 0;
1248                             q0->count--;
1249                             break;
1250                         }
1251                     break;
1252                 }
1253             }
1254         }
1255
1256         // remove the quad
1257         quad_count--;
1258         quad_group[min_box_area_index] = quad_group[quad_count];
1259         centers[min_box_area_index] = centers[quad_count];
1260     }
1261
1262     return quad_count;
1263 }
1264
1265 //=====================================================================================
1266
1267 static int
1268 icvFindConnectedQuads( CvCBQuad *quad, int quad_count, CvCBQuad **out_group,
1269                        int group_idx, CvMemStorage* storage )
1270 {
1271     cv::Ptr<CvMemStorage> temp_storage(cvCreateChildMemStorage( storage ));
1272     CvSeq* stack = cvCreateSeq( 0, sizeof(*stack), sizeof(void*), temp_storage );
1273     int i, count = 0;
1274
1275     // Scan the array for a first unlabeled quad
1276     for( i = 0; i < quad_count; i++ )
1277     {
1278         if( quad[i].count > 0 && quad[i].group_idx < 0)
1279             break;
1280     }
1281
1282     // Recursively find a group of connected quads starting from the seed quad[i]
1283     if( i < quad_count )
1284     {
1285         CvCBQuad* q = &quad[i];
1286         cvSeqPush( stack, &q );
1287         out_group[count++] = q;
1288         q->group_idx = group_idx;
1289         q->ordered = false;
1290
1291         while( stack->total )
1292         {
1293             cvSeqPop( stack, &q );
1294             for( i = 0; i < 4; i++ )
1295             {
1296                 CvCBQuad *neighbor = q->neighbors[i];
1297                 if( neighbor && neighbor->count > 0 && neighbor->group_idx < 0 )
1298                 {
1299                     cvSeqPush( stack, &neighbor );
1300                     out_group[count++] = neighbor;
1301                     neighbor->group_idx = group_idx;
1302                     neighbor->ordered = false;
1303                 }
1304             }
1305         }
1306     }
1307
1308     return count;
1309 }
1310
1311
1312 //=====================================================================================
1313
1314 static int
1315 icvCheckQuadGroup( CvCBQuad **quad_group, int quad_count,
1316                    CvCBCorner **out_corners, CvSize pattern_size )
1317 {
1318     const int ROW1 = 1000000;
1319     const int ROW2 = 2000000;
1320     const int ROW_ = 3000000;
1321     int result = 0;
1322     int i, out_corner_count = 0, corner_count = 0;
1323     std::vector<CvCBCorner*> corners(quad_count*4);
1324
1325     int j, k, kk;
1326     int width = 0, height = 0;
1327     int hist[5] = {0,0,0,0,0};
1328     CvCBCorner* first = 0, *first2 = 0, *right, *cur, *below, *c;
1329
1330     // build dual graph, which vertices are internal quad corners
1331     // and two vertices are connected iff they lie on the same quad edge
1332     for( i = 0; i < quad_count; i++ )
1333     {
1334         CvCBQuad* q = quad_group[i];
1335         /*CvScalar color = q->count == 0 ? cvScalar(0,255,255) :
1336                          q->count == 1 ? cvScalar(0,0,255) :
1337                          q->count == 2 ? cvScalar(0,255,0) :
1338                          q->count == 3 ? cvScalar(255,255,0) :
1339                                          cvScalar(255,0,0);*/
1340
1341         for( j = 0; j < 4; j++ )
1342         {
1343             //cvLine( debug_img, cvPointFrom32f(q->corners[j]->pt), cvPointFrom32f(q->corners[(j+1)&3]->pt), color, 1, CV_AA, 0 );
1344             if( q->neighbors[j] )
1345             {
1346                 CvCBCorner *a = q->corners[j], *b = q->corners[(j+1)&3];
1347                 // mark internal corners that belong to:
1348                 //   - a quad with a single neighbor - with ROW1,
1349                 //   - a quad with two neighbors     - with ROW2
1350                 // make the rest of internal corners with ROW_
1351                 int row_flag = q->count == 1 ? ROW1 : q->count == 2 ? ROW2 : ROW_;
1352
1353                 if( a->row == 0 )
1354                 {
1355                     corners[corner_count++] = a;
1356                     a->row = row_flag;
1357                 }
1358                 else if( a->row > row_flag )
1359                     a->row = row_flag;
1360
1361                 if( q->neighbors[(j+1)&3] )
1362                 {
1363                     if( a->count >= 4 || b->count >= 4 )
1364                         goto finalize;
1365                     for( k = 0; k < 4; k++ )
1366                     {
1367                         if( a->neighbors[k] == b )
1368                             goto finalize;
1369                         if( b->neighbors[k] == a )
1370                             goto finalize;
1371                     }
1372                     a->neighbors[a->count++] = b;
1373                     b->neighbors[b->count++] = a;
1374                 }
1375             }
1376         }
1377     }
1378
1379     if( corner_count != pattern_size.width*pattern_size.height )
1380         goto finalize;
1381
1382     for( i = 0; i < corner_count; i++ )
1383     {
1384         int n = corners[i]->count;
1385         assert( 0 <= n && n <= 4 );
1386         hist[n]++;
1387         if( !first && n == 2 )
1388         {
1389             if( corners[i]->row == ROW1 )
1390                 first = corners[i];
1391             else if( !first2 && corners[i]->row == ROW2 )
1392                 first2 = corners[i];
1393         }
1394     }
1395
1396     // start with a corner that belongs to a quad with a single neighbor.
1397     // if we do not have such, start with a corner of a quad with two neighbors.
1398     if( !first )
1399         first = first2;
1400
1401     if( !first || hist[0] != 0 || hist[1] != 0 || hist[2] != 4 ||
1402         hist[3] != (pattern_size.width + pattern_size.height)*2 - 8 )
1403         goto finalize;
1404
1405     cur = first;
1406     right = below = 0;
1407     out_corners[out_corner_count++] = cur;
1408
1409     for( k = 0; k < 4; k++ )
1410     {
1411         c = cur->neighbors[k];
1412         if( c )
1413         {
1414             if( !right )
1415                 right = c;
1416             else if( !below )
1417                 below = c;
1418         }
1419     }
1420
1421     if( !right || (right->count != 2 && right->count != 3) ||
1422         !below || (below->count != 2 && below->count != 3) )
1423         goto finalize;
1424
1425     cur->row = 0;
1426     //cvCircle( debug_img, cvPointFrom32f(cur->pt), 3, cvScalar(0,255,0), -1, 8, 0 );
1427
1428     first = below; // remember the first corner in the next row
1429     // find and store the first row (or column)
1430     for(j=1;;j++)
1431     {
1432         right->row = 0;
1433         out_corners[out_corner_count++] = right;
1434         //cvCircle( debug_img, cvPointFrom32f(right->pt), 3, cvScalar(0,255-j*10,0), -1, 8, 0 );
1435         if( right->count == 2 )
1436             break;
1437         if( right->count != 3 || out_corner_count >= MAX(pattern_size.width,pattern_size.height) )
1438             goto finalize;
1439         cur = right;
1440         for( k = 0; k < 4; k++ )
1441         {
1442             c = cur->neighbors[k];
1443             if( c && c->row > 0 )
1444             {
1445                 for( kk = 0; kk < 4; kk++ )
1446                 {
1447                     if( c->neighbors[kk] == below )
1448                         break;
1449                 }
1450                 if( kk < 4 )
1451                     below = c;
1452                 else
1453                     right = c;
1454             }
1455         }
1456     }
1457
1458     width = out_corner_count;
1459     if( width == pattern_size.width )
1460         height = pattern_size.height;
1461     else if( width == pattern_size.height )
1462         height = pattern_size.width;
1463     else
1464         goto finalize;
1465
1466     // find and store all the other rows
1467     for( i = 1; ; i++ )
1468     {
1469         if( !first )
1470             break;
1471         cur = first;
1472         first = 0;
1473         for( j = 0;; j++ )
1474         {
1475             cur->row = i;
1476             out_corners[out_corner_count++] = cur;
1477             //cvCircle( debug_img, cvPointFrom32f(cur->pt), 3, cvScalar(0,0,255-j*10), -1, 8, 0 );
1478             if( cur->count == 2 + (i < height-1) && j > 0 )
1479                 break;
1480
1481             right = 0;
1482
1483             // find a neighbor that has not been processed yet
1484             // and that has a neighbor from the previous row
1485             for( k = 0; k < 4; k++ )
1486             {
1487                 c = cur->neighbors[k];
1488                 if( c && c->row > i )
1489                 {
1490                     for( kk = 0; kk < 4; kk++ )
1491                     {
1492                         if( c->neighbors[kk] && c->neighbors[kk]->row == i-1 )
1493                             break;
1494                     }
1495                     if( kk < 4 )
1496                     {
1497                         right = c;
1498                         if( j > 0 )
1499                             break;
1500                     }
1501                     else if( j == 0 )
1502                         first = c;
1503                 }
1504             }
1505             if( !right )
1506                 goto finalize;
1507             cur = right;
1508         }
1509
1510         if( j != width - 1 )
1511             goto finalize;
1512     }
1513
1514     if( out_corner_count != corner_count )
1515         goto finalize;
1516
1517     // check if we need to transpose the board
1518     if( width != pattern_size.width )
1519     {
1520         CV_SWAP( width, height, k );
1521
1522         memcpy( &corners[0], out_corners, corner_count*sizeof(corners[0]) );
1523         for( i = 0; i < height; i++ )
1524             for( j = 0; j < width; j++ )
1525                 out_corners[i*width + j] = corners[j*height + i];
1526     }
1527
1528     // check if we need to revert the order in each row
1529     {
1530         CvPoint2D32f p0 = out_corners[0]->pt, p1 = out_corners[pattern_size.width-1]->pt,
1531                      p2 = out_corners[pattern_size.width]->pt;
1532         if( (p1.x - p0.x)*(p2.y - p1.y) - (p1.y - p0.y)*(p2.x - p1.x) < 0 )
1533         {
1534             if( width % 2 == 0 )
1535             {
1536                 for( i = 0; i < height; i++ )
1537                     for( j = 0; j < width/2; j++ )
1538                         CV_SWAP( out_corners[i*width+j], out_corners[i*width+width-j-1], c );
1539             }
1540             else
1541             {
1542                 for( j = 0; j < width; j++ )
1543                     for( i = 0; i < height/2; i++ )
1544                         CV_SWAP( out_corners[i*width+j], out_corners[(height - i - 1)*width+j], c );
1545             }
1546         }
1547     }
1548
1549     result = corner_count;
1550
1551 finalize:
1552
1553     if( result <= 0 )
1554     {
1555         corner_count = MIN( corner_count, pattern_size.width*pattern_size.height );
1556         for( i = 0; i < corner_count; i++ )
1557             out_corners[i] = corners[i];
1558         result = -corner_count;
1559
1560         if (result == -pattern_size.width*pattern_size.height)
1561             result = -result;
1562     }
1563
1564     return result;
1565 }
1566
1567
1568
1569
1570 //=====================================================================================
1571
1572 static void icvFindQuadNeighbors( CvCBQuad *quads, int quad_count )
1573 {
1574     const float thresh_scale = 1.f;
1575     int idx, i, k, j;
1576     float dx, dy, dist;
1577
1578     // find quad neighbors
1579     for( idx = 0; idx < quad_count; idx++ )
1580     {
1581         CvCBQuad* cur_quad = &quads[idx];
1582
1583         // choose the points of the current quadrangle that are close to
1584         // some points of the other quadrangles
1585         // (it can happen for split corners (due to dilation) of the
1586         // checker board). Search only in other quadrangles!
1587
1588         // for each corner of this quadrangle
1589         for( i = 0; i < 4; i++ )
1590         {
1591             CvPoint2D32f pt;
1592             float min_dist = FLT_MAX;
1593             int closest_corner_idx = -1;
1594             CvCBQuad *closest_quad = 0;
1595             CvCBCorner *closest_corner = 0;
1596
1597             if( cur_quad->neighbors[i] )
1598                 continue;
1599
1600             pt = cur_quad->corners[i]->pt;
1601
1602             // find the closest corner in all other quadrangles
1603             for( k = 0; k < quad_count; k++ )
1604             {
1605                 if( k == idx )
1606                     continue;
1607
1608                 for( j = 0; j < 4; j++ )
1609                 {
1610                     if( quads[k].neighbors[j] )
1611                         continue;
1612
1613                     dx = pt.x - quads[k].corners[j]->pt.x;
1614                     dy = pt.y - quads[k].corners[j]->pt.y;
1615                     dist = dx * dx + dy * dy;
1616
1617                     if( dist < min_dist &&
1618                         dist <= cur_quad->edge_len*thresh_scale &&
1619                         dist <= quads[k].edge_len*thresh_scale )
1620                     {
1621                         // check edge lengths, make sure they're compatible
1622                         // edges that are different by more than 1:4 are rejected
1623                         float ediff = cur_quad->edge_len - quads[k].edge_len;
1624                         if (ediff > 32*cur_quad->edge_len ||
1625                             ediff > 32*quads[k].edge_len)
1626                         {
1627                             PRINTF("Incompatible edge lengths\n");
1628                             continue;
1629                         }
1630                         closest_corner_idx = j;
1631                         closest_quad = &quads[k];
1632                         min_dist = dist;
1633                     }
1634                 }
1635             }
1636
1637             // we found a matching corner point?
1638             if( closest_corner_idx >= 0 && min_dist < FLT_MAX )
1639             {
1640                 // If another point from our current quad is closer to the found corner
1641                 // than the current one, then we don't count this one after all.
1642                 // This is necessary to support small squares where otherwise the wrong
1643                 // corner will get matched to closest_quad;
1644                 closest_corner = closest_quad->corners[closest_corner_idx];
1645
1646                 for( j = 0; j < 4; j++ )
1647                 {
1648                     if( cur_quad->neighbors[j] == closest_quad )
1649                         break;
1650
1651                     dx = closest_corner->pt.x - cur_quad->corners[j]->pt.x;
1652                     dy = closest_corner->pt.y - cur_quad->corners[j]->pt.y;
1653
1654                     if( dx * dx + dy * dy < min_dist )
1655                         break;
1656                 }
1657
1658                 if( j < 4 || cur_quad->count >= 4 || closest_quad->count >= 4 )
1659                     continue;
1660
1661                 // Check that each corner is a neighbor of different quads
1662                 for( j = 0; j < closest_quad->count; j++ )
1663                 {
1664                     if( closest_quad->neighbors[j] == cur_quad )
1665                         break;
1666                 }
1667                 if( j < closest_quad->count )
1668                     continue;
1669
1670                 // check whether the closest corner to closest_corner
1671                 // is different from cur_quad->corners[i]->pt
1672                 for( k = 0; k < quad_count; k++ )
1673                 {
1674                     CvCBQuad* q = &quads[k];
1675                     if( k == idx || q == closest_quad )
1676                         continue;
1677
1678                     for( j = 0; j < 4; j++ )
1679                         if( !q->neighbors[j] )
1680                         {
1681                             dx = closest_corner->pt.x - q->corners[j]->pt.x;
1682                             dy = closest_corner->pt.y - q->corners[j]->pt.y;
1683                             dist = dx*dx + dy*dy;
1684                             if( dist < min_dist )
1685                                 break;
1686                         }
1687                     if( j < 4 )
1688                         break;
1689                 }
1690
1691                 if( k < quad_count )
1692                     continue;
1693
1694                 closest_corner->pt.x = (pt.x + closest_corner->pt.x) * 0.5f;
1695                 closest_corner->pt.y = (pt.y + closest_corner->pt.y) * 0.5f;
1696
1697                 // We've found one more corner - remember it
1698                 cur_quad->count++;
1699                 cur_quad->neighbors[i] = closest_quad;
1700                 cur_quad->corners[i] = closest_corner;
1701
1702                 closest_quad->count++;
1703                 closest_quad->neighbors[closest_corner_idx] = cur_quad;
1704             }
1705         }
1706     }
1707 }
1708
1709 //=====================================================================================
1710
1711 // returns corners in clockwise order
1712 // corners don't necessarily start at same position on quad (e.g.,
1713 //   top left corner)
1714
1715 static int
1716 icvGenerateQuads( CvCBQuad **out_quads, CvCBCorner **out_corners,
1717                   CvMemStorage *storage, const cv::Mat & image_, int flags, int *max_quad_buf_size )
1718 {
1719     CvMat image_old(image_), *image = &image_old;
1720     int quad_count = 0;
1721     cv::Ptr<CvMemStorage> temp_storage;
1722
1723     if( out_quads )
1724         *out_quads = 0;
1725
1726     if( out_corners )
1727         *out_corners = 0;
1728
1729     CvSeq *src_contour = 0;
1730     CvSeq *root;
1731     CvContourEx* board = 0;
1732     CvContourScanner scanner;
1733     int i, idx, min_size;
1734
1735     CV_Assert( out_corners && out_quads );
1736
1737     // empiric bound for minimal allowed perimeter for squares
1738     min_size = 25; //cvRound( image->cols * image->rows * .03 * 0.01 * 0.92 );
1739
1740     // create temporary storage for contours and the sequence of pointers to found quadrangles
1741     temp_storage.reset(cvCreateChildMemStorage( storage ));
1742     root = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSeq*), temp_storage );
1743
1744     // initialize contour retrieving routine
1745     scanner = cvStartFindContours( image, temp_storage, sizeof(CvContourEx),
1746                                    CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
1747
1748     // get all the contours one by one
1749     while( (src_contour = cvFindNextContour( scanner )) != 0 )
1750     {
1751         CvSeq *dst_contour = 0;
1752         CvRect rect = ((CvContour*)src_contour)->rect;
1753
1754         // reject contours with too small perimeter
1755         if( CV_IS_SEQ_HOLE(src_contour) && rect.width*rect.height >= min_size )
1756         {
1757             const int min_approx_level = 1, max_approx_level = MAX_CONTOUR_APPROX;
1758             int approx_level;
1759             for( approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++ )
1760             {
1761                 dst_contour = cvApproxPoly( src_contour, sizeof(CvContour), temp_storage,
1762                                             CV_POLY_APPROX_DP, (float)approx_level );
1763                 if( dst_contour->total == 4 )
1764                     break;
1765
1766                 // we call this again on its own output, because sometimes
1767                 // cvApproxPoly() does not simplify as much as it should.
1768                 dst_contour = cvApproxPoly( dst_contour, sizeof(CvContour), temp_storage,
1769                                             CV_POLY_APPROX_DP, (float)approx_level );
1770
1771                 if( dst_contour->total == 4 )
1772                     break;
1773             }
1774
1775             // reject non-quadrangles
1776             if( dst_contour->total == 4 && cvCheckContourConvexity(dst_contour) )
1777             {
1778                 CvPoint pt[4];
1779                 double d1, d2, p = cvContourPerimeter(dst_contour);
1780                 double area = fabs(cvContourArea(dst_contour, CV_WHOLE_SEQ));
1781                 double dx, dy;
1782
1783                 for( i = 0; i < 4; i++ )
1784                     pt[i] = *(CvPoint*)cvGetSeqElem(dst_contour, i);
1785
1786                 dx = pt[0].x - pt[2].x;
1787                 dy = pt[0].y - pt[2].y;
1788                 d1 = sqrt(dx*dx + dy*dy);
1789
1790                 dx = pt[1].x - pt[3].x;
1791                 dy = pt[1].y - pt[3].y;
1792                 d2 = sqrt(dx*dx + dy*dy);
1793
1794                 // philipg.  Only accept those quadrangles which are more square
1795                 // than rectangular and which are big enough
1796                 double d3, d4;
1797                 dx = pt[0].x - pt[1].x;
1798                 dy = pt[0].y - pt[1].y;
1799                 d3 = sqrt(dx*dx + dy*dy);
1800                 dx = pt[1].x - pt[2].x;
1801                 dy = pt[1].y - pt[2].y;
1802                 d4 = sqrt(dx*dx + dy*dy);
1803                 if( !(flags & CV_CALIB_CB_FILTER_QUADS) ||
1804                     (d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > min_size &&
1805                     d1 >= 0.15 * p && d2 >= 0.15 * p) )
1806                 {
1807                     CvContourEx* parent = (CvContourEx*)(src_contour->v_prev);
1808                     parent->counter++;
1809                     if( !board || board->counter < parent->counter )
1810                         board = parent;
1811                     dst_contour->v_prev = (CvSeq*)parent;
1812                     //for( i = 0; i < 4; i++ ) cvLine( debug_img, pt[i], pt[(i+1)&3], cvScalar(200,255,255), 1, CV_AA, 0 );
1813                     cvSeqPush( root, &dst_contour );
1814                 }
1815             }
1816         }
1817     }
1818
1819     // finish contour retrieving
1820     cvEndFindContours( &scanner );
1821
1822     // allocate quad & corner buffers
1823     *max_quad_buf_size = MAX(1, (root->total+root->total / 2)) * 2;
1824     *out_quads = (CvCBQuad*)cvAlloc(*max_quad_buf_size * sizeof((*out_quads)[0]));
1825     *out_corners = (CvCBCorner*)cvAlloc(*max_quad_buf_size * 4 * sizeof((*out_corners)[0]));
1826
1827     // Create array of quads structures
1828     for( idx = 0; idx < root->total; idx++ )
1829     {
1830         CvCBQuad* q = &(*out_quads)[quad_count];
1831         src_contour = *(CvSeq**)cvGetSeqElem( root, idx );
1832         if( (flags & CV_CALIB_CB_FILTER_QUADS) && src_contour->v_prev != (CvSeq*)board )
1833             continue;
1834
1835         // reset group ID
1836         memset( q, 0, sizeof(*q) );
1837         q->group_idx = -1;
1838         assert( src_contour->total == 4 );
1839         for( i = 0; i < 4; i++ )
1840         {
1841             CvPoint * onePoint = (CvPoint*)cvGetSeqElem(src_contour, i);
1842             CV_Assert(onePoint != NULL);
1843             CvPoint2D32f pt = cvPointTo32f(*onePoint);
1844             CvCBCorner* corner = &(*out_corners)[quad_count*4 + i];
1845
1846             memset( corner, 0, sizeof(*corner) );
1847             corner->pt = pt;
1848             q->corners[i] = corner;
1849         }
1850         q->edge_len = FLT_MAX;
1851         for( i = 0; i < 4; i++ )
1852         {
1853             float dx = q->corners[i]->pt.x - q->corners[(i+1)&3]->pt.x;
1854             float dy = q->corners[i]->pt.y - q->corners[(i+1)&3]->pt.y;
1855             float d = dx*dx + dy*dy;
1856             if( q->edge_len > d )
1857                 q->edge_len = d;
1858         }
1859         quad_count++;
1860     }
1861
1862     return quad_count;
1863 }
1864
1865 static bool processQuads(CvCBQuad *quads, int quad_count, CvSize pattern_size, int max_quad_buf_size,
1866                          CvMemStorage * storage, CvCBCorner *corners, CvPoint2D32f *out_corners, int *out_corner_count, int & prev_sqr_size)
1867 {
1868     if( quad_count <= 0 )
1869         return false;
1870
1871     bool found = false;
1872
1873     // Find quad's neighbors
1874     icvFindQuadNeighbors( quads, quad_count );
1875
1876     // allocate extra for adding in icvOrderFoundQuads
1877     CvCBQuad **quad_group = 0;
1878     CvCBCorner **corner_group = 0;
1879
1880     quad_group = (CvCBQuad**)cvAlloc( sizeof(quad_group[0]) * max_quad_buf_size);
1881     corner_group = (CvCBCorner**)cvAlloc( sizeof(corner_group[0]) * max_quad_buf_size * 4 );
1882
1883     for( int group_idx = 0; ; group_idx++ )
1884     {
1885         int count = icvFindConnectedQuads( quads, quad_count, quad_group, group_idx, storage );
1886
1887         if( count == 0 )
1888             break;
1889
1890         // order the quad corners globally
1891         // maybe delete or add some
1892         PRINTF("Starting ordering of inner quads (%d)\n", count);
1893         count = icvOrderFoundConnectedQuads(count, quad_group, &quad_count, &quads, &corners,
1894                                             pattern_size, max_quad_buf_size, storage );
1895         PRINTF("Finished ordering of inner quads (%d)\n", count);
1896
1897         if (count == 0)
1898             continue;       // haven't found inner quads
1899
1900         // If count is more than it should be, this will remove those quads
1901         // which cause maximum deviation from a nice square pattern.
1902         count = icvCleanFoundConnectedQuads( count, quad_group, pattern_size );
1903         PRINTF("Connected group: %d, count: %d\n", group_idx, count);
1904
1905         count = icvCheckQuadGroup( quad_group, count, corner_group, pattern_size );
1906         PRINTF("Connected group: %d, count: %d\n", group_idx, count);
1907
1908         int n = count > 0 ? pattern_size.width * pattern_size.height : -count;
1909         n = MIN( n, pattern_size.width * pattern_size.height );
1910         float sum_dist = 0;
1911         int total = 0;
1912
1913         for(int i = 0; i < n; i++ )
1914         {
1915             int ni = 0;
1916             float avgi = corner_group[i]->meanDist(&ni);
1917             sum_dist += avgi*ni;
1918             total += ni;
1919         }
1920         prev_sqr_size = cvRound(sum_dist/MAX(total, 1));
1921
1922         if( count > 0 || (out_corner_count && -count > *out_corner_count) )
1923         {
1924             // copy corners to output array
1925             for(int i = 0; i < n; i++ )
1926                 out_corners[i] = corner_group[i]->pt;
1927
1928             if( out_corner_count )
1929                 *out_corner_count = n;
1930
1931             if( count == pattern_size.width*pattern_size.height
1932                     && icvCheckBoardMonotony( out_corners, pattern_size ))
1933             {
1934                 found = true;
1935                 break;
1936             }
1937         }
1938     }
1939
1940     cvFree(&quad_group);
1941     cvFree(&corner_group);
1942
1943     return found;
1944 }
1945
1946 //==================================================================================================
1947
1948 CV_IMPL void
1949 cvDrawChessboardCorners( CvArr* _image, CvSize pattern_size,
1950                          CvPoint2D32f* corners, int count, int found )
1951 {
1952     const int shift = 0;
1953     const int radius = 4;
1954     const int r = radius*(1 << shift);
1955     int i;
1956     CvMat stub, *image;
1957     double scale = 1;
1958     int type, cn, line_type;
1959
1960     image = cvGetMat( _image, &stub );
1961
1962     type = CV_MAT_TYPE(image->type);
1963     cn = CV_MAT_CN(type);
1964     if( cn != 1 && cn != 3 && cn != 4 )
1965         CV_Error( CV_StsUnsupportedFormat, "Number of channels must be 1, 3 or 4" );
1966
1967     switch( CV_MAT_DEPTH(image->type) )
1968     {
1969     case CV_8U:
1970         scale = 1;
1971         break;
1972     case CV_16U:
1973         scale = 256;
1974         break;
1975     case CV_32F:
1976         scale = 1./255;
1977         break;
1978     default:
1979         CV_Error( CV_StsUnsupportedFormat,
1980             "Only 8-bit, 16-bit or floating-point 32-bit images are supported" );
1981     }
1982
1983     line_type = type == CV_8UC1 || type == CV_8UC3 ? CV_AA : 8;
1984
1985     if( !found )
1986     {
1987         CvScalar color(0,0,255,0);
1988         if( cn == 1 )
1989             color = cvScalarAll(200);
1990         color.val[0] *= scale;
1991         color.val[1] *= scale;
1992         color.val[2] *= scale;
1993         color.val[3] *= scale;
1994
1995         for( i = 0; i < count; i++ )
1996         {
1997             CvPoint pt;
1998             pt.x = cvRound(corners[i].x*(1 << shift));
1999             pt.y = cvRound(corners[i].y*(1 << 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             cvLine( image, cvPoint( pt.x - r, pt.y + r),
2003                     cvPoint( pt.x + r, pt.y - r), color, 1, line_type, shift );
2004             cvCircle( image, pt, r+(1<<shift), color, 1, line_type, shift );
2005         }
2006     }
2007     else
2008     {
2009         int x, y;
2010         CvPoint prev_pt;
2011         const int line_max = 7;
2012         static const CvScalar line_colors[line_max] =
2013         {
2014             CvScalar(0,0,255),
2015             CvScalar(0,128,255),
2016             CvScalar(0,200,200),
2017             CvScalar(0,255,0),
2018             CvScalar(200,200,0),
2019             CvScalar(255,0,0),
2020             CvScalar(255,0,255)
2021         };
2022
2023         for( y = 0, i = 0; y < pattern_size.height; y++ )
2024         {
2025             CvScalar color = line_colors[y % line_max];
2026             if( cn == 1 )
2027                 color = cvScalarAll(200);
2028             color.val[0] *= scale;
2029             color.val[1] *= scale;
2030             color.val[2] *= scale;
2031             color.val[3] *= scale;
2032
2033             for( x = 0; x < pattern_size.width; x++, i++ )
2034             {
2035                 CvPoint pt;
2036                 pt.x = cvRound(corners[i].x*(1 << shift));
2037                 pt.y = cvRound(corners[i].y*(1 << shift));
2038
2039                 if( i != 0 )
2040                     cvLine( image, prev_pt, pt, color, 1, line_type, shift );
2041
2042                 cvLine( image, cvPoint(pt.x - r, pt.y - r),
2043                         cvPoint(pt.x + r, pt.y + r), color, 1, line_type, shift );
2044                 cvLine( image, cvPoint(pt.x - r, pt.y + r),
2045                         cvPoint(pt.x + r, pt.y - r), color, 1, line_type, shift );
2046                 cvCircle( image, pt, r+(1<<shift), color, 1, line_type, shift );
2047                 prev_pt = pt;
2048             }
2049         }
2050     }
2051 }
2052
2053 bool cv::findChessboardCorners( InputArray _image, Size patternSize,
2054                             OutputArray corners, int flags )
2055 {
2056     CV_INSTRUMENT_REGION()
2057
2058     int count = patternSize.area()*2;
2059     std::vector<Point2f> tmpcorners(count+1);
2060     Mat image = _image.getMat(); CvMat c_image = image;
2061     bool ok = cvFindChessboardCorners(&c_image, patternSize,
2062         (CvPoint2D32f*)&tmpcorners[0], &count, flags ) > 0;
2063     if( count > 0 )
2064     {
2065         tmpcorners.resize(count);
2066         Mat(tmpcorners).copyTo(corners);
2067     }
2068     else
2069         corners.release();
2070     return ok;
2071 }
2072
2073 namespace
2074 {
2075 int quiet_error(int /*status*/, const char* /*func_name*/,
2076                                        const char* /*err_msg*/, const char* /*file_name*/,
2077                                        int /*line*/, void* /*userdata*/ )
2078 {
2079   return 0;
2080 }
2081 }
2082
2083 void cv::drawChessboardCorners( InputOutputArray _image, Size patternSize,
2084                             InputArray _corners,
2085                             bool patternWasFound )
2086 {
2087     CV_INSTRUMENT_REGION()
2088
2089     Mat corners = _corners.getMat();
2090     if( corners.empty() )
2091         return;
2092     Mat image = _image.getMat(); CvMat c_image = image;
2093     int nelems = corners.checkVector(2, CV_32F, true);
2094     CV_Assert(nelems >= 0);
2095     cvDrawChessboardCorners( &c_image, patternSize, corners.ptr<CvPoint2D32f>(),
2096                              nelems, patternWasFound );
2097 }
2098
2099 bool cv::findCirclesGrid( InputArray image, Size patternSize,
2100                                    OutputArray centers, int flags,
2101                                    const Ptr<FeatureDetector> &blobDetector,
2102                                    CirclesGridFinderParameters parameters)
2103 {
2104     CirclesGridFinderParameters2 parameters2;
2105     *((CirclesGridFinderParameters*)&parameters2) = parameters;
2106     return cv::findCirclesGrid2(image, patternSize, centers, flags, blobDetector, parameters2);
2107 }
2108
2109 bool cv::findCirclesGrid2( InputArray _image, Size patternSize,
2110                           OutputArray _centers, int flags, const Ptr<FeatureDetector> &blobDetector,
2111                           CirclesGridFinderParameters2 parameters)
2112 {
2113     CV_INSTRUMENT_REGION()
2114
2115     bool isAsymmetricGrid = (flags & CALIB_CB_ASYMMETRIC_GRID) ? true : false;
2116     bool isSymmetricGrid  = (flags & CALIB_CB_SYMMETRIC_GRID ) ? true : false;
2117     CV_Assert(isAsymmetricGrid ^ isSymmetricGrid);
2118
2119     Mat image = _image.getMat();
2120     std::vector<Point2f> centers;
2121
2122     std::vector<KeyPoint> keypoints;
2123     blobDetector->detect(image, keypoints);
2124     std::vector<Point2f> points;
2125     for (size_t i = 0; i < keypoints.size(); i++)
2126     {
2127       points.push_back (keypoints[i].pt);
2128     }
2129
2130     if(flags & CALIB_CB_ASYMMETRIC_GRID)
2131       parameters.gridType = CirclesGridFinderParameters::ASYMMETRIC_GRID;
2132     if(flags & CALIB_CB_SYMMETRIC_GRID)
2133       parameters.gridType = CirclesGridFinderParameters::SYMMETRIC_GRID;
2134
2135     if(flags & CALIB_CB_CLUSTERING)
2136     {
2137       CirclesGridClusterFinder circlesGridClusterFinder(parameters);
2138       circlesGridClusterFinder.findGrid(points, patternSize, centers);
2139       Mat(centers).copyTo(_centers);
2140       return !centers.empty();
2141     }
2142
2143     const int attempts = 2;
2144     const size_t minHomographyPoints = 4;
2145     Mat H;
2146     for (int i = 0; i < attempts; i++)
2147     {
2148       centers.clear();
2149       CirclesGridFinder boxFinder(patternSize, points, parameters);
2150       bool isFound = false;
2151 #define BE_QUIET 1
2152 #if BE_QUIET
2153       void* oldCbkData;
2154       ErrorCallback oldCbk = redirectError(quiet_error, 0, &oldCbkData);
2155 #endif
2156       CV_TRY
2157       {
2158         isFound = boxFinder.findHoles();
2159       }
2160       CV_CATCH(Exception, e)
2161       {
2162           CV_UNUSED(e);
2163       }
2164 #if BE_QUIET
2165       redirectError(oldCbk, oldCbkData);
2166 #endif
2167       if (isFound)
2168       {
2169         switch(parameters.gridType)
2170         {
2171           case CirclesGridFinderParameters::SYMMETRIC_GRID:
2172             boxFinder.getHoles(centers);
2173             break;
2174           case CirclesGridFinderParameters::ASYMMETRIC_GRID:
2175         boxFinder.getAsymmetricHoles(centers);
2176         break;
2177           default:
2178             CV_Error(CV_StsBadArg, "Unknown pattern type");
2179         }
2180
2181         if (i != 0)
2182         {
2183           Mat orgPointsMat;
2184           transform(centers, orgPointsMat, H.inv());
2185           convertPointsFromHomogeneous(orgPointsMat, centers);
2186         }
2187         Mat(centers).copyTo(_centers);
2188         return true;
2189       }
2190
2191       boxFinder.getHoles(centers);
2192       if (i != attempts - 1)
2193       {
2194         if (centers.size() < minHomographyPoints)
2195           break;
2196         H = CirclesGridFinder::rectifyGrid(boxFinder.getDetectedGridSize(), centers, points, points);
2197       }
2198     }
2199     Mat(centers).copyTo(_centers);
2200     return false;
2201 }
2202
2203 bool cv::findCirclesGrid( InputArray _image, Size patternSize,
2204                           OutputArray _centers, int flags, const Ptr<FeatureDetector> &blobDetector)
2205 {
2206     return cv::findCirclesGrid2(_image, patternSize, _centers, flags, blobDetector, CirclesGridFinderParameters2());
2207 }
2208
2209 /* End of file. */