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