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