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