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