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