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