1 //M*//////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
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.
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
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.
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.
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.
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 ===============================================================
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.
54 Reliability additions and modifications made by Philip Gruebele.
55 <a href="mailto:pgruebele@cox.net">pgruebele@cox.net</a>
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
60 \************************************************************************************/
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.
70 \************************************************************************************/
72 #include "precomp.hpp"
73 #include "opencv2/imgproc/imgproc_c.h"
74 #include "opencv2/calib3d/calib3d_c.h"
75 #include "circlesgrid.hpp"
79 #include "opencv2/core/utility.hpp"
80 #include <opencv2/core/utils/logger.defines.hpp>
82 //#define ENABLE_TRIM_COL_ROW
84 //#define DEBUG_CHESSBOARD
86 //#undef CV_LOG_STRIP_LEVEL
87 //#define CV_LOG_STRIP_LEVEL CV_LOG_LEVEL_VERBOSE + 1
88 #include <opencv2/core/utils/logger.hpp>
90 #ifdef DEBUG_CHESSBOARD
91 static int PRINTF( const char* fmt, ... )
95 return vprintf(fmt, args);
104 //=====================================================================================
105 // Implementation for the enhanced calibration object detection
106 //=====================================================================================
108 #define MAX_CONTOUR_APPROX 7
110 #define USE_CV_FINDCONTOURS // switch between cv::findContours() and legacy C API
111 #ifdef USE_CV_FINDCONTOURS
112 struct QuadCountour {
116 QuadCountour(const Point pt_[4], int parent_contour_) :
117 parent_contour(parent_contour_)
119 pt[0] = pt_[0]; pt[1] = pt_[1]; pt[2] = pt_[2]; pt[3] = pt_[3];
130 //=====================================================================================
132 /// Corner info structure
133 /** This structure stores information about the chessboard corner.*/
136 CvPoint2D32f pt; // Coordinates of the corner
137 int row; // Board row index
138 int count; // Number of neighbor corners
139 struct CvCBCorner* neighbors[4]; // Neighbor corners
141 float meanDist(int *_n) const
145 for( int i = 0; i < 4; i++ )
149 float dx = neighbors[i]->pt.x - pt.x;
150 float dy = neighbors[i]->pt.y - pt.y;
151 sum += sqrt(dx*dx + dy*dy);
161 //=====================================================================================
162 /// Quadrangle contour info structure
163 /** This structure stores information about the chessboard quadrange.*/
166 int count; // Number of quad neighbors
167 int group_idx; // quad group ID
168 int row, col; // row and column of this quad
169 bool ordered; // true if corners/neighbors are ordered counter-clockwise
170 float edge_len; // quad edge len, in pix^2
171 // neighbors and corners are synced, i.e., neighbor 0 shares corner 0
172 CvCBCorner *corners[4]; // Coordinates of quad corners
173 struct CvCBQuad *neighbors[4]; // Pointers of quad neighbors
176 //=====================================================================================
178 #ifdef DEBUG_CHESSBOARD
179 #include "opencv2/highgui.hpp"
180 #include "opencv2/imgproc.hpp"
181 static void SHOW(const std::string & name, Mat & img)
184 while ((uchar)waitKey(0) != 'q') {}
186 static void SHOW_QUADS(const std::string & name, const Mat & img_, CvCBQuad * quads, int quads_count)
188 Mat img = img_.clone();
189 if (img.channels() == 1)
190 cvtColor(img, img, COLOR_GRAY2BGR);
191 for (int i = 0; i < quads_count; ++i)
193 CvCBQuad & quad = quads[i];
194 for (int j = 0; j < 4; ++j)
196 line(img, quad.corners[j]->pt, quad.corners[(j + 1) % 4]->pt, Scalar(0, 240, 0), 1, LINE_AA);
200 while ((uchar)waitKey(0) != 'q') {}
204 #define SHOW_QUADS(...)
207 //=====================================================================================
209 static int icvGenerateQuads( CvCBQuad **quads, CvCBCorner **corners,
210 CvMemStorage *storage, const Mat &image_, int flags, int *max_quad_buf_size);
212 static bool processQuads(CvCBQuad *quads, int quad_count, CvSize pattern_size, int max_quad_buf_size,
213 CvMemStorage * storage, CvCBCorner *corners, CvPoint2D32f *out_corners, int *out_corner_count, int & prev_sqr_size);
216 icvGenerateQuadsEx( CvCBQuad **out_quads, CvCBCorner **out_corners,
217 CvMemStorage *storage, CvMat *image, CvMat *thresh_img, int dilation, int flags );*/
219 static void icvFindQuadNeighbors( CvCBQuad *quads, int quad_count );
221 static int icvFindConnectedQuads( CvCBQuad *quads, int quad_count,
222 CvCBQuad **quad_group, int group_idx,
223 CvMemStorage* storage );
225 static int icvCheckQuadGroup( CvCBQuad **quad_group, int count,
226 CvCBCorner **out_corners, CvSize pattern_size );
228 static int icvCleanFoundConnectedQuads( int quad_count,
229 CvCBQuad **quads, CvSize pattern_size );
231 static int icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,
232 int *all_count, CvCBQuad **all_quads, CvCBCorner **corners,
233 CvSize pattern_size, int max_quad_buf_size, CvMemStorage* storage );
235 static void icvOrderQuad(CvCBQuad *quad, CvCBCorner *corner, int common);
237 #ifdef ENABLE_TRIM_COL_ROW
238 static int icvTrimCol(CvCBQuad **quads, int count, int col, int dir);
240 static int icvTrimRow(CvCBQuad **quads, int count, int row, int dir);
243 static int icvAddOuterQuad(CvCBQuad *quad, CvCBQuad **quads, int quad_count,
244 CvCBQuad **all_quads, int all_count, CvCBCorner **corners, int max_quad_buf_size);
246 static void icvRemoveQuadFromGroup(CvCBQuad **quads, int count, CvCBQuad *q0);
248 static int icvCheckBoardMonotony( CvPoint2D32f* corners, CvSize pattern_size );
250 /***************************************************************************************************/
251 //COMPUTE INTENSITY HISTOGRAM OF INPUT IMAGE
252 static int icvGetIntensityHistogram( const Mat & img, std::vector<int>& piHist )
254 // sum up all pixel in row direction and divide by number of columns
255 for ( int j=0; j<img.rows; j++ )
257 const uchar * row = img.ptr(j);
258 for ( int i=0; i<img.cols; i++ )
265 /***************************************************************************************************/
266 //SMOOTH HISTOGRAM USING WINDOW OF SIZE 2*iWidth+1
267 static int icvSmoothHistogram( const std::vector<int>& piHist, std::vector<int>& piHistSmooth, int iWidth )
270 for ( int i=0; i<256; i++)
273 for ( int ii=-iWidth; ii<=iWidth; ii++)
276 if (iIdx >= 0 && iIdx < 256)
278 iSmooth += piHist[iIdx];
281 piHistSmooth[i] = iSmooth/(2*iWidth+1);
285 /***************************************************************************************************/
286 //COMPUTE FAST HISTOGRAM GRADIENT
287 static int icvGradientOfHistogram( const std::vector<int>& piHist, std::vector<int>& piHistGrad )
290 for ( int i=1; i<255; i++)
292 piHistGrad[i] = piHist[i-1] - piHist[i+1];
293 if ( abs(piHistGrad[i]) < 100 )
295 if ( piHistGrad[i-1] == 0)
296 piHistGrad[i] = -100;
298 piHistGrad[i] = piHistGrad[i-1];
303 /***************************************************************************************************/
304 //PERFORM SMART IMAGE THRESHOLDING BASED ON ANALYSIS OF INTENSTY HISTOGRAM
305 static bool icvBinarizationHistogramBased( Mat & img )
307 CV_Assert(img.channels() == 1 && img.depth() == CV_8U);
308 int iCols = img.cols;
309 int iRows = img.rows;
310 int iMaxPix = iCols*iRows;
311 int iMaxPix1 = iMaxPix/100;
312 const int iNumBins = 256;
313 std::vector<int> piHistIntensity(iNumBins, 0);
314 std::vector<int> piHistSmooth(iNumBins, 0);
315 std::vector<int> piHistGrad(iNumBins, 0);
316 std::vector<int> piAccumSum(iNumBins, 0);
317 std::vector<int> piMaxPos; piMaxPos.reserve(20);
322 icvGetIntensityHistogram( img, piHistIntensity );
324 // get accumulated sum starting from bright
325 piAccumSum[iNumBins-1] = piHistIntensity[iNumBins-1];
326 for ( int i=iNumBins-2; i>=0; i-- )
328 piAccumSum[i] = piHistIntensity[i] + piAccumSum[i+1];
331 // first smooth the distribution
332 icvSmoothHistogram( piHistIntensity, piHistSmooth, iWidth );
335 icvGradientOfHistogram( piHistSmooth, piHistGrad );
339 for ( int i=iNumBins-2; (i>2) && (iCntMaxima<20); i--)
341 if ( (piHistGrad[i-1] < 0) && (piHistGrad[i] > 0) )
343 piMaxPos.push_back(i);
349 int iSumAroundMax = 0;
350 for ( int i=0; i<iCntMaxima; i++ )
353 iSumAroundMax = piHistSmooth[iIdx-1] + piHistSmooth[iIdx] + piHistSmooth[iIdx+1];
354 if ( iSumAroundMax < iMaxPix1 && iIdx < 64 )
356 piMaxPos.erase(piMaxPos.begin() + i);
362 CV_Assert((size_t)iCntMaxima == piMaxPos.size());
364 PRINTF("HIST: MAXIMA COUNT: %d (%d, %d, %d, ...)\n", iCntMaxima,
365 iCntMaxima > 0 ? piMaxPos[0] : -1,
366 iCntMaxima > 1 ? piMaxPos[1] : -1,
367 iCntMaxima > 2 ? piMaxPos[2] : -1);
371 // no any maxima inside (except 0 and 255 which are not handled above)
372 // Does image black-write already?
373 const int iMaxPix2 = iMaxPix / 2;
374 for (int sum = 0, i = 0; i < 256; ++i) // select mean intensity
376 sum += piHistIntensity[i];
384 else if (iCntMaxima == 1)
386 iThresh = piMaxPos[0]/2;
388 else if ( iCntMaxima == 2)
390 iThresh = (piMaxPos[0] + piMaxPos[1])/2;
392 else // iCntMaxima >= 3
394 // CHECKING THRESHOLD FOR WHITE
395 int iIdxAccSum = 0, iAccum = 0;
396 for (int i=iNumBins-1; i>0; i--)
398 iAccum += piHistIntensity[i];
399 // iMaxPix/18 is about 5,5%, minimum required number of pixels required for white part of chessboard
400 if ( iAccum > (iMaxPix/18) )
408 int iBrightMax = piMaxPos[0];
409 // printf("iBrightMax = %d\n", iBrightMax);
410 for ( int n=0; n<iCntMaxima-1; n++)
413 if ( piMaxPos[n] < iIdxAccSum )
417 iBrightMax = piMaxPos[n];
420 // CHECKING THRESHOLD FOR BLACK
421 int iMaxVal = piHistIntensity[piMaxPos[iIdxBGMax]];
423 //IF TOO CLOSE TO 255, jump to next maximum
424 if ( piMaxPos[iIdxBGMax] >= 250 && iIdxBGMax + 1 < iCntMaxima )
427 iMaxVal = piHistIntensity[piMaxPos[iIdxBGMax]];
430 for ( int n=iIdxBGMax + 1; n<iCntMaxima; n++)
432 if ( piHistIntensity[piMaxPos[n]] >= iMaxVal )
434 iMaxVal = piHistIntensity[piMaxPos[n]];
439 //SETTING THRESHOLD FOR BINARIZATION
440 int iDist2 = (iBrightMax - piMaxPos[iIdxBGMax])/2;
441 iThresh = iBrightMax - iDist2;
442 PRINTF("THRESHOLD SELECTED = %d, BRIGHTMAX = %d, DARKMAX = %d\n", iThresh, iBrightMax, piMaxPos[iIdxBGMax]);
448 for ( int jj=0; jj<iRows; jj++)
450 uchar * row = img.ptr(jj);
451 for ( int ii=0; ii<iCols; ii++)
453 if ( row[ii] < iThresh )
465 int cvFindChessboardCorners( const void* arr, CvSize pattern_size,
466 CvPoint2D32f* out_corners, int* out_corner_count,
471 CvCBCorner *corners = 0;
473 cv::Ptr<CvMemStorage> storage;
478 const int min_dilations = 0;
479 const int max_dilations = 7;
481 if( out_corner_count )
482 *out_corner_count = 0;
484 Mat img = cvarrToMat((CvMat*)arr).clone();
486 if( img.depth() != CV_8U || (img.channels() != 1 && img.channels() != 3 && img.channels() != 4) )
487 CV_Error( CV_StsUnsupportedFormat, "Only 8-bit grayscale or color images are supported" );
489 if( pattern_size.width <= 2 || pattern_size.height <= 2 )
490 CV_Error( CV_StsOutOfRange, "Both width and height of the pattern should have bigger than 2" );
493 CV_Error( CV_StsNullPtr, "Null pointer to corners" );
495 if (img.channels() != 1)
497 cvtColor(img, img, COLOR_BGR2GRAY);
501 Mat thresh_img_new = img.clone();
502 icvBinarizationHistogramBased( thresh_img_new ); // process image in-place
503 SHOW("New binarization", thresh_img_new);
505 if( flags & CV_CALIB_CB_FAST_CHECK)
507 //perform new method for checking chessboard using a binary image.
508 //image is binarised using a threshold dependent on the image histogram
509 if (checkChessboardBinary(thresh_img_new, pattern_size) <= 0) //fall back to the old method
511 if (checkChessboard(img, pattern_size) <= 0)
518 storage.reset(cvCreateMemStorage(0));
520 int prev_sqr_size = 0;
522 // Try our standard "1" dilation, but if the pattern is not found, iterate the whole procedure with higher dilations.
523 // This is necessary because some squares simply do not separate properly with a single dilation. However,
524 // we want to use the minimum number of dilations possible since dilations cause the squares to become smaller,
525 // making it difficult to detect smaller squares.
526 for( int dilations = min_dilations; dilations <= max_dilations; dilations++ )
529 break; // already found it
531 //USE BINARY IMAGE COMPUTED USING icvBinarizationHistogramBased METHOD
532 dilate( thresh_img_new, thresh_img_new, Mat(), Point(-1, -1), 1 );
534 // So we can find rectangles that go to the edge, we draw a white line around the image edge.
535 // Otherwise FindContours will miss those clipped rectangle contours.
536 // The border color will be the image mean, because otherwise we risk screwing up filters like cvSmooth()...
537 rectangle( thresh_img_new, Point(0,0), Point(thresh_img_new.cols-1, thresh_img_new.rows-1), Scalar(255,255,255), 3, LINE_8);
538 int max_quad_buf_size = 0;
541 #ifdef USE_CV_FINDCONTOURS
542 Mat binarized_img = thresh_img_new;
544 Mat binarized_img = thresh_img_new.clone(); // make clone because cvFindContours modifies the source image
546 int quad_count = icvGenerateQuads( &quads, &corners, storage, binarized_img, flags, &max_quad_buf_size );
547 PRINTF("Quad count: %d/%d\n", quad_count, (pattern_size.width/2+1)*(pattern_size.height/2+1));
548 SHOW_QUADS("New quads", thresh_img_new, quads, quad_count);
549 if (processQuads(quads, quad_count, pattern_size, max_quad_buf_size, storage, corners, out_corners, out_corner_count, prev_sqr_size))
553 PRINTF("Chessboard detection result 0: %d\n", found);
555 // revert to old, slower, method if detection failed
558 if( flags & CV_CALIB_CB_NORMALIZE_IMAGE )
560 equalizeHist( img, img );
566 PRINTF("Fallback to old algorithm\n");
567 const bool useAdaptive = flags & CV_CALIB_CB_ADAPTIVE_THRESH;
570 // empiric threshold level
571 // thresholding performed here and not inside the cycle to save processing time
572 double mean = cv::mean(img).val[0];
573 int thresh_level = MAX(cvRound( mean - 10 ), 10);
574 threshold( img, thresh_img, thresh_level, 255, THRESH_BINARY );
576 //if flag CV_CALIB_CB_ADAPTIVE_THRESH is not set it doesn't make sense to iterate over k
577 int max_k = useAdaptive ? 6 : 1;
578 for( k = 0; k < max_k; k++ )
580 for( int dilations = min_dilations; dilations <= max_dilations; dilations++ )
583 break; // already found it
585 // convert the input grayscale image to binary (black-n-white)
588 int block_size = cvRound(prev_sqr_size == 0
589 ? MIN(img.cols, img.rows) * (k % 2 == 0 ? 0.2 : 0.1)
590 : prev_sqr_size * 2);
591 block_size = block_size | 1;
593 adaptiveThreshold( img, thresh_img, 255, ADAPTIVE_THRESH_MEAN_C, THRESH_BINARY, block_size, (k/2)*5 );
595 dilate( thresh_img, thresh_img, Mat(), Point(-1, -1), dilations-1 );
600 dilate( thresh_img, thresh_img, Mat(), Point(-1, -1), 1 );
602 SHOW("Old binarization", thresh_img);
604 // So we can find rectangles that go to the edge, we draw a white line around the image edge.
605 // Otherwise FindContours will miss those clipped rectangle contours.
606 // The border color will be the image mean, because otherwise we risk screwing up filters like cvSmooth()...
607 rectangle( thresh_img, Point(0,0), Point(thresh_img.cols-1, thresh_img.rows-1), Scalar(255,255,255), 3, LINE_8);
608 int max_quad_buf_size = 0;
611 #ifdef USE_CV_FINDCONTOURS
612 Mat binarized_img = thresh_img;
614 Mat binarized_img = (useAdaptive) ? thresh_img : thresh_img.clone(); // make clone because cvFindContours modifies the source image
616 int quad_count = icvGenerateQuads( &quads, &corners, storage, binarized_img, flags, &max_quad_buf_size);
617 PRINTF("Quad count: %d/%d\n", quad_count, (pattern_size.width/2+1)*(pattern_size.height/2+1));
618 SHOW_QUADS("Old quads", thresh_img, quads, quad_count);
619 if (processQuads(quads, quad_count, pattern_size, max_quad_buf_size, storage, corners, out_corners, out_corner_count, prev_sqr_size))
625 PRINTF("Chessboard detection result 1: %d\n", found);
628 found = icvCheckBoardMonotony( out_corners, pattern_size );
630 PRINTF("Chessboard detection result 2: %d\n", found);
632 // check that none of the found corners is too close to the image boundary
635 const int BORDER = 8;
636 for( k = 0; k < pattern_size.width*pattern_size.height; k++ )
638 if( out_corners[k].x <= BORDER || out_corners[k].x > img.cols - BORDER ||
639 out_corners[k].y <= BORDER || out_corners[k].y > img.rows - BORDER )
643 found = k == pattern_size.width*pattern_size.height;
646 PRINTF("Chessboard detection result 3: %d\n", found);
650 if ( pattern_size.height % 2 == 0 && pattern_size.width % 2 == 0 )
652 int last_row = (pattern_size.height-1)*pattern_size.width;
653 double dy0 = out_corners[last_row].y - out_corners[0].y;
656 int n = pattern_size.width*pattern_size.height;
657 for(int i = 0; i < n/2; i++ )
660 CV_SWAP(out_corners[i], out_corners[n-i-1], temp);
666 cvFindCornerSubPix( &old_img, out_corners, pattern_size.width*pattern_size.height,
667 cvSize(wsize, wsize), cvSize(-1,-1),
668 cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 15, 0.1));
683 // Checks that each board row and column is pretty much monotonous curve:
684 // It analyzes each row and each column of the chessboard as following:
685 // for each corner c lying between end points in the same row/column it checks that
686 // the point projection to the line segment (a,b) is lying between projections
687 // of the neighbor corners in the same row/column.
689 // This function has been created as temporary workaround for the bug in current implementation
690 // of cvFindChessboardCornes that produces absolutely unordered sets of corners.
694 icvCheckBoardMonotony( CvPoint2D32f* corners, CvSize pattern_size )
698 for( k = 0; k < 2; k++ )
700 for( i = 0; i < (k == 0 ? pattern_size.height : pattern_size.width); i++ )
702 CvPoint2D32f a = k == 0 ? corners[i*pattern_size.width] : corners[i];
703 CvPoint2D32f b = k == 0 ? corners[(i+1)*pattern_size.width-1] :
704 corners[(pattern_size.height-1)*pattern_size.width + i];
705 float prevt = 0, dx0 = b.x - a.x, dy0 = b.y - a.y;
706 if( fabs(dx0) + fabs(dy0) < FLT_EPSILON )
708 for( j = 1; j < (k == 0 ? pattern_size.width : pattern_size.height) - 1; j++ )
710 CvPoint2D32f c = k == 0 ? corners[i*pattern_size.width + j] :
711 corners[j*pattern_size.width + i];
712 float t = ((c.x - a.x)*dx0 + (c.y - a.y)*dy0)/(dx0*dx0 + dy0*dy0);
713 if( t < prevt || t > 1 )
724 // order a group of connected quads
727 // clockwise from there
728 // note: "top left" is nominal, depends on initial ordering of starting quad
729 // but all other quads are ordered consistently
731 // can change the number of quads in the group
732 // can add quads, so we need to have quad/corner arrays passed in
736 icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,
737 int *all_count, CvCBQuad **all_quads, CvCBCorner **corners,
738 CvSize pattern_size, int max_quad_buf_size, CvMemStorage* storage )
740 cv::Ptr<CvMemStorage> temp_storage(cvCreateChildMemStorage( storage ));
741 CvSeq* stack = cvCreateSeq( 0, sizeof(*stack), sizeof(void*), temp_storage );
743 // first find an interior quad
744 CvCBQuad *start = NULL;
745 for (int i=0; i<quad_count; i++)
747 if (quads[i]->count == 4)
755 return 0; // no 4-connected quad
757 // start with first one, assign rows/cols
758 int row_min = 0, col_min = 0, row_max=0, col_max = 0;
760 std::map<int, int> col_hist;
761 std::map<int, int> row_hist;
763 cvSeqPush(stack, &start);
766 start->ordered = true;
768 // Recursively order the quads so that all position numbers (e.g.,
769 // 0,1,2,3) are in the at the same relative corner (e.g., lower right).
771 while( stack->total )
774 cvSeqPop( stack, &q );
781 if (row > row_max) row_max = row;
782 if (row < row_min) row_min = row;
783 if (col > col_max) col_max = col;
784 if (col < col_min) col_min = col;
786 for(int i = 0; i < 4; i++ )
788 CvCBQuad *neighbor = q->neighbors[i];
789 switch(i) // adjust col, row for this quad
790 { // start at top left, go clockwise
801 // just do inside quads
802 if (neighbor && neighbor->ordered == false && neighbor->count == 4)
804 PRINTF("col: %d row: %d\n", col, row);
805 icvOrderQuad(neighbor, q->corners[i], (i+2)%4); // set in order
806 neighbor->ordered = true;
809 cvSeqPush( stack, &neighbor );
814 for (int i=col_min; i<=col_max; i++)
815 PRINTF("HIST[%d] = %d\n", i, col_hist[i]);
817 // analyze inner quad structure
818 int w = pattern_size.width - 1;
819 int h = pattern_size.height - 1;
820 int drow = row_max - row_min + 1;
821 int dcol = col_max - col_min + 1;
823 // normalize pattern and found quad indices
824 if ((w > h && dcol < drow) ||
825 (w < h && drow < dcol))
827 h = pattern_size.width - 1;
828 w = pattern_size.height - 1;
831 PRINTF("Size: %dx%d Pattern: %dx%d\n", dcol, drow, w, h);
833 // check if there are enough inner quads
834 if (dcol < w || drow < h) // found enough inner quads?
836 PRINTF("Too few inner quad rows/cols\n");
837 return 0; // no, return
839 #ifdef ENABLE_TRIM_COL_ROW
840 // too many columns, not very common
841 if (dcol == w+1) // too many, trim
843 PRINTF("Trimming cols\n");
844 if (col_hist[col_max] > col_hist[col_min])
846 PRINTF("Trimming left col\n");
847 quad_count = icvTrimCol(quads,quad_count,col_min,-1);
851 PRINTF("Trimming right col\n");
852 quad_count = icvTrimCol(quads,quad_count,col_max,+1);
856 // too many rows, not very common
857 if (drow == h+1) // too many, trim
859 PRINTF("Trimming rows\n");
860 if (row_hist[row_max] > row_hist[row_min])
862 PRINTF("Trimming top row\n");
863 quad_count = icvTrimRow(quads,quad_count,row_min,-1);
867 PRINTF("Trimming bottom row\n");
868 quad_count = icvTrimRow(quads,quad_count,row_max,+1);
873 // check edges of inner quads
874 // if there is an outer quad missing, fill it in
875 // first order all inner quads
877 for (int i=0; i<quad_count; i++)
879 if (quads[i]->count == 4)
880 { // ok, look at neighbors
881 int col = quads[i]->col;
882 int row = quads[i]->row;
883 for (int j=0; j<4; j++)
885 switch(j) // adjust col, row for this quad
886 { // start at top left, go clockwise
896 CvCBQuad *neighbor = quads[i]->neighbors[j];
897 if (neighbor && !neighbor->ordered && // is it an inner quad?
898 col <= col_max && col >= col_min &&
899 row <= row_max && row >= row_min)
901 // if so, set in order
902 PRINTF("Adding inner: col: %d row: %d\n", col, row);
904 icvOrderQuad(neighbor, quads[i]->corners[j], (j+2)%4);
905 neighbor->ordered = true;
913 // if we have found inner quads, add corresponding outer quads,
917 PRINTF("Found %d inner quads not connected to outer quads, repairing\n", found);
918 for (int i=0; i<quad_count && *all_count < max_quad_buf_size; i++)
920 if (quads[i]->count < 4 && quads[i]->ordered)
922 int added = icvAddOuterQuad(quads[i],quads,quad_count,all_quads,*all_count,corners, max_quad_buf_size);
928 if (*all_count >= max_quad_buf_size)
933 // final trimming of outer quads
934 if (dcol == w && drow == h) // found correct inner quads
936 PRINTF("Inner bounds ok, check outer quads\n");
937 int rcount = quad_count;
938 for (int i=quad_count-1; i>=0; i--) // eliminate any quad not connected to
941 if (quads[i]->ordered == false)
944 for (int j=0; j<4; j++) // any neighbors that are ordered?
946 if (quads[i]->neighbors[j] && quads[i]->neighbors[j]->ordered)
949 if (!outer) // not an outer quad, eliminate
951 PRINTF("Removing quad %d\n", i);
952 icvRemoveQuadFromGroup(quads,rcount,quads[i]);
966 // looks for the neighbor of <quad> that isn't present,
967 // tries to add it in.
971 icvAddOuterQuad( CvCBQuad *quad, CvCBQuad **quads, int quad_count,
972 CvCBQuad **all_quads, int all_count, CvCBCorner **corners, int max_quad_buf_size )
976 for (int i=0; i<4 && all_count < max_quad_buf_size; i++) // find no-neighbor corners
978 if (!quad->neighbors[i]) // ok, create and add neighbor
981 PRINTF("Adding quad as neighbor 2\n");
982 CvCBQuad *q = &(*all_quads)[all_count];
983 memset( q, 0, sizeof(*q) );
985 quads[quad_count] = q;
988 // set neighbor and group id
989 quad->neighbors[i] = q;
991 q->neighbors[j] = quad;
992 q->group_idx = quad->group_idx;
993 q->count = 1; // number of neighbors
995 q->edge_len = quad->edge_len;
997 // make corners of new quad
998 // same as neighbor quad, but offset
999 CvPoint2D32f pt = quad->corners[i]->pt;
1001 float dx = pt.x - quad->corners[j]->pt.x;
1002 float dy = pt.y - quad->corners[j]->pt.y;
1003 for (int k=0; k<4; k++)
1005 corner = &(*corners)[all_count*4+k];
1006 pt = quad->corners[k]->pt;
1007 memset( corner, 0, sizeof(*corner) );
1009 q->corners[k] = corner;
1013 // have to set exact corner
1014 q->corners[j] = quad->corners[i];
1016 // now find other neighbor and add it, if possible
1017 if (quad->neighbors[(i+3)%4] &&
1018 quad->neighbors[(i+3)%4]->ordered &&
1019 quad->neighbors[(i+3)%4]->neighbors[i] &&
1020 quad->neighbors[(i+3)%4]->neighbors[i]->ordered )
1022 CvCBQuad *qn = quad->neighbors[(i+3)%4]->neighbors[i];
1024 q->neighbors[(j+1)%4] = qn;
1025 qn->neighbors[(i+1)%4] = q;
1027 // have to set exact corner
1028 q->corners[(j+1)%4] = qn->corners[(i+1)%4];
1038 // trimming routines
1039 #ifdef ENABLE_TRIM_COL_ROW
1041 icvTrimCol(CvCBQuad **quads, int count, int col, int dir)
1044 // find the right quad(s)
1045 for (int i=0; i<count; i++)
1047 #ifdef DEBUG_CHESSBOARD
1048 if (quads[i]->ordered)
1049 PRINTF("index: %d cur: %d\n", col, quads[i]->col);
1051 if (quads[i]->ordered && quads[i]->col == col)
1055 if (quads[i]->neighbors[1])
1057 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[1]);
1060 if (quads[i]->neighbors[2])
1062 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[2]);
1068 if (quads[i]->neighbors[0])
1070 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[0]);
1073 if (quads[i]->neighbors[3])
1075 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[3]);
1086 icvTrimRow(CvCBQuad **quads, int count, int row, int dir)
1088 int i, rcount = count;
1089 // find the right quad(s)
1090 for (i=0; i<count; i++)
1092 #ifdef DEBUG_CHESSBOARD
1093 if (quads[i]->ordered)
1094 PRINTF("index: %d cur: %d\n", row, quads[i]->row);
1096 if (quads[i]->ordered && quads[i]->row == row)
1098 if (dir == 1) // remove from bottom
1100 if (quads[i]->neighbors[2])
1102 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[2]);
1105 if (quads[i]->neighbors[3])
1107 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[3]);
1111 else // remove from top
1113 if (quads[i]->neighbors[0])
1115 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[0]);
1118 if (quads[i]->neighbors[1])
1120 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[1]);
1132 // remove quad from quad group
1136 icvRemoveQuadFromGroup(CvCBQuad **quads, int count, CvCBQuad *q0)
1139 // remove any references to this quad as a neighbor
1140 for(i = 0; i < count; i++ )
1142 CvCBQuad *q = quads[i];
1143 for(j = 0; j < 4; j++ )
1145 if( q->neighbors[j] == q0 )
1147 q->neighbors[j] = 0;
1149 for(int k = 0; k < 4; k++ )
1150 if( q0->neighbors[k] == q )
1152 q0->neighbors[k] = 0;
1162 for(i = 0; i < count; i++ )
1164 CvCBQuad *q = quads[i];
1167 quads[i] = quads[count-1];
1174 // put quad into correct order, where <corner> has value <common>
1178 icvOrderQuad(CvCBQuad *quad, CvCBCorner *corner, int common)
1182 for (tc=0; tc<4; tc++)
1183 if (quad->corners[tc]->pt.x == corner->pt.x &&
1184 quad->corners[tc]->pt.y == corner->pt.y)
1189 while (tc != common)
1194 tempc = quad->corners[3];
1195 tempq = quad->neighbors[3];
1196 for (int i=3; i>0; i--)
1198 quad->corners[i] = quad->corners[i-1];
1199 quad->neighbors[i] = quad->neighbors[i-1];
1201 quad->corners[0] = tempc;
1202 quad->neighbors[0] = tempq;
1209 // if we found too many connect quads, remove those which probably do not belong.
1211 icvCleanFoundConnectedQuads( int quad_count, CvCBQuad **quad_group, CvSize pattern_size )
1213 CvPoint2D32f center;
1215 // number of quads this pattern should contain
1216 int count = ((pattern_size.width + 1)*(pattern_size.height + 1) + 1)/2;
1218 // if we have more quadrangles than we should,
1219 // try to eliminate duplicates or ones which don't belong to the pattern rectangle...
1220 if( quad_count <= count )
1223 // create an array of quadrangle centers
1224 cv::AutoBuffer<CvPoint2D32f> centers( quad_count );
1225 cv::Ptr<CvMemStorage> temp_storage(cvCreateMemStorage(0));
1227 for( i = 0; i < quad_count; i++ )
1230 CvCBQuad* q = quad_group[i];
1232 for( j = 0; j < 4; j++ )
1234 CvPoint2D32f pt = q->corners[j]->pt;
1246 center.x /= quad_count;
1247 center.y /= quad_count;
1249 // If we still have more quadrangles than we should,
1250 // we try to eliminate bad ones based on minimizing the bounding box.
1251 // We iteratively remove the point which reduces the size of
1252 // the bounding box of the blobs the most
1253 // (since we want the rectangle to be as small as possible)
1254 // remove the quadrange that causes the biggest reduction
1255 // in pattern size until we have the correct number
1256 for( ; quad_count > count; quad_count-- )
1258 double min_box_area = DBL_MAX;
1259 int skip, min_box_area_index = -1;
1261 // For each point, calculate box area without that point
1262 for( skip = 0; skip < quad_count; skip++ )
1264 // get bounding rectangle
1265 CvPoint2D32f temp = centers[skip]; // temporarily make index 'skip' the same as
1266 centers[skip] = center; // pattern center (so it is not counted for convex hull)
1267 CvMat pointMat = cvMat(1, quad_count, CV_32FC2, centers);
1268 CvSeq *hull = cvConvexHull2( &pointMat, temp_storage, CV_CLOCKWISE, 1 );
1269 centers[skip] = temp;
1270 double hull_area = fabs(cvContourArea(hull, CV_WHOLE_SEQ));
1272 // remember smallest box area
1273 if( hull_area < min_box_area )
1275 min_box_area = hull_area;
1276 min_box_area_index = skip;
1278 cvClearMemStorage( temp_storage );
1281 CvCBQuad *q0 = quad_group[min_box_area_index];
1283 // remove any references to this quad as a neighbor
1284 for( i = 0; i < quad_count; i++ )
1286 CvCBQuad *q = quad_group[i];
1287 for( j = 0; j < 4; j++ )
1289 if( q->neighbors[j] == q0 )
1291 q->neighbors[j] = 0;
1293 for( k = 0; k < 4; k++ )
1294 if( q0->neighbors[k] == q )
1296 q0->neighbors[k] = 0;
1307 quad_group[min_box_area_index] = quad_group[quad_count];
1308 centers[min_box_area_index] = centers[quad_count];
1314 //=====================================================================================
1317 icvFindConnectedQuads( CvCBQuad *quad, int quad_count, CvCBQuad **out_group,
1318 int group_idx, CvMemStorage* storage )
1320 cv::Ptr<CvMemStorage> temp_storage(cvCreateChildMemStorage( storage ));
1321 CvSeq* stack = cvCreateSeq( 0, sizeof(*stack), sizeof(void*), temp_storage );
1324 // Scan the array for a first unlabeled quad
1325 for( i = 0; i < quad_count; i++ )
1327 if( quad[i].count > 0 && quad[i].group_idx < 0)
1331 // Recursively find a group of connected quads starting from the seed quad[i]
1332 if( i < quad_count )
1334 CvCBQuad* q = &quad[i];
1335 cvSeqPush( stack, &q );
1336 out_group[count++] = q;
1337 q->group_idx = group_idx;
1340 while( stack->total )
1342 cvSeqPop( stack, &q );
1343 for( i = 0; i < 4; i++ )
1345 CvCBQuad *neighbor = q->neighbors[i];
1346 if( neighbor && neighbor->count > 0 && neighbor->group_idx < 0 )
1348 cvSeqPush( stack, &neighbor );
1349 out_group[count++] = neighbor;
1350 neighbor->group_idx = group_idx;
1351 neighbor->ordered = false;
1361 //=====================================================================================
1364 icvCheckQuadGroup( CvCBQuad **quad_group, int quad_count,
1365 CvCBCorner **out_corners, CvSize pattern_size )
1367 const int ROW1 = 1000000;
1368 const int ROW2 = 2000000;
1369 const int ROW_ = 3000000;
1371 int i, out_corner_count = 0, corner_count = 0;
1372 std::vector<CvCBCorner*> corners(quad_count*4);
1375 int width = 0, height = 0;
1376 int hist[5] = {0,0,0,0,0};
1377 CvCBCorner* first = 0, *first2 = 0, *right, *cur, *below, *c;
1379 // build dual graph, which vertices are internal quad corners
1380 // and two vertices are connected iff they lie on the same quad edge
1381 for( i = 0; i < quad_count; i++ )
1383 CvCBQuad* q = quad_group[i];
1384 /*CvScalar color = q->count == 0 ? cvScalar(0,255,255) :
1385 q->count == 1 ? cvScalar(0,0,255) :
1386 q->count == 2 ? cvScalar(0,255,0) :
1387 q->count == 3 ? cvScalar(255,255,0) :
1388 cvScalar(255,0,0);*/
1390 for( j = 0; j < 4; j++ )
1392 //cvLine( debug_img, cvPointFrom32f(q->corners[j]->pt), cvPointFrom32f(q->corners[(j+1)&3]->pt), color, 1, CV_AA, 0 );
1393 if( q->neighbors[j] )
1395 CvCBCorner *a = q->corners[j], *b = q->corners[(j+1)&3];
1396 // mark internal corners that belong to:
1397 // - a quad with a single neighbor - with ROW1,
1398 // - a quad with two neighbors - with ROW2
1399 // make the rest of internal corners with ROW_
1400 int row_flag = q->count == 1 ? ROW1 : q->count == 2 ? ROW2 : ROW_;
1404 corners[corner_count++] = a;
1407 else if( a->row > row_flag )
1410 if( q->neighbors[(j+1)&3] )
1412 if( a->count >= 4 || b->count >= 4 )
1414 for( k = 0; k < 4; k++ )
1416 if( a->neighbors[k] == b )
1418 if( b->neighbors[k] == a )
1421 a->neighbors[a->count++] = b;
1422 b->neighbors[b->count++] = a;
1428 if( corner_count != pattern_size.width*pattern_size.height )
1431 for( i = 0; i < corner_count; i++ )
1433 int n = corners[i]->count;
1434 assert( 0 <= n && n <= 4 );
1436 if( !first && n == 2 )
1438 if( corners[i]->row == ROW1 )
1440 else if( !first2 && corners[i]->row == ROW2 )
1441 first2 = corners[i];
1445 // start with a corner that belongs to a quad with a single neighbor.
1446 // if we do not have such, start with a corner of a quad with two neighbors.
1450 if( !first || hist[0] != 0 || hist[1] != 0 || hist[2] != 4 ||
1451 hist[3] != (pattern_size.width + pattern_size.height)*2 - 8 )
1456 out_corners[out_corner_count++] = cur;
1458 for( k = 0; k < 4; k++ )
1460 c = cur->neighbors[k];
1470 if( !right || (right->count != 2 && right->count != 3) ||
1471 !below || (below->count != 2 && below->count != 3) )
1475 //cvCircle( debug_img, cvPointFrom32f(cur->pt), 3, cvScalar(0,255,0), -1, 8, 0 );
1477 first = below; // remember the first corner in the next row
1478 // find and store the first row (or column)
1482 out_corners[out_corner_count++] = right;
1483 //cvCircle( debug_img, cvPointFrom32f(right->pt), 3, cvScalar(0,255-j*10,0), -1, 8, 0 );
1484 if( right->count == 2 )
1486 if( right->count != 3 || out_corner_count >= MAX(pattern_size.width,pattern_size.height) )
1489 for( k = 0; k < 4; k++ )
1491 c = cur->neighbors[k];
1492 if( c && c->row > 0 )
1494 for( kk = 0; kk < 4; kk++ )
1496 if( c->neighbors[kk] == below )
1507 width = out_corner_count;
1508 if( width == pattern_size.width )
1509 height = pattern_size.height;
1510 else if( width == pattern_size.height )
1511 height = pattern_size.width;
1515 // find and store all the other rows
1525 out_corners[out_corner_count++] = cur;
1526 //cvCircle( debug_img, cvPointFrom32f(cur->pt), 3, cvScalar(0,0,255-j*10), -1, 8, 0 );
1527 if( cur->count == 2 + (i < height-1) && j > 0 )
1532 // find a neighbor that has not been processed yet
1533 // and that has a neighbor from the previous row
1534 for( k = 0; k < 4; k++ )
1536 c = cur->neighbors[k];
1537 if( c && c->row > i )
1539 for( kk = 0; kk < 4; kk++ )
1541 if( c->neighbors[kk] && c->neighbors[kk]->row == i-1 )
1559 if( j != width - 1 )
1563 if( out_corner_count != corner_count )
1566 // check if we need to transpose the board
1567 if( width != pattern_size.width )
1569 CV_SWAP( width, height, k );
1571 memcpy( &corners[0], out_corners, corner_count*sizeof(corners[0]) );
1572 for( i = 0; i < height; i++ )
1573 for( j = 0; j < width; j++ )
1574 out_corners[i*width + j] = corners[j*height + i];
1577 // check if we need to revert the order in each row
1579 CvPoint2D32f p0 = out_corners[0]->pt, p1 = out_corners[pattern_size.width-1]->pt,
1580 p2 = out_corners[pattern_size.width]->pt;
1581 if( (p1.x - p0.x)*(p2.y - p1.y) - (p1.y - p0.y)*(p2.x - p1.x) < 0 )
1583 if( width % 2 == 0 )
1585 for( i = 0; i < height; i++ )
1586 for( j = 0; j < width/2; j++ )
1587 CV_SWAP( out_corners[i*width+j], out_corners[i*width+width-j-1], c );
1591 for( j = 0; j < width; j++ )
1592 for( i = 0; i < height/2; i++ )
1593 CV_SWAP( out_corners[i*width+j], out_corners[(height - i - 1)*width+j], c );
1598 result = corner_count;
1604 corner_count = MIN( corner_count, pattern_size.width*pattern_size.height );
1605 for( i = 0; i < corner_count; i++ )
1606 out_corners[i] = corners[i];
1607 result = -corner_count;
1609 if (result == -pattern_size.width*pattern_size.height)
1619 //=====================================================================================
1621 static void icvFindQuadNeighbors( CvCBQuad *quads, int quad_count )
1623 const float thresh_scale = 1.f;
1627 // find quad neighbors
1628 for( idx = 0; idx < quad_count; idx++ )
1630 CvCBQuad* cur_quad = &quads[idx];
1632 // choose the points of the current quadrangle that are close to
1633 // some points of the other quadrangles
1634 // (it can happen for split corners (due to dilation) of the
1635 // checker board). Search only in other quadrangles!
1637 // for each corner of this quadrangle
1638 for( i = 0; i < 4; i++ )
1641 float min_dist = FLT_MAX;
1642 int closest_corner_idx = -1;
1643 CvCBQuad *closest_quad = 0;
1644 CvCBCorner *closest_corner = 0;
1646 if( cur_quad->neighbors[i] )
1649 pt = cur_quad->corners[i]->pt;
1651 // find the closest corner in all other quadrangles
1652 for( k = 0; k < quad_count; k++ )
1657 for( j = 0; j < 4; j++ )
1659 if( quads[k].neighbors[j] )
1662 dx = pt.x - quads[k].corners[j]->pt.x;
1663 dy = pt.y - quads[k].corners[j]->pt.y;
1664 dist = dx * dx + dy * dy;
1666 if( dist < min_dist &&
1667 dist <= cur_quad->edge_len*thresh_scale &&
1668 dist <= quads[k].edge_len*thresh_scale )
1670 // check edge lengths, make sure they're compatible
1671 // edges that are different by more than 1:4 are rejected
1672 float ediff = cur_quad->edge_len - quads[k].edge_len;
1673 if (ediff > 32*cur_quad->edge_len ||
1674 ediff > 32*quads[k].edge_len)
1676 PRINTF("Incompatible edge lengths\n");
1679 closest_corner_idx = j;
1680 closest_quad = &quads[k];
1686 // we found a matching corner point?
1687 if( closest_corner_idx >= 0 && min_dist < FLT_MAX )
1689 // If another point from our current quad is closer to the found corner
1690 // than the current one, then we don't count this one after all.
1691 // This is necessary to support small squares where otherwise the wrong
1692 // corner will get matched to closest_quad;
1693 closest_corner = closest_quad->corners[closest_corner_idx];
1695 for( j = 0; j < 4; j++ )
1697 if( cur_quad->neighbors[j] == closest_quad )
1700 dx = closest_corner->pt.x - cur_quad->corners[j]->pt.x;
1701 dy = closest_corner->pt.y - cur_quad->corners[j]->pt.y;
1703 if( dx * dx + dy * dy < min_dist )
1707 if( j < 4 || cur_quad->count >= 4 || closest_quad->count >= 4 )
1710 // Check that each corner is a neighbor of different quads
1711 for( j = 0; j < closest_quad->count; j++ )
1713 if( closest_quad->neighbors[j] == cur_quad )
1716 if( j < closest_quad->count )
1719 // check whether the closest corner to closest_corner
1720 // is different from cur_quad->corners[i]->pt
1721 for( k = 0; k < quad_count; k++ )
1723 CvCBQuad* q = &quads[k];
1724 if( k == idx || q == closest_quad )
1727 for( j = 0; j < 4; j++ )
1728 if( !q->neighbors[j] )
1730 dx = closest_corner->pt.x - q->corners[j]->pt.x;
1731 dy = closest_corner->pt.y - q->corners[j]->pt.y;
1732 dist = dx*dx + dy*dy;
1733 if( dist < min_dist )
1740 if( k < quad_count )
1743 closest_corner->pt.x = (pt.x + closest_corner->pt.x) * 0.5f;
1744 closest_corner->pt.y = (pt.y + closest_corner->pt.y) * 0.5f;
1746 // We've found one more corner - remember it
1748 cur_quad->neighbors[i] = closest_quad;
1749 cur_quad->corners[i] = closest_corner;
1751 closest_quad->count++;
1752 closest_quad->neighbors[closest_corner_idx] = cur_quad;
1758 //=====================================================================================
1760 // returns corners in clockwise order
1761 // corners don't necessarily start at same position on quad (e.g.,
1765 icvGenerateQuads( CvCBQuad **out_quads, CvCBCorner **out_corners,
1766 CvMemStorage *storage, const cv::Mat & image_, int flags, int *max_quad_buf_size )
1769 cv::Ptr<CvMemStorage> temp_storage;
1777 // empiric bound for minimal allowed perimeter for squares
1778 int min_size = 25; //cvRound( image->cols * image->rows * .03 * 0.01 * 0.92 );
1780 bool filterQuads = (flags & CALIB_CB_FILTER_QUADS) != 0;
1781 #ifdef USE_CV_FINDCONTOURS // use cv::findContours
1784 std::vector<std::vector<Point> > contours;
1785 std::vector<Vec4i> hierarchy;
1787 cv::findContours(image_, contours, hierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
1789 if (contours.empty())
1791 CV_LOG_DEBUG(NULL, "calib3d(chessboard): cv::findContours() returns no contours");
1792 *max_quad_buf_size = 0;
1796 std::vector<int> contour_child_counter(contours.size(), 0);
1799 std::vector<QuadCountour> contour_quads;
1801 for (int idx = (int)(contours.size() - 1); idx >= 0; --idx)
1803 int parentIdx = hierarchy[idx][3];
1804 if (hierarchy[idx][2] != -1 || parentIdx == -1) // holes only (no child contours and with parent)
1806 const std::vector<Point>& contour = contours[idx];
1808 Rect contour_rect = boundingRect(contour);
1809 if (contour_rect.area() < min_size)
1812 std::vector<Point> approx_contour;
1814 const int min_approx_level = 1, max_approx_level = MAX_CONTOUR_APPROX;
1815 for (int approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++ )
1817 approxPolyDP(contour, approx_contour, (float)approx_level, true);
1818 if (approx_contour.size() == 4)
1821 // we call this again on its own output, because sometimes
1822 // approxPoly() does not simplify as much as it should.
1823 std::vector<Point> approx_contour_tmp;
1824 std::swap(approx_contour, approx_contour_tmp);
1825 approxPolyDP(approx_contour_tmp, approx_contour, (float)approx_level, true);
1826 if (approx_contour.size() == 4)
1830 // reject non-quadrangles
1831 if (approx_contour.size() != 4)
1833 if (!cv::isContourConvex(approx_contour))
1837 for (int i = 0; i < 4; ++i)
1838 pt[i] = approx_contour[i];
1839 CV_LOG_VERBOSE(NULL, 9, "... contours(" << contour_quads.size() << " added):" << pt[0] << " " << pt[1] << " " << pt[2] << " " << pt[3]);
1843 double p = cv::arcLength(approx_contour, true);
1844 double area = cv::contourArea(approx_contour, false);
1846 double d1 = sqrt(normL2Sqr<double>(pt[0] - pt[2]));
1847 double d2 = sqrt(normL2Sqr<double>(pt[1] - pt[3]));
1849 // philipg. Only accept those quadrangles which are more square
1850 // than rectangular and which are big enough
1851 double d3 = sqrt(normL2Sqr<double>(pt[0] - pt[1]));
1852 double d4 = sqrt(normL2Sqr<double>(pt[1] - pt[2]));
1853 if (!(d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > min_size &&
1854 d1 >= 0.15 * p && d2 >= 0.15 * p))
1858 contour_child_counter[parentIdx]++;
1859 if (boardIdx != parentIdx && (boardIdx < 0 || contour_child_counter[boardIdx] < contour_child_counter[parentIdx]))
1860 boardIdx = parentIdx;
1862 contour_quads.push_back(QuadCountour(pt, parentIdx));
1865 size_t total = contour_quads.size();
1866 *max_quad_buf_size = (int)std::max((size_t)2, total * 3);
1867 *out_quads = (CvCBQuad*)cvAlloc(*max_quad_buf_size * sizeof((*out_quads)[0]));
1868 *out_corners = (CvCBCorner*)cvAlloc(*max_quad_buf_size * 4 * sizeof((*out_corners)[0]));
1870 // Create array of quads structures
1871 for(int idx = 0; idx < (int)contour_quads.size(); idx++ )
1873 CvCBQuad* q = &(*out_quads)[quad_count];
1875 QuadCountour& qc = contour_quads[idx];
1876 if (filterQuads && qc.parent_contour != boardIdx)
1880 memset(q, 0, sizeof(*q));
1882 for (int i = 0; i < 4; ++i)
1884 CvCBCorner* corner = &(*out_corners)[quad_count*4 + i];
1886 memset(corner, 0, sizeof(*corner));
1887 corner->pt = qc.pt[i];
1888 q->corners[i] = corner;
1890 q->edge_len = FLT_MAX;
1891 for (int i = 0; i < 4; ++i)
1893 // TODO simplify with normL2Sqr<float>()
1894 float dx = q->corners[i]->pt.x - q->corners[(i+1)&3]->pt.x;
1895 float dy = q->corners[i]->pt.y - q->corners[(i+1)&3]->pt.y;
1896 float d = dx*dx + dy*dy;
1897 if (q->edge_len > d)
1903 #else // use legacy API: cvStartFindContours / cvFindNextContour / cvEndFindContours
1905 CvMat image_old(image_), *image = &image_old;
1907 CvSeq *src_contour = 0;
1909 CvContourEx* board = 0;
1910 CvContourScanner scanner;
1913 CV_Assert( out_corners && out_quads );
1915 // create temporary storage for contours and the sequence of pointers to found quadrangles
1916 temp_storage.reset(cvCreateChildMemStorage( storage ));
1917 root = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSeq*), temp_storage );
1919 // initialize contour retrieving routine
1920 scanner = cvStartFindContours( image, temp_storage, sizeof(CvContourEx),
1921 CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
1923 // get all the contours one by one
1924 while( (src_contour = cvFindNextContour( scanner )) != 0 )
1926 CvSeq *dst_contour = 0;
1927 CvRect rect = ((CvContour*)src_contour)->rect;
1929 // reject contours with too small perimeter
1930 if( CV_IS_SEQ_HOLE(src_contour) && rect.width*rect.height >= min_size )
1932 const int min_approx_level = 1, max_approx_level = MAX_CONTOUR_APPROX;
1934 for( approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++ )
1936 dst_contour = cvApproxPoly( src_contour, sizeof(CvContour), temp_storage,
1937 CV_POLY_APPROX_DP, (float)approx_level );
1938 if( dst_contour->total == 4 )
1941 // we call this again on its own output, because sometimes
1942 // cvApproxPoly() does not simplify as much as it should.
1943 dst_contour = cvApproxPoly( dst_contour, sizeof(CvContour), temp_storage,
1944 CV_POLY_APPROX_DP, (float)approx_level );
1946 if( dst_contour->total == 4 )
1950 // reject non-quadrangles
1951 if( dst_contour->total == 4 && cvCheckContourConvexity(dst_contour) )
1954 double d1, d2, p = cvContourPerimeter(dst_contour);
1955 double area = fabs(cvContourArea(dst_contour, CV_WHOLE_SEQ));
1958 for( i = 0; i < 4; i++ )
1959 pt[i] = *(CvPoint*)cvGetSeqElem(dst_contour, i);
1961 dx = pt[0].x - pt[2].x;
1962 dy = pt[0].y - pt[2].y;
1963 d1 = sqrt(dx*dx + dy*dy);
1965 dx = pt[1].x - pt[3].x;
1966 dy = pt[1].y - pt[3].y;
1967 d2 = sqrt(dx*dx + dy*dy);
1969 // philipg. Only accept those quadrangles which are more square
1970 // than rectangular and which are big enough
1972 dx = pt[0].x - pt[1].x;
1973 dy = pt[0].y - pt[1].y;
1974 d3 = sqrt(dx*dx + dy*dy);
1975 dx = pt[1].x - pt[2].x;
1976 dy = pt[1].y - pt[2].y;
1977 d4 = sqrt(dx*dx + dy*dy);
1979 (d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > min_size &&
1980 d1 >= 0.15 * p && d2 >= 0.15 * p))
1982 CvContourEx* parent = (CvContourEx*)(src_contour->v_prev);
1984 if( !board || board->counter < parent->counter )
1986 dst_contour->v_prev = (CvSeq*)parent;
1987 //for( i = 0; i < 4; i++ ) cvLine( debug_img, pt[i], pt[(i+1)&3], cvScalar(200,255,255), 1, CV_AA, 0 );
1988 cvSeqPush( root, &dst_contour );
1994 // finish contour retrieving
1995 cvEndFindContours( &scanner );
1997 // allocate quad & corner buffers
1998 int total = root->total;
1999 *max_quad_buf_size = MAX(1, (total + total / 2)) * 2;
2000 *out_quads = (CvCBQuad*)cvAlloc(*max_quad_buf_size * sizeof((*out_quads)[0]));
2001 *out_corners = (CvCBCorner*)cvAlloc(*max_quad_buf_size * 4 * sizeof((*out_corners)[0]));
2003 // Create array of quads structures
2004 for( idx = 0; idx < root->total; idx++ )
2006 CvCBQuad* q = &(*out_quads)[quad_count];
2007 src_contour = *(CvSeq**)cvGetSeqElem( root, idx );
2008 if (filterQuads && src_contour->v_prev != (CvSeq*)board)
2012 memset( q, 0, sizeof(*q) );
2014 assert( src_contour->total == 4 );
2015 for( i = 0; i < 4; i++ )
2017 CvPoint * onePoint = (CvPoint*)cvGetSeqElem(src_contour, i);
2018 CV_Assert(onePoint != NULL);
2019 CvPoint2D32f pt = cvPointTo32f(*onePoint);
2020 CvCBCorner* corner = &(*out_corners)[quad_count*4 + i];
2022 memset( corner, 0, sizeof(*corner) );
2024 q->corners[i] = corner;
2026 q->edge_len = FLT_MAX;
2027 for( i = 0; i < 4; i++ )
2029 float dx = q->corners[i]->pt.x - q->corners[(i+1)&3]->pt.x;
2030 float dy = q->corners[i]->pt.y - q->corners[(i+1)&3]->pt.y;
2031 float d = dx*dx + dy*dy;
2032 if( q->edge_len > d )
2039 CV_LOG_VERBOSE(NULL, 3, "Total quad contours: " << total);
2040 CV_LOG_VERBOSE(NULL, 3, "max_quad_buf_size=" << *max_quad_buf_size);
2041 CV_LOG_VERBOSE(NULL, 3, "filtered quad_count=" << quad_count);
2046 static bool processQuads(CvCBQuad *quads, int quad_count, CvSize pattern_size, int max_quad_buf_size,
2047 CvMemStorage * storage, CvCBCorner *corners, CvPoint2D32f *out_corners, int *out_corner_count, int & prev_sqr_size)
2049 if( quad_count <= 0 )
2054 // Find quad's neighbors
2055 icvFindQuadNeighbors( quads, quad_count );
2057 // allocate extra for adding in icvOrderFoundQuads
2058 CvCBQuad **quad_group = 0;
2059 CvCBCorner **corner_group = 0;
2061 quad_group = (CvCBQuad**)cvAlloc( sizeof(quad_group[0]) * max_quad_buf_size);
2062 corner_group = (CvCBCorner**)cvAlloc( sizeof(corner_group[0]) * max_quad_buf_size * 4 );
2064 for( int group_idx = 0; ; group_idx++ )
2066 int count = icvFindConnectedQuads( quads, quad_count, quad_group, group_idx, storage );
2071 // order the quad corners globally
2072 // maybe delete or add some
2073 PRINTF("Starting ordering of inner quads (%d)\n", count);
2074 count = icvOrderFoundConnectedQuads(count, quad_group, &quad_count, &quads, &corners,
2075 pattern_size, max_quad_buf_size, storage );
2076 PRINTF("Finished ordering of inner quads (%d)\n", count);
2079 continue; // haven't found inner quads
2081 // If count is more than it should be, this will remove those quads
2082 // which cause maximum deviation from a nice square pattern.
2083 count = icvCleanFoundConnectedQuads( count, quad_group, pattern_size );
2084 PRINTF("Connected group: %d, count: %d\n", group_idx, count);
2086 count = icvCheckQuadGroup( quad_group, count, corner_group, pattern_size );
2087 PRINTF("Connected group: %d, count: %d\n", group_idx, count);
2089 int n = count > 0 ? pattern_size.width * pattern_size.height : -count;
2090 n = MIN( n, pattern_size.width * pattern_size.height );
2094 for(int i = 0; i < n; i++ )
2097 float avgi = corner_group[i]->meanDist(&ni);
2098 sum_dist += avgi*ni;
2101 prev_sqr_size = cvRound(sum_dist/MAX(total, 1));
2103 if( count > 0 || (out_corner_count && -count > *out_corner_count) )
2105 // copy corners to output array
2106 for(int i = 0; i < n; i++ )
2107 out_corners[i] = corner_group[i]->pt;
2109 if( out_corner_count )
2110 *out_corner_count = n;
2112 if( count == pattern_size.width*pattern_size.height
2113 && icvCheckBoardMonotony( out_corners, pattern_size ))
2121 cvFree(&quad_group);
2122 cvFree(&corner_group);
2127 //==================================================================================================
2130 cvDrawChessboardCorners( CvArr* _image, CvSize pattern_size,
2131 CvPoint2D32f* corners, int count, int found )
2133 const int shift = 0;
2134 const int radius = 4;
2135 const int r = radius*(1 << shift);
2139 int type, cn, line_type;
2141 image = cvGetMat( _image, &stub );
2143 type = CV_MAT_TYPE(image->type);
2144 cn = CV_MAT_CN(type);
2145 if( cn != 1 && cn != 3 && cn != 4 )
2146 CV_Error( CV_StsUnsupportedFormat, "Number of channels must be 1, 3 or 4" );
2148 switch( CV_MAT_DEPTH(image->type) )
2160 CV_Error( CV_StsUnsupportedFormat,
2161 "Only 8-bit, 16-bit or floating-point 32-bit images are supported" );
2164 line_type = type == CV_8UC1 || type == CV_8UC3 ? CV_AA : 8;
2168 CvScalar color(0,0,255,0);
2170 color = cvScalarAll(200);
2171 color.val[0] *= scale;
2172 color.val[1] *= scale;
2173 color.val[2] *= scale;
2174 color.val[3] *= scale;
2176 for( i = 0; i < count; i++ )
2179 pt.x = cvRound(corners[i].x*(1 << shift));
2180 pt.y = cvRound(corners[i].y*(1 << shift));
2181 cvLine( image, cvPoint( pt.x - r, pt.y - r ),
2182 cvPoint( pt.x + r, pt.y + r ), color, 1, line_type, shift );
2183 cvLine( image, cvPoint( pt.x - r, pt.y + r),
2184 cvPoint( pt.x + r, pt.y - r), color, 1, line_type, shift );
2185 cvCircle( image, pt, r+(1<<shift), color, 1, line_type, shift );
2192 const int line_max = 7;
2193 static const CvScalar line_colors[line_max] =
2196 CvScalar(0,128,255),
2197 CvScalar(0,200,200),
2199 CvScalar(200,200,0),
2204 for( y = 0, i = 0; y < pattern_size.height; y++ )
2206 CvScalar color = line_colors[y % line_max];
2208 color = cvScalarAll(200);
2209 color.val[0] *= scale;
2210 color.val[1] *= scale;
2211 color.val[2] *= scale;
2212 color.val[3] *= scale;
2214 for( x = 0; x < pattern_size.width; x++, i++ )
2217 pt.x = cvRound(corners[i].x*(1 << shift));
2218 pt.y = cvRound(corners[i].y*(1 << shift));
2221 cvLine( image, prev_pt, pt, color, 1, line_type, shift );
2223 cvLine( image, cvPoint(pt.x - r, pt.y - r),
2224 cvPoint(pt.x + r, pt.y + r), color, 1, line_type, shift );
2225 cvLine( image, cvPoint(pt.x - r, pt.y + r),
2226 cvPoint(pt.x + r, pt.y - r), color, 1, line_type, shift );
2227 cvCircle( image, pt, r+(1<<shift), color, 1, line_type, shift );
2234 bool cv::findChessboardCorners( InputArray _image, Size patternSize,
2235 OutputArray corners, int flags )
2237 CV_INSTRUMENT_REGION()
2239 int count = patternSize.area()*2;
2240 std::vector<Point2f> tmpcorners(count+1);
2241 Mat image = _image.getMat(); CvMat c_image = image;
2242 bool ok = cvFindChessboardCorners(&c_image, patternSize,
2243 (CvPoint2D32f*)&tmpcorners[0], &count, flags ) > 0;
2246 tmpcorners.resize(count);
2247 Mat(tmpcorners).copyTo(corners);
2256 int quiet_error(int /*status*/, const char* /*func_name*/,
2257 const char* /*err_msg*/, const char* /*file_name*/,
2258 int /*line*/, void* /*userdata*/ )
2264 void cv::drawChessboardCorners( InputOutputArray _image, Size patternSize,
2265 InputArray _corners,
2266 bool patternWasFound )
2268 CV_INSTRUMENT_REGION()
2270 Mat corners = _corners.getMat();
2271 if( corners.empty() )
2273 Mat image = _image.getMat(); CvMat c_image = image;
2274 int nelems = corners.checkVector(2, CV_32F, true);
2275 CV_Assert(nelems >= 0);
2276 cvDrawChessboardCorners( &c_image, patternSize, corners.ptr<CvPoint2D32f>(),
2277 nelems, patternWasFound );
2280 bool cv::findCirclesGrid( InputArray image, Size patternSize,
2281 OutputArray centers, int flags,
2282 const Ptr<FeatureDetector> &blobDetector,
2283 CirclesGridFinderParameters parameters)
2285 CirclesGridFinderParameters2 parameters2;
2286 *((CirclesGridFinderParameters*)¶meters2) = parameters;
2287 return cv::findCirclesGrid2(image, patternSize, centers, flags, blobDetector, parameters2);
2290 bool cv::findCirclesGrid2( InputArray _image, Size patternSize,
2291 OutputArray _centers, int flags, const Ptr<FeatureDetector> &blobDetector,
2292 CirclesGridFinderParameters2 parameters)
2294 CV_INSTRUMENT_REGION()
2296 bool isAsymmetricGrid = (flags & CALIB_CB_ASYMMETRIC_GRID) ? true : false;
2297 bool isSymmetricGrid = (flags & CALIB_CB_SYMMETRIC_GRID ) ? true : false;
2298 CV_Assert(isAsymmetricGrid ^ isSymmetricGrid);
2300 Mat image = _image.getMat();
2301 std::vector<Point2f> centers;
2303 std::vector<KeyPoint> keypoints;
2304 blobDetector->detect(image, keypoints);
2305 std::vector<Point2f> points;
2306 for (size_t i = 0; i < keypoints.size(); i++)
2308 points.push_back (keypoints[i].pt);
2311 if(flags & CALIB_CB_ASYMMETRIC_GRID)
2312 parameters.gridType = CirclesGridFinderParameters::ASYMMETRIC_GRID;
2313 if(flags & CALIB_CB_SYMMETRIC_GRID)
2314 parameters.gridType = CirclesGridFinderParameters::SYMMETRIC_GRID;
2316 if(flags & CALIB_CB_CLUSTERING)
2318 CirclesGridClusterFinder circlesGridClusterFinder(parameters);
2319 circlesGridClusterFinder.findGrid(points, patternSize, centers);
2320 Mat(centers).copyTo(_centers);
2321 return !centers.empty();
2324 const int attempts = 2;
2325 const size_t minHomographyPoints = 4;
2327 for (int i = 0; i < attempts; i++)
2330 CirclesGridFinder boxFinder(patternSize, points, parameters);
2331 bool isFound = false;
2335 ErrorCallback oldCbk = redirectError(quiet_error, 0, &oldCbkData);
2339 isFound = boxFinder.findHoles();
2341 CV_CATCH(Exception, e)
2346 redirectError(oldCbk, oldCbkData);
2350 switch(parameters.gridType)
2352 case CirclesGridFinderParameters::SYMMETRIC_GRID:
2353 boxFinder.getHoles(centers);
2355 case CirclesGridFinderParameters::ASYMMETRIC_GRID:
2356 boxFinder.getAsymmetricHoles(centers);
2359 CV_Error(CV_StsBadArg, "Unknown pattern type");
2365 transform(centers, orgPointsMat, H.inv());
2366 convertPointsFromHomogeneous(orgPointsMat, centers);
2368 Mat(centers).copyTo(_centers);
2372 boxFinder.getHoles(centers);
2373 if (i != attempts - 1)
2375 if (centers.size() < minHomographyPoints)
2377 H = CirclesGridFinder::rectifyGrid(boxFinder.getDetectedGridSize(), centers, points, points);
2380 Mat(centers).copyTo(_centers);
2384 bool cv::findCirclesGrid( InputArray _image, Size patternSize,
2385 OutputArray _centers, int flags, const Ptr<FeatureDetector> &blobDetector)
2387 return cv::findCirclesGrid2(_image, patternSize, _centers, flags, blobDetector, CirclesGridFinderParameters2());