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 \************************************************************************************/
64 //#define DEBUG_CHESSBOARD
65 #ifdef DEBUG_CHESSBOARD
67 #include "..//..//otherlibs/highgui/highgui.h"
73 //=====================================================================================
74 // Implementation for the enhanced calibration object detection
75 //=====================================================================================
77 #define MAX_CONTOUR_APPROX 7
79 typedef struct CvContourEx
86 //=====================================================================================
88 /// Corner info structure
89 /** This structure stores information about the chessboard corner.*/
90 typedef struct CvCBCorner
92 CvPoint2D32f pt; // Coordinates of the corner
93 int row; // Board row index
94 int count; // Number of neighbor corners
95 struct CvCBCorner* neighbors[4]; // Neighbor corners
99 //=====================================================================================
100 /// Quadrangle contour info structure
101 /** This structure stores information about the chessboard quadrange.*/
102 typedef struct CvCBQuad
104 int count; // Number of quad neighbors
105 int group_idx; // quad group ID
106 int row, col; // row and column of this quad
107 bool ordered; // true if corners/neighbors are ordered counter-clockwise
108 float edge_len; // quad edge len, in pix^2
109 // neighbors and corners are synced, i.e., neighbor 0 shares corner 0
110 CvCBCorner *corners[4]; // Coordinates of quad corners
111 struct CvCBQuad *neighbors[4]; // Pointers of quad neighbors
115 //=====================================================================================
117 //static CvMat* debug_img = 0;
119 static int icvGenerateQuads( CvCBQuad **quads, CvCBCorner **corners,
120 CvMemStorage *storage, CvMat *image, int flags );
123 icvGenerateQuadsEx( CvCBQuad **out_quads, CvCBCorner **out_corners,
124 CvMemStorage *storage, CvMat *image, CvMat *thresh_img, int dilation, int flags );
126 static void icvFindQuadNeighbors( CvCBQuad *quads, int quad_count );
128 static int icvFindConnectedQuads( CvCBQuad *quads, int quad_count,
129 CvCBQuad **quad_group, int group_idx,
130 CvMemStorage* storage );
132 static int icvCheckQuadGroup( CvCBQuad **quad_group, int count,
133 CvCBCorner **out_corners, CvSize pattern_size );
135 static int icvCleanFoundConnectedQuads( int quad_count,
136 CvCBQuad **quads, CvSize pattern_size );
138 static int icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,
139 int *all_count, CvCBQuad **all_quads, CvCBCorner **corners,
140 CvSize pattern_size, CvMemStorage* storage );
142 static void icvOrderQuad(CvCBQuad *quad, CvCBCorner *corner, int common);
144 static int icvTrimCol(CvCBQuad **quads, int count, int col, int dir);
146 static int icvTrimRow(CvCBQuad **quads, int count, int row, int dir);
148 static int icvAddOuterQuad(CvCBQuad *quad, CvCBQuad **quads, int quad_count,
149 CvCBQuad **all_quads, int all_count, CvCBCorner **corners);
151 static void icvRemoveQuadFromGroup(CvCBQuad **quads, int count, CvCBQuad *q0);
155 icvCalcAffineTranf2D32f(CvPoint2D32f* pts1, CvPoint2D32f* pts2, int count, CvMat* affine_trans)
159 for( j = 0; j < count; j++ )
161 if( pts1[j].x >= 0 ) real_count++;
163 if(real_count < 3) return;
164 CvMat* xy = cvCreateMat( 2*real_count, 6, CV_32FC1 );
165 CvMat* uv = cvCreateMat( 2*real_count, 1, CV_32FC1 );
166 //estimate affine transfromation
167 for( i = 0, j = 0; j < count; j++ )
171 CV_MAT_ELEM( *xy, float, i*2+1, 2 ) = CV_MAT_ELEM( *xy, float, i*2, 0 ) = pts2[j].x;
172 CV_MAT_ELEM( *xy, float, i*2+1, 3 ) = CV_MAT_ELEM( *xy, float, i*2, 1 ) = pts2[j].y;
173 CV_MAT_ELEM( *xy, float, i*2, 2 ) = CV_MAT_ELEM( *xy, float, i*2, 3 ) = CV_MAT_ELEM( *xy, float, i*2, 5 ) = \
174 CV_MAT_ELEM( *xy, float, i*2+1, 0 ) = CV_MAT_ELEM( *xy, float, i*2+1, 1 ) = CV_MAT_ELEM( *xy, float, i*2+1, 4 ) = 0;
175 CV_MAT_ELEM( *xy, float, i*2, 4 ) = CV_MAT_ELEM( *xy, float, i*2+1, 5 ) = 1;
176 CV_MAT_ELEM( *uv, float, i*2, 0 ) = pts1[j].x;
177 CV_MAT_ELEM( *uv, float, i*2+1, 0 ) = pts1[j].y;
182 cvSolve( xy, uv, affine_trans, CV_SVD );
189 int cvFindChessboardCorners( const void* arr, CvSize pattern_size,
190 CvPoint2D32f* out_corners, int* out_corner_count,
193 const int min_dilations = 0;
194 const int max_dilations = 3;
197 CvMat* thresh_img = 0;
198 #ifdef DEBUG_CHESSBOARD
199 IplImage *dbg_img = 0;
200 IplImage *dbg1_img = 0;
201 IplImage *dbg2_img = 0;
203 CvMemStorage* storage = 0;
205 #define EXTRA_QUADS 10 // extra quad slots for additions
206 CvCBQuad *quads = 0, **quad_group = 0;
207 CvCBCorner *corners = 0, **corner_group = 0;
209 int expected_corners_num = (pattern_size.width/2+1)*(pattern_size.height/2+1);
211 if( out_corner_count )
212 *out_corner_count = 0;
214 CV_FUNCNAME( "cvFindChessBoardCornerGuesses2" );
218 int quad_count, group_idx, i, dilations;
219 CvMat stub, *img = (CvMat*)arr;
221 CV_CALL( img = cvGetMat( img, &stub ));
224 if( CV_MAT_DEPTH( img->type ) != CV_8U || CV_MAT_CN( img->type ) == 2 )
225 CV_ERROR( CV_StsUnsupportedFormat, "Only 8-bit grayscale or color images are supported" );
227 if( pattern_size.width <= 2 || pattern_size.height <= 2 )
228 CV_ERROR( CV_StsOutOfRange, "pattern should have at least 2x2 size" );
231 CV_ERROR( CV_StsNullPtr, "Null pointer to corners" );
233 CV_CALL( storage = cvCreateMemStorage(0) );
234 CV_CALL( thresh_img = cvCreateMat( img->rows, img->cols, CV_8UC1 ));
236 #ifdef DEBUG_CHESSBOARD
237 CV_CALL( dbg_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3 ));
238 CV_CALL( dbg1_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3 ));
239 CV_CALL( dbg2_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3 ));
242 if( CV_MAT_CN(img->type) != 1 || (flags & CV_CALIB_CB_NORMALIZE_IMAGE) )
244 // equalize the input image histogram -
245 // that should make the contrast between "black" and "white" areas big enough
246 CV_CALL( norm_img = cvCreateMat( img->rows, img->cols, CV_8UC1 ));
248 if( CV_MAT_CN(img->type) != 1 )
250 CV_CALL( cvCvtColor( img, norm_img, CV_BGR2GRAY ));
254 if( flags & CV_CALIB_CB_NORMALIZE_IMAGE )
256 cvEqualizeHist( img, norm_img );
261 // Try our standard "1" dilation, but if the pattern is not found, iterate the whole procedure with higher dilations.
262 // This is necessary because some squares simply do not separate properly with a single dilation. However,
263 // we want to use the minimum number of dilations possible since dilations cause the squares to become smaller,
264 // making it difficult to detect smaller squares.
265 for( dilations = min_dilations; dilations <= max_dilations; dilations++ )
267 if (found) break; // already found it
269 // convert the input grayscale image to binary (black-n-white)
270 if( flags & CV_CALIB_CB_ADAPTIVE_THRESH )
272 int block_size = cvRound(MIN(img->cols,img->rows)*0.2)|1;
273 cvDilate( img, thresh_img, 0, dilations );
276 cvAdaptiveThreshold( img, thresh_img, 255,
277 CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, block_size, 0 );
279 cvDilate( thresh_img, thresh_img, 0, dilations-1 );
283 // Make dilation before the thresholding.
284 // It splits chessboard corners
285 //cvDilate( img, thresh_img, 0, 1 );
287 // empiric threshold level
288 double mean = cvMean( img );
289 int thresh_level = cvRound( mean - 10 );
290 thresh_level = MAX( thresh_level, 10 );
292 cvThreshold( img, thresh_img, thresh_level, 255, CV_THRESH_BINARY );
293 cvDilate( thresh_img, thresh_img, 0, dilations );
297 #ifdef DEBUG_CHESSBOARD
298 cvCvtColor(thresh_img,dbg_img,CV_GRAY2BGR);
301 // So we can find rectangles that go to the edge, we draw a white line around the image edge.
302 // Otherwise FindContours will miss those clipped rectangle contours.
303 // The border color will be the image mean, because otherwise we risk screwing up filters like cvSmooth()...
304 cvRectangle( thresh_img, cvPoint(0,0), cvPoint(thresh_img->cols-1,
305 thresh_img->rows-1), CV_RGB(255,255,255), 3, 8);
307 CV_CALL( quad_count = icvGenerateQuads( &quads, &corners, storage, thresh_img, flags ));
310 PRINTF("Quad count: %d/%d\n", quad_count, expected_corners_num);
312 // Run multi-level quads extraction
313 // In case one-level binarization did not give enough number of quads
314 if( quad_count < expected_corners_num )
316 CV_CALL( quad_count = icvGenerateQuadsEx( &quads, &corners, storage, img, thresh_img, dilations, flags ));
317 PRINTF("EX quad count: %d/%d\n", quad_count, expected_corners_num);
320 #ifdef DEBUG_CHESSBOARD
321 cvCopy(dbg_img, dbg1_img);
322 cvNamedWindow("all_quads", 1);
323 // copy corners to temp array
324 for( i = 0; i < quad_count; i++ )
326 for (int k=0; k<4; k++)
328 CvPoint2D32f pt1, pt2;
329 CvScalar color = CV_RGB(30,255,30);
330 pt1 = quads[i].corners[k]->pt;
331 pt2 = quads[i].corners[(k+1)%4]->pt;
332 pt2.x = (pt1.x + pt2.x)/2;
333 pt2.y = (pt1.y + pt2.y)/2;
335 color = CV_RGB(200,200,0);
336 cvLine( dbg1_img, cvPointFrom32f(pt1), cvPointFrom32f(pt2), color, 3, 8);
341 cvShowImage("all_quads", (IplImage*)dbg1_img);
346 if( quad_count <= 0 )
349 // Find quad's neighbors
350 CV_CALL( icvFindQuadNeighbors( quads, quad_count ));
352 // allocate extra for adding in icvOrderFoundQuads
353 CV_CALL( quad_group = (CvCBQuad**)cvAlloc( sizeof(quad_group[0]) * (quad_count+5)));
354 CV_CALL( corner_group = (CvCBCorner**)cvAlloc( sizeof(corner_group[0]) * (quad_count+5)*4 ));
356 for( group_idx = 0; ; group_idx++ )
359 CV_CALL( count = icvFindConnectedQuads( quads, quad_count, quad_group, group_idx, storage ));
365 // order the quad corners globally
366 // maybe delete or add some
367 PRINTF("Starting ordering of inner quads\n");
368 count = icvOrderFoundConnectedQuads(count, quad_group, &quad_count, &quads, &corners,
369 pattern_size, storage );
370 PRINTF("Orig count: %d After ordering: %d\n", icount, count);
373 #ifdef DEBUG_CHESSBOARD
374 cvCopy(dbg_img,dbg2_img);
375 cvNamedWindow("connected_group", 1);
376 // copy corners to temp array
377 for( i = 0; i < quad_count; i++ )
379 if (quads[i].group_idx == group_idx)
380 for (int k=0; k<4; k++)
382 CvPoint2D32f pt1, pt2;
383 CvScalar color = CV_RGB(30,255,30);
384 if (quads[i].ordered)
385 color = CV_RGB(255,30,30);
386 pt1 = quads[i].corners[k]->pt;
387 pt2 = quads[i].corners[(k+1)%4]->pt;
388 pt2.x = (pt1.x + pt2.x)/2;
389 pt2.y = (pt1.y + pt2.y)/2;
391 color = CV_RGB(200,200,0);
392 cvLine( dbg2_img, cvPointFrom32f(pt1), cvPointFrom32f(pt2), color, 3, 8);
395 cvShowImage("connected_group", (IplImage*)dbg2_img);
400 continue; // haven't found inner quads
403 // If count is more than it should be, this will remove those quads
404 // which cause maximum deviation from a nice square pattern.
405 CV_CALL( count = icvCleanFoundConnectedQuads( count, quad_group, pattern_size ));
406 PRINTF("Connected group: %d orig count: %d cleaned: %d\n", group_idx, icount, count);
408 CV_CALL( count = icvCheckQuadGroup( quad_group, count, corner_group, pattern_size ));
409 PRINTF("Connected group: %d count: %d cleaned: %d\n", group_idx, icount, count);
411 if( count > 0 || (out_corner_count && -count > *out_corner_count) )
413 int n = count > 0 ? pattern_size.width * pattern_size.height : -count;
414 n = MIN( n, pattern_size.width * pattern_size.height );
416 // copy corners to output array
417 for( i = 0; i < n; i++ )
418 out_corners[i] = corner_group[i]->pt;
420 if( out_corner_count )
421 *out_corner_count = n;
433 cvFree( &quad_group );
434 cvFree( &corner_group );
441 cvReleaseMemStorage( &storage );
442 cvReleaseMat( &norm_img );
443 cvReleaseMat( &thresh_img );
453 // order a group of connected quads
456 // clockwise from there
457 // note: "top left" is nominal, depends on initial ordering of starting quad
458 // but all other quads are ordered consistently
460 // can change the number of quads in the group
461 // can add quads, so we need to have quad/corner arrays passed in
465 icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,
466 int *all_count, CvCBQuad **all_quads, CvCBCorner **corners,
467 CvSize pattern_size, CvMemStorage* storage )
469 CvMemStorage* temp_storage = cvCreateChildMemStorage( storage );
470 CvSeq* stack = cvCreateSeq( 0, sizeof(*stack), sizeof(void*), temp_storage );
472 // first find an interior quad
473 CvCBQuad *start = NULL;
474 for (int i=0; i<quad_count; i++)
476 if (quads[i]->count == 4)
484 return 0; // no 4-connected quad
486 // start with first one, assign rows/cols
487 int row_min = 0, col_min = 0, row_max=0, col_max = 0;
489 int col_hist[HSIZE*2];
490 int row_hist[HSIZE*2]; // bad programming, should allow variable size
492 for (int i=0; i<HSIZE*2; i++) // init to zero
493 col_hist[i] = row_hist[i] = 0;
494 cvSeqPush(stack, &start);
497 start->ordered = true;
499 // Recursively order the quads so that all position numbers (e.g.,
500 // 0,1,2,3) are in the at the same relative corner (e.g., lower right).
502 while( stack->total )
505 cvSeqPop( stack, &q );
508 col_hist[col+HSIZE]++;
509 row_hist[row+HSIZE]++;
512 if (row > row_max) row_max = row;
513 if (row < row_min) row_min = row;
514 if (col > col_max) col_max = col;
515 if (col < col_min) col_min = col;
517 for(int i = 0; i < 4; i++ )
519 CvCBQuad *neighbor = q->neighbors[i];
520 switch(i) // adjust col, row for this quad
521 { // start at top left, go clockwise
532 // just do inside quads
533 if (neighbor && neighbor->ordered == false && neighbor->count == 4)
535 PRINTF("col: %d row: %d\n", col, row);
536 icvOrderQuad(neighbor, q->corners[i], (i+2)%4); // set in order
537 neighbor->ordered = true;
540 cvSeqPush( stack, &neighbor );
545 cvReleaseMemStorage( &temp_storage );
547 for (int i=col_min; i<=col_max; i++)
548 PRINTF("HIST[%d] = %d\n", i, col_hist[i+HSIZE]);
550 // analyze inner quad structure
551 int w = pattern_size.width - 1;
552 int h = pattern_size.height - 1;
553 int drow = row_max - row_min + 1;
554 int dcol = col_max - col_min + 1;
556 // normalize pattern and found quad indices
557 if ((w > h && dcol < drow) ||
558 (w < h && drow < dcol))
560 h = pattern_size.width - 1;
561 w = pattern_size.height - 1;
564 PRINTF("Size: %dx%d Pattern: %dx%d\n", dcol, drow, w, h);
566 // check if there are enough inner quads
567 if (dcol < w || drow < h) // found enough inner quads?
569 PRINTF("Too few inner quad rows/cols\n");
570 return 0; // no, return
573 // too many columns, not very common
574 if (dcol == w+1) // too many, trim
576 PRINTF("Trimming cols\n");
577 if (col_hist[col_max+HSIZE] > col_hist[col_min+HSIZE])
579 PRINTF("Trimming left col\n");
580 quad_count = icvTrimCol(quads,quad_count,col_min,-1);
584 PRINTF("Trimming right col\n");
585 quad_count = icvTrimCol(quads,quad_count,col_max,+1);
589 // too many rows, not very common
590 if (drow == h+1) // too many, trim
592 PRINTF("Trimming rows\n");
593 if (row_hist[row_max+HSIZE] > row_hist[row_min+HSIZE])
595 PRINTF("Trimming top row\n");
596 quad_count = icvTrimRow(quads,quad_count,row_min,-1);
600 PRINTF("Trimming bottom row\n");
601 quad_count = icvTrimRow(quads,quad_count,row_max,+1);
606 // check edges of inner quads
607 // if there is an outer quad missing, fill it in
608 // first order all inner quads
610 for (int i=0; i<quad_count; i++)
612 if (quads[i]->count == 4)
613 { // ok, look at neighbors
614 int col = quads[i]->col;
615 int row = quads[i]->row;
616 for (int j=0; j<4; j++)
618 switch(j) // adjust col, row for this quad
619 { // start at top left, go clockwise
629 CvCBQuad *neighbor = quads[i]->neighbors[j];
630 if (neighbor && !neighbor->ordered && // is it an inner quad?
631 col <= col_max && col >= col_min &&
632 row <= row_max && row >= row_min)
634 // if so, set in order
635 PRINTF("Adding inner: col: %d row: %d\n", col, row);
637 icvOrderQuad(neighbor, quads[i]->corners[j], (j+2)%4);
638 neighbor->ordered = true;
646 // if we have found inner quads, add corresponding outer quads,
650 PRINTF("Found %d inner quads not connected to outer quads, repairing\n", found);
651 for (int i=0; i<quad_count; i++)
653 if (quads[i]->count < 4 && quads[i]->ordered)
655 int added = icvAddOuterQuad(quads[i],quads,quad_count,all_quads,*all_count,corners);
663 // final trimming of outer quads
664 if (dcol == w && drow == h) // found correct inner quads
666 PRINTF("Inner bounds ok, check outer quads\n");
667 int rcount = quad_count;
668 for (int i=quad_count-1; i>=0; i--) // eliminate any quad not connected to
671 if (quads[i]->ordered == false)
674 for (int j=0; j<4; j++) // any neighbors that are ordered?
676 if (quads[i]->neighbors[j] && quads[i]->neighbors[j]->ordered)
679 if (!outer) // not an outer quad, eliminate
681 PRINTF("Removing quad %d\n", i);
682 icvRemoveQuadFromGroup(quads,rcount,quads[i]);
696 // looks for the neighbor of <quad> that isn't present,
697 // tries to add it in.
701 icvAddOuterQuad( CvCBQuad *quad, CvCBQuad **quads, int quad_count,
702 CvCBQuad **all_quads, int all_count, CvCBCorner **corners )
706 for (int i=0; i<4; i++) // find no-neighbor corners
708 if (!quad->neighbors[i]) // ok, create and add neighbor
711 PRINTF("Adding quad as neighbor 2\n");
712 CvCBQuad *q = &(*all_quads)[all_count];
713 memset( q, 0, sizeof(*q) );
715 quads[quad_count] = q;
718 // set neighbor and group id
719 quad->neighbors[i] = q;
721 q->neighbors[j] = quad;
722 q->group_idx = quad->group_idx;
723 q->count = 1; // number of neighbors
725 q->edge_len = quad->edge_len;
727 // make corners of new quad
728 // same as neighbor quad, but offset
729 CvPoint2D32f pt = quad->corners[i]->pt;
731 float dx = pt.x - quad->corners[j]->pt.x;
732 float dy = pt.y - quad->corners[j]->pt.y;
733 for (int k=0; k<4; k++)
735 corner = &(*corners)[all_count*4+k];
736 pt = quad->corners[k]->pt;
737 memset( corner, 0, sizeof(*corner) );
739 q->corners[k] = corner;
743 // have to set exact corner
744 q->corners[j] = quad->corners[i];
746 // now find other neighbor and add it, if possible
747 if (quad->neighbors[(i+3)%4] &&
748 quad->neighbors[(i+3)%4]->ordered &&
749 quad->neighbors[(i+3)%4]->neighbors[i] &&
750 quad->neighbors[(i+3)%4]->neighbors[i]->ordered )
752 CvCBQuad *qn = quad->neighbors[(i+3)%4]->neighbors[i];
754 q->neighbors[(j+1)%4] = qn;
755 qn->neighbors[(i+1)%4] = q;
757 // have to set exact corner
758 q->corners[(j+1)%4] = qn->corners[(i+1)%4];
771 icvTrimCol(CvCBQuad **quads, int count, int col, int dir)
774 // find the right quad(s)
775 for (int i=0; i<count; i++)
777 #ifdef DEBUG_CHESSBOARD
778 if (quads[i]->ordered)
779 PRINTF("index: %d cur: %d\n", col, quads[i]->col);
781 if (quads[i]->ordered && quads[i]->col == col)
785 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[1]);
787 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[2]);
792 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[0]);
794 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[3]);
804 icvTrimRow(CvCBQuad **quads, int count, int row, int dir)
807 // find the right quad(s)
808 for (int i=0; i<count; i++)
810 #ifdef DEBUG_CHESSBOARD
811 if (quads[i]->ordered)
812 PRINTF("index: %d cur: %d\n", row, quads[i]->row);
814 if (quads[i]->ordered && quads[i]->row == row)
816 if (dir == 1) // remove from bottom
818 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[2]);
820 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[3]);
823 else // remove from top
825 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[0]);
827 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[1]);
838 // remove quad from quad group
842 icvRemoveQuadFromGroup(CvCBQuad **quads, int count, CvCBQuad *q0)
844 // remove any references to this quad as a neighbor
845 for(int i = 0; i < count; i++ )
847 CvCBQuad *q = quads[i];
848 for(int j = 0; j < 4; j++ )
850 if( q->neighbors[j] == q0 )
854 for(int k = 0; k < 4; k++ )
855 if( q0->neighbors[k] == q )
857 q0->neighbors[k] = 0;
867 for(int i = 0; i < count; i++ )
869 CvCBQuad *q = quads[i];
872 quads[i] = quads[count-1];
879 // put quad into correct order, where <corner> has value <common>
883 icvOrderQuad(CvCBQuad *quad, CvCBCorner *corner, int common)
887 for (tc=0; tc<4; tc++)
888 if (quad->corners[tc]->pt.x == corner->pt.x &&
889 quad->corners[tc]->pt.y == corner->pt.y)
899 tempc = quad->corners[3];
900 tempq = quad->neighbors[3];
901 for (int i=3; i>0; i--)
903 quad->corners[i] = quad->corners[i-1];
904 quad->neighbors[i] = quad->neighbors[i-1];
906 quad->corners[0] = tempc;
907 quad->neighbors[0] = tempq;
914 // if we found too many connect quads, remove those which probably do not belong.
916 icvCleanFoundConnectedQuads( int quad_count, CvCBQuad **quad_group, CvSize pattern_size )
918 CvMemStorage *temp_storage = 0;
919 CvPoint2D32f *centers = 0;
921 CV_FUNCNAME( "icvCleanFoundConnectedQuads" );
925 CvPoint2D32f center = {0,0};
927 // number of quads this pattern should contain
928 int count = ((pattern_size.width + 1)*(pattern_size.height + 1) + 1)/2;
930 // if we have more quadrangles than we should,
931 // try to eliminate duplicates or ones which don't belong to the pattern rectangle...
932 if( quad_count <= count )
935 // create an array of quadrangle centers
936 CV_CALL( centers = (CvPoint2D32f *)cvAlloc( sizeof(centers[0])*quad_count ));
937 CV_CALL( temp_storage = cvCreateMemStorage(0));
939 for( i = 0; i < quad_count; i++ )
941 CvPoint2D32f ci = {0,0};
942 CvCBQuad* q = quad_group[i];
944 for( j = 0; j < 4; j++ )
946 CvPoint2D32f pt = q->corners[j]->pt;
958 center.x /= quad_count;
959 center.y /= quad_count;
961 // If we still have more quadrangles than we should,
962 // we try to eliminate bad ones based on minimizing the bounding box.
963 // We iteratively remove the point which reduces the size of
964 // the bounding box of the blobs the most
965 // (since we want the rectangle to be as small as possible)
966 // remove the quadrange that causes the biggest reduction
967 // in pattern size until we have the correct number
968 for( ; quad_count > count; quad_count-- )
970 double min_box_area = DBL_MAX;
971 int skip, min_box_area_index = -1;
974 // For each point, calculate box area without that point
975 for( skip = 0; skip < quad_count; skip++ )
977 // get bounding rectangle
978 CvPoint2D32f temp = centers[skip]; // temporarily make index 'skip' the same as
979 centers[skip] = center; // pattern center (so it is not counted for convex hull)
980 CvMat pointMat = cvMat(1, quad_count, CV_32FC2, centers);
981 CvSeq *hull = cvConvexHull2( &pointMat, temp_storage, CV_CLOCKWISE, 1 );
982 centers[skip] = temp;
983 double hull_area = fabs(cvContourArea(hull, CV_WHOLE_SEQ));
985 // remember smallest box area
986 if( hull_area < min_box_area )
988 min_box_area = hull_area;
989 min_box_area_index = skip;
991 cvClearMemStorage( temp_storage );
994 q0 = quad_group[min_box_area_index];
996 // remove any references to this quad as a neighbor
997 for( i = 0; i < quad_count; i++ )
1000 for( j = 0; j < 4; j++ )
1002 if( q->neighbors[j] == q0 )
1004 q->neighbors[j] = 0;
1006 for( k = 0; k < 4; k++ )
1007 if( q0->neighbors[k] == q )
1009 q0->neighbors[k] = 0;
1020 quad_group[min_box_area_index] = quad_group[quad_count];
1021 centers[min_box_area_index] = centers[quad_count];
1026 cvReleaseMemStorage( &temp_storage );
1032 //=====================================================================================
1035 icvFindConnectedQuads( CvCBQuad *quad, int quad_count, CvCBQuad **out_group,
1036 int group_idx, CvMemStorage* storage )
1038 CvMemStorage* temp_storage = cvCreateChildMemStorage( storage );
1039 CvSeq* stack = cvCreateSeq( 0, sizeof(*stack), sizeof(void*), temp_storage );
1042 // Scan the array for a first unlabeled quad
1043 for( i = 0; i < quad_count; i++ )
1045 if( quad[i].count > 0 && quad[i].group_idx < 0)
1049 // Recursively find a group of connected quads starting from the seed quad[i]
1050 if( i < quad_count )
1052 CvCBQuad* q = &quad[i];
1053 cvSeqPush( stack, &q );
1054 out_group[count++] = q;
1055 q->group_idx = group_idx;
1058 while( stack->total )
1060 cvSeqPop( stack, &q );
1061 for( i = 0; i < 4; i++ )
1063 CvCBQuad *neighbor = q->neighbors[i];
1064 if( neighbor && neighbor->count > 0 && neighbor->group_idx < 0 )
1066 cvSeqPush( stack, &neighbor );
1067 out_group[count++] = neighbor;
1068 neighbor->group_idx = group_idx;
1069 neighbor->ordered = false;
1075 cvReleaseMemStorage( &temp_storage );
1080 //=====================================================================================
1083 icvCheckQuadGroup( CvCBQuad **quad_group, int quad_count,
1084 CvCBCorner **out_corners, CvSize pattern_size )
1086 const int ROW1 = 1000000;
1087 const int ROW2 = 2000000;
1088 const int ROW_ = 3000000;
1090 int i, out_corner_count = 0, corner_count = 0;
1091 CvCBCorner** corners = 0;
1093 CV_FUNCNAME( "icvCheckQuadGroup" );
1098 int width = 0, height = 0;
1099 int hist[5] = {0,0,0,0,0};
1100 CvCBCorner* first = 0, *first2 = 0, *right, *cur, *below, *c;
1101 CV_CALL( corners = (CvCBCorner**)cvAlloc( quad_count*4*sizeof(corners[0]) ));
1103 // build dual graph, which vertices are internal quad corners
1104 // and two vertices are connected iff they lie on the same quad edge
1105 for( i = 0; i < quad_count; i++ )
1107 CvCBQuad* q = quad_group[i];
1108 /*CvScalar color = q->count == 0 ? cvScalar(0,255,255) :
1109 q->count == 1 ? cvScalar(0,0,255) :
1110 q->count == 2 ? cvScalar(0,255,0) :
1111 q->count == 3 ? cvScalar(255,255,0) :
1112 cvScalar(255,0,0);*/
1114 for( j = 0; j < 4; j++ )
1116 //cvLine( debug_img, cvPointFrom32f(q->corners[j]->pt), cvPointFrom32f(q->corners[(j+1)&3]->pt), color, 1, CV_AA, 0 );
1117 if( q->neighbors[j] )
1119 CvCBCorner *a = q->corners[j], *b = q->corners[(j+1)&3];
1120 // mark internal corners that belong to:
1121 // - a quad with a single neighbor - with ROW1,
1122 // - a quad with two neighbors - with ROW2
1123 // make the rest of internal corners with ROW_
1124 int row_flag = q->count == 1 ? ROW1 : q->count == 2 ? ROW2 : ROW_;
1128 corners[corner_count++] = a;
1131 else if( a->row > row_flag )
1134 if( q->neighbors[(j+1)&3] )
1136 if( a->count >= 4 || b->count >= 4 )
1138 for( k = 0; k < 4; k++ )
1140 if( a->neighbors[k] == b )
1142 if( b->neighbors[k] == a )
1145 a->neighbors[a->count++] = b;
1146 b->neighbors[b->count++] = a;
1152 if( corner_count != pattern_size.width*pattern_size.height )
1155 for( i = 0; i < corner_count; i++ )
1157 int n = corners[i]->count;
1158 assert( 0 <= n && n <= 4 );
1160 if( !first && n == 2 )
1162 if( corners[i]->row == ROW1 )
1164 else if( !first2 && corners[i]->row == ROW2 )
1165 first2 = corners[i];
1169 // start with a corner that belongs to a quad with a signle neighbor.
1170 // if we do not have such, start with a corner of a quad with two neighbors.
1174 if( !first || hist[0] != 0 || hist[1] != 0 || hist[2] != 4 ||
1175 hist[3] != (pattern_size.width + pattern_size.height)*2 - 8 )
1180 out_corners[out_corner_count++] = cur;
1182 for( k = 0; k < 4; k++ )
1184 c = cur->neighbors[k];
1194 if( !right || right->count != 2 && right->count != 3 ||
1195 !below || below->count != 2 && below->count != 3 )
1199 //cvCircle( debug_img, cvPointFrom32f(cur->pt), 3, cvScalar(0,255,0), -1, 8, 0 );
1201 first = below; // remember the first corner in the next row
1202 // find and store the first row (or column)
1206 out_corners[out_corner_count++] = right;
1207 //cvCircle( debug_img, cvPointFrom32f(right->pt), 3, cvScalar(0,255-j*10,0), -1, 8, 0 );
1208 if( right->count == 2 )
1210 if( right->count != 3 || out_corner_count >= MAX(pattern_size.width,pattern_size.height) )
1213 for( k = 0; k < 4; k++ )
1215 c = cur->neighbors[k];
1216 if( c && c->row > 0 )
1218 for( kk = 0; kk < 4; kk++ )
1220 if( c->neighbors[kk] == below )
1231 width = out_corner_count;
1232 if( width == pattern_size.width )
1233 height = pattern_size.height;
1234 else if( width == pattern_size.height )
1235 height = pattern_size.width;
1239 // find and store all the other rows
1249 out_corners[out_corner_count++] = cur;
1250 //cvCircle( debug_img, cvPointFrom32f(cur->pt), 3, cvScalar(0,0,255-j*10), -1, 8, 0 );
1251 if( cur->count == 2 + (i < height-1) && j > 0 )
1256 // find a neighbor that has not been processed yet
1257 // and that has a neighbor from the previous row
1258 for( k = 0; k < 4; k++ )
1260 c = cur->neighbors[k];
1261 if( c && c->row > i )
1263 for( kk = 0; kk < 4; kk++ )
1265 if( c->neighbors[kk] && c->neighbors[kk]->row == i-1 )
1283 if( j != width - 1 )
1287 if( out_corner_count != corner_count )
1290 // check if we need to transpose the board
1291 if( width != pattern_size.width )
1293 CV_SWAP( width, height, k );
1295 memcpy( corners, out_corners, corner_count*sizeof(corners[0]) );
1296 for( i = 0; i < height; i++ )
1297 for( j = 0; j < width; j++ )
1298 out_corners[i*width + j] = corners[j*height + i];
1301 // check if we need to revert the order in each row
1303 CvPoint2D32f p0 = out_corners[0]->pt, p1 = out_corners[pattern_size.width-1]->pt,
1304 p2 = out_corners[pattern_size.width]->pt;
1305 if( (p1.x - p0.x)*(p2.y - p1.y) - (p1.y - p0.y)*(p2.x - p1.x) < 0 )
1307 if( width % 2 == 0 )
1309 for( i = 0; i < height; i++ )
1310 for( j = 0; j < width/2; j++ )
1311 CV_SWAP( out_corners[i*width+j], out_corners[i*width+width-j-1], c );
1315 for( j = 0; j < width; j++ )
1316 for( i = 0; i < height/2; i++ )
1317 CV_SWAP( out_corners[i*width+j], out_corners[(height - i - 1)*width+j], c );
1322 result = corner_count;
1326 if( result <= 0 && corners )
1328 corner_count = MIN( corner_count, pattern_size.width*pattern_size.height );
1329 for( i = 0; i < corner_count; i++ )
1330 out_corners[i] = corners[i];
1331 result = -corner_count;
1333 if (result == -pattern_size.width*pattern_size.height)
1345 //=====================================================================================
1347 static void icvFindQuadNeighbors( CvCBQuad *quads, int quad_count )
1349 const float thresh_scale = 1.f;
1353 // find quad neighbors
1354 for( idx = 0; idx < quad_count; idx++ )
1356 CvCBQuad* cur_quad = &quads[idx];
1358 // choose the points of the current quadrangle that are close to
1359 // some points of the other quadrangles
1360 // (it can happen for split corners (due to dilation) of the
1361 // checker board). Search only in other quadrangles!
1363 // for each corner of this quadrangle
1364 for( i = 0; i < 4; i++ )
1367 float min_dist = FLT_MAX;
1368 int closest_corner_idx = -1;
1369 CvCBQuad *closest_quad = 0;
1370 CvCBCorner *closest_corner = 0;
1372 if( cur_quad->neighbors[i] )
1375 pt = cur_quad->corners[i]->pt;
1377 // find the closest corner in all other quadrangles
1378 for( k = 0; k < quad_count; k++ )
1383 for( j = 0; j < 4; j++ )
1385 if( quads[k].neighbors[j] )
1388 dx = pt.x - quads[k].corners[j]->pt.x;
1389 dy = pt.y - quads[k].corners[j]->pt.y;
1390 dist = dx * dx + dy * dy;
1392 if( dist < min_dist &&
1393 dist <= cur_quad->edge_len*thresh_scale &&
1394 dist <= quads[k].edge_len*thresh_scale )
1396 closest_corner_idx = j;
1397 closest_quad = &quads[k];
1403 // we found a matching corner point?
1404 if( closest_corner_idx >= 0 && min_dist < FLT_MAX )
1406 // If another point from our current quad is closer to the found corner
1407 // than the current one, then we don't count this one after all.
1408 // This is necessary to support small squares where otherwise the wrong
1409 // corner will get matched to closest_quad;
1410 closest_corner = closest_quad->corners[closest_corner_idx];
1412 for( j = 0; j < 4; j++ )
1414 if( cur_quad->neighbors[j] == closest_quad )
1417 dx = closest_corner->pt.x - cur_quad->corners[j]->pt.x;
1418 dy = closest_corner->pt.y - cur_quad->corners[j]->pt.y;
1420 if( dx * dx + dy * dy < min_dist )
1424 if( j < 4 || cur_quad->count >= 4 || closest_quad->count >= 4 )
1427 // Check that each corner is a neighbor of different quads
1428 for( j = 0; j < closest_quad->count; j++ )
1430 if( closest_quad->neighbors[j] == cur_quad )
1433 if( j < closest_quad->count )
1436 // check whether the closest corner to closest_corner
1437 // is different from cur_quad->corners[i]->pt
1438 for( k = 0; k < quad_count; k++ )
1440 CvCBQuad* q = &quads[k];
1441 if( k == idx || q == closest_quad )
1444 for( j = 0; j < 4; j++ )
1445 if( !q->neighbors[j] )
1447 dx = closest_corner->pt.x - q->corners[j]->pt.x;
1448 dy = closest_corner->pt.y - q->corners[j]->pt.y;
1449 dist = dx*dx + dy*dy;
1450 if( dist < min_dist )
1457 if( k < quad_count )
1460 closest_corner->pt.x = (pt.x + closest_corner->pt.x) * 0.5f;
1461 closest_corner->pt.y = (pt.y + closest_corner->pt.y) * 0.5f;
1463 // We've found one more corner - remember it
1465 cur_quad->neighbors[i] = closest_quad;
1466 cur_quad->corners[i] = closest_corner;
1468 closest_quad->count++;
1469 closest_quad->neighbors[closest_corner_idx] = cur_quad;
1475 //=====================================================================================
1477 // returns corners in clockwise order
1478 // corners don't necessarily start at same position on quad (e.g.,
1482 icvGenerateQuads( CvCBQuad **out_quads, CvCBCorner **out_corners,
1483 CvMemStorage *storage, CvMat *image, int flags )
1486 CvMemStorage *temp_storage = 0;
1494 CV_FUNCNAME( "icvGenerateQuads" );
1498 CvSeq *src_contour = 0;
1500 CvContourEx* board = 0;
1501 CvContourScanner scanner;
1502 int i, idx, min_size;
1504 CV_ASSERT( out_corners && out_quads );
1506 // empiric bound for minimal allowed perimeter for squares
1507 min_size = cvRound( image->cols * image->rows * .03 * 0.01 * 0.92 );
1509 // create temporary storage for contours and the sequence of pointers to found quadrangles
1510 CV_CALL( temp_storage = cvCreateChildMemStorage( storage ));
1511 CV_CALL( root = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSeq*), temp_storage ));
1513 // initialize contour retrieving routine
1514 CV_CALL( scanner = cvStartFindContours( image, temp_storage, sizeof(CvContourEx),
1515 CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE ));
1517 // get all the contours one by one
1518 while( (src_contour = cvFindNextContour( scanner )) != 0 )
1520 CvSeq *dst_contour = 0;
1521 CvRect rect = ((CvContour*)src_contour)->rect;
1523 // reject contours with too small perimeter
1524 if( CV_IS_SEQ_HOLE(src_contour) && rect.width*rect.height >= min_size )
1526 const int min_approx_level = 2, max_approx_level = MAX_CONTOUR_APPROX;
1528 for( approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++ )
1530 dst_contour = cvApproxPoly( src_contour, sizeof(CvContour), temp_storage,
1531 CV_POLY_APPROX_DP, (float)approx_level );
1532 // we call this again on its own output, because sometimes
1533 // cvApproxPoly() does not simplify as much as it should.
1534 dst_contour = cvApproxPoly( dst_contour, sizeof(CvContour), temp_storage,
1535 CV_POLY_APPROX_DP, (float)approx_level );
1537 if( dst_contour->total == 4 )
1541 // reject non-quadrangles
1542 if( dst_contour->total == 4 && cvCheckContourConvexity(dst_contour) )
1545 double d1, d2, p = cvContourPerimeter(dst_contour);
1546 double area = fabs(cvContourArea(dst_contour, CV_WHOLE_SEQ));
1549 for( i = 0; i < 4; i++ )
1550 pt[i] = *(CvPoint*)cvGetSeqElem(dst_contour, i);
1552 dx = pt[0].x - pt[2].x;
1553 dy = pt[0].y - pt[2].y;
1554 d1 = sqrt(dx*dx + dy*dy);
1556 dx = pt[1].x - pt[3].x;
1557 dy = pt[1].y - pt[3].y;
1558 d2 = sqrt(dx*dx + dy*dy);
1560 // philipg. Only accept those quadrangles which are more square
1561 // than rectangular and which are big enough
1563 dx = pt[0].x - pt[1].x;
1564 dy = pt[0].y - pt[1].y;
1565 d3 = sqrt(dx*dx + dy*dy);
1566 dx = pt[1].x - pt[2].x;
1567 dy = pt[1].y - pt[2].y;
1568 d4 = sqrt(dx*dx + dy*dy);
1569 if( !(flags & CV_CALIB_CB_FILTER_QUADS) ||
1570 d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > min_size &&
1571 d1 >= 0.15 * p && d2 >= 0.15 * p )
1573 CvContourEx* parent = (CvContourEx*)(src_contour->v_prev);
1575 if( !board || board->counter < parent->counter )
1577 dst_contour->v_prev = (CvSeq*)parent;
1578 //for( i = 0; i < 4; i++ ) cvLine( debug_img, pt[i], pt[(i+1)&3], cvScalar(200,255,255), 1, CV_AA, 0 );
1579 cvSeqPush( root, &dst_contour );
1585 // finish contour retrieving
1586 cvEndFindContours( &scanner );
1588 // allocate quad & corner buffers
1589 CV_CALL( *out_quads = (CvCBQuad*)cvAlloc((EXTRA_QUADS+root->total) * sizeof((*out_quads)[0])));
1590 CV_CALL( *out_corners = (CvCBCorner*)cvAlloc((EXTRA_QUADS+root->total) * 4 * sizeof((*out_corners)[0])));
1592 // Create array of quads structures
1593 for( idx = 0; idx < root->total; idx++ )
1595 CvCBQuad* q = &(*out_quads)[quad_count];
1596 src_contour = *(CvSeq**)cvGetSeqElem( root, idx );
1597 if( (flags & CV_CALIB_CB_FILTER_QUADS) && src_contour->v_prev != (CvSeq*)board )
1601 memset( q, 0, sizeof(*q) );
1603 assert( src_contour->total == 4 );
1604 for( i = 0; i < 4; i++ )
1606 CvPoint2D32f pt = cvPointTo32f(*(CvPoint*)cvGetSeqElem(src_contour, i));
1607 CvCBCorner* corner = &(*out_corners)[quad_count*4 + i];
1609 memset( corner, 0, sizeof(*corner) );
1611 q->corners[i] = corner;
1613 q->edge_len = FLT_MAX;
1614 for( i = 0; i < 4; i++ )
1616 float dx = q->corners[i]->pt.x - q->corners[(i+1)&3]->pt.x;
1617 float dy = q->corners[i]->pt.y - q->corners[(i+1)&3]->pt.y;
1618 float d = dx*dx + dy*dy;
1619 if( q->edge_len > d )
1627 if( cvGetErrStatus() < 0 )
1630 cvFree( out_quads );
1632 cvFree( out_corners );
1636 cvReleaseMemStorage( &temp_storage );
1641 //=====================================================================================
1643 static int is_equal_quad( const void* _a, const void* _b, void* )
1645 CvRect a = (*((CvContour**)_a))->rect;
1646 CvRect b = (*((CvContour**)_b))->rect;
1648 int dx = MIN( b.x + b.width - 1, a.x + a.width - 1) - MAX( b.x, a.x);
1649 int dy = MIN( b.y + b.height - 1, a.y + a.height - 1) - MAX( b.y, a.y);
1650 int w = (a.width + b.width)>>1;
1651 int h = (a.height + b.height)>>1;
1653 if( dx > w*0.75 && dy > h*0.75 && dx < w*1.25 && dy < h*1.25 ) return 1;
1659 icvGenerateQuadsEx( CvCBQuad **out_quads, CvCBCorner **out_corners,
1660 CvMemStorage *storage, CvMat *image, CvMat *thresh_img, int dilations, int flags )
1664 CvMemStorage *temp_storage = 0;
1672 CV_FUNCNAME( "icvGenerateQuads" );
1676 CvSeq *src_contour = 0;
1677 CvSeq *root, *root_tmp;
1678 CvContourEx* board = 0;
1679 CvContourScanner scanner;
1680 int i, idx, min_size;
1681 int step_level = 25;
1683 CV_ASSERT( out_corners && out_quads );
1685 // empiric bound for minimal allowed perimeter for squares
1686 min_size = cvRound( image->cols * image->rows * .03 * 0.01 * 0.92 );
1688 // create temporary storage for contours and the sequence of pointers to found quadrangles
1689 CV_CALL( temp_storage = cvCreateChildMemStorage( storage ));
1690 CV_CALL( root_tmp = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSeq*), temp_storage ));
1691 CV_CALL( root = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSeq*), temp_storage ));
1693 //perform contours slicing
1694 cvEqualizeHist(image,image);
1695 for(l = step_level; l < 256-step_level; l+= step_level)
1697 cvThreshold( image, thresh_img, l, 255, CV_THRESH_BINARY );
1698 cvDilate( thresh_img, thresh_img, 0, dilations );
1700 //draw frame to extract edge quads
1701 cvRectangle( thresh_img, cvPoint(0,0), cvPoint(thresh_img->cols-1,
1702 thresh_img->rows-1), CV_RGB(255,255,255), 3, 8);
1704 // initialize contour retrieving routine
1705 CV_CALL( scanner = cvStartFindContours( thresh_img, temp_storage, sizeof(CvContourEx),
1706 CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE ));
1708 // get all the contours one by one
1709 while( (src_contour = cvFindNextContour( scanner )) != 0 )
1711 CvSeq *dst_contour = 0;
1712 CvRect rect = ((CvContour*)src_contour)->rect;
1714 // reject contours with too small perimeter
1715 if( CV_IS_SEQ_HOLE(src_contour) && rect.width*rect.height >= min_size )
1717 const int min_approx_level = 2, max_approx_level = MAX_CONTOUR_APPROX;
1719 for( approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++ )
1721 dst_contour = cvApproxPoly( src_contour, sizeof(CvContour), temp_storage,
1722 CV_POLY_APPROX_DP, (float)approx_level );
1723 // we call this again on its own output, because sometimes
1724 // cvApproxPoly() does not simplify as much as it should.
1725 dst_contour = cvApproxPoly( dst_contour, sizeof(CvContour), temp_storage,
1726 CV_POLY_APPROX_DP, (float)approx_level );
1728 if( dst_contour->total == 4 )
1732 // reject non-quadrangles
1733 if( dst_contour->total == 4 && cvCheckContourConvexity(dst_contour) )
1736 double d1, d2, p = cvContourPerimeter(dst_contour);
1737 double area = fabs(cvContourArea(dst_contour, CV_WHOLE_SEQ));
1740 for( i = 0; i < 4; i++ )
1741 pt[i] = *(CvPoint*)cvGetSeqElem(dst_contour, i);
1743 //check border condition. if this is edge square we will add this as is
1744 int edge_flag = 0, eps = 2;
1745 for( i = 0; i < 4; i++ )
1746 if( pt[i].x <= eps || pt[i].y <= eps ||
1747 pt[i].x >= image->width - eps ||
1748 pt[i].y >= image->height - eps ) edge_flag = 1;
1750 dx = pt[0].x - pt[2].x;
1751 dy = pt[0].y - pt[2].y;
1752 d1 = sqrt(dx*dx + dy*dy);
1754 dx = pt[1].x - pt[3].x;
1755 dy = pt[1].y - pt[3].y;
1756 d2 = sqrt(dx*dx + dy*dy);
1758 // philipg. Only accept those quadrangles which are more square
1759 // than rectangular and which are big enough
1761 dx = pt[0].x - pt[1].x;
1762 dy = pt[0].y - pt[1].y;
1763 d3 = sqrt(dx*dx + dy*dy);
1764 dx = pt[1].x - pt[2].x;
1765 dy = pt[1].y - pt[2].y;
1766 d4 = sqrt(dx*dx + dy*dy);
1768 (!(flags & CV_CALIB_CB_FILTER_QUADS) ||
1769 d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > min_size &&
1770 d1 >= 0.15 * p && d2 >= 0.15 * p) )
1772 CvContourEx* parent = (CvContourEx*)(src_contour->v_prev);
1774 if( !board || board->counter < parent->counter )
1776 dst_contour->v_prev = (CvSeq*)parent;
1777 //for( i = 0; i < 4; i++ ) cvLine( debug_img, pt[i], pt[(i+1)&3], cvScalar(200,255,255), 1, CV_AA, 0 );
1778 cvSeqPush( root_tmp, &dst_contour );
1783 // finish contour retrieving
1784 cvEndFindContours( &scanner );
1788 // Perform clustering of extracted quads
1789 // Same quad can be extracted from different binarization levels
1790 if( root_tmp->total )
1793 int n_quads = cvSeqPartition( root_tmp, temp_storage, &idx_seq, is_equal_quad, 0 );
1794 for( i = 0; i < n_quads; i++ )
1796 //extract biggest quad in group
1799 for( int j = 0; j < root_tmp->total; j++ )
1801 int index = *(int*)cvGetSeqElem(idx_seq, j);
1802 if(index!=i) continue;
1803 CvContour* cnt = *(CvContour**)cvGetSeqElem(root_tmp, j);
1804 if(cnt->rect.width > max_size)
1806 max_size = cnt->rect.width;
1807 max_seq = (CvSeq*)cnt;
1810 cvSeqPush( root, &max_seq);
1814 // allocate quad & corner buffers
1815 CV_CALL( *out_quads = (CvCBQuad*)cvAlloc((EXTRA_QUADS+root->total) * sizeof((*out_quads)[0])));
1816 CV_CALL( *out_corners = (CvCBCorner*)cvAlloc((EXTRA_QUADS+root->total) * 4 * sizeof((*out_corners)[0])));
1818 // Create array of quads structures
1819 for( idx = 0; idx < root->total; idx++ )
1821 CvCBQuad* q = &(*out_quads)[quad_count];
1822 src_contour = *(CvSeq**)cvGetSeqElem( root, idx );
1823 if( (flags & CV_CALIB_CB_FILTER_QUADS) && src_contour->v_prev != (CvSeq*)board )
1827 memset( q, 0, sizeof(*q) );
1829 assert( src_contour->total == 4 );
1830 for( i = 0; i < 4; i++ )
1832 CvPoint2D32f pt = cvPointTo32f(*(CvPoint*)cvGetSeqElem(src_contour, i));
1833 CvCBCorner* corner = &(*out_corners)[quad_count*4 + i];
1835 memset( corner, 0, sizeof(*corner) );
1837 q->corners[i] = corner;
1839 q->edge_len = FLT_MAX;
1840 for( i = 0; i < 4; i++ )
1842 float dx = q->corners[i]->pt.x - q->corners[(i+1)&3]->pt.x;
1843 float dy = q->corners[i]->pt.y - q->corners[(i+1)&3]->pt.y;
1844 float d = dx*dx + dy*dy;
1845 if( q->edge_len > d )
1853 if( cvGetErrStatus() < 0 )
1856 cvFree( out_quads );
1858 cvFree( out_corners );
1862 cvReleaseMemStorage( &temp_storage );
1868 cvDrawChessboardCorners( CvArr* _image, CvSize pattern_size,
1869 CvPoint2D32f* corners, int count, int found )
1871 CV_FUNCNAME( "cvDrawChessboardCorners" );
1875 const int shift = 0;
1876 const int radius = 4;
1877 const int r = radius*(1 << shift);
1881 int type, cn, line_type;
1883 CV_CALL( image = cvGetMat( _image, &stub ));
1885 type = CV_MAT_TYPE(image->type);
1886 cn = CV_MAT_CN(type);
1887 if( cn != 1 && cn != 3 && cn != 4 )
1888 CV_ERROR( CV_StsUnsupportedFormat, "Number of channels must be 1, 3 or 4" );
1890 switch( CV_MAT_DEPTH(image->type) )
1902 CV_ERROR( CV_StsUnsupportedFormat,
1903 "Only 8-bit, 16-bit or floating-point 32-bit images are supported" );
1906 line_type = type == CV_8UC1 || type == CV_8UC3 ? CV_AA : 8;
1910 CvScalar color = {{0,0,255}};
1912 color = cvScalarAll(200);
1913 color.val[0] *= scale;
1914 color.val[1] *= scale;
1915 color.val[2] *= scale;
1916 color.val[3] *= scale;
1918 for( i = 0; i < count; i++ )
1921 pt.x = cvRound(corners[i].x*(1 << shift));
1922 pt.y = cvRound(corners[i].y*(1 << shift));
1923 cvLine( image, cvPoint( pt.x - r, pt.y - r ),
1924 cvPoint( pt.x + r, pt.y + r ), color, 1, line_type, shift );
1925 cvLine( image, cvPoint( pt.x - r, pt.y + r),
1926 cvPoint( pt.x + r, pt.y - r), color, 1, line_type, shift );
1927 cvCircle( image, pt, r+(1<<shift), color, 1, line_type, shift );
1933 CvPoint prev_pt = {0, 0};
1934 const int line_max = 7;
1935 static const CvScalar line_colors[line_max] =
1946 for( y = 0, i = 0; y < pattern_size.height; y++ )
1948 CvScalar color = line_colors[y % line_max];
1950 color = cvScalarAll(200);
1951 color.val[0] *= scale;
1952 color.val[1] *= scale;
1953 color.val[2] *= scale;
1954 color.val[3] *= scale;
1956 for( x = 0; x < pattern_size.width; x++, i++ )
1959 pt.x = cvRound(corners[i].x*(1 << shift));
1960 pt.y = cvRound(corners[i].y*(1 << shift));
1963 cvLine( image, prev_pt, pt, color, 1, line_type, shift );
1965 cvLine( image, cvPoint(pt.x - r, pt.y - r),
1966 cvPoint(pt.x + r, pt.y + r), color, 1, line_type, shift );
1967 cvLine( image, cvPoint(pt.x - r, pt.y + r),
1968 cvPoint(pt.x + r, pt.y - r), color, 1, line_type, shift );
1969 cvCircle( image, pt, r+(1<<shift), color, 1, line_type, shift );