1 //M*//////////////////////////////////////////////////////////////////////////////////////
2 //
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
8 //
9 //
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
21 //
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
25 //
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
28 //
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
39 //
40 //M*/
41
42 /************************************************************************************\
43 This is improved variant of chessboard corner detection algorithm that
44 uses a graph of connected quads. It is based on the code contributed
45 by Vladimir Vezhnevets and Philip Gruebele.
46 Here is the copyright notice from the original Vladimir's code:
47 ===============================================================
48
49 The algorithms developed and implemented by Vezhnevets Vldimir
50 aka Dead Moroz (vvp@graphics.cs.msu.ru)
51 See http://graphics.cs.msu.su/en/research/calibration/opencv.html
52 for detailed information.
53
54 Reliability additions and modifications made by Philip Gruebele.
55 <a href="mailto:pgruebele@cox.net">pgruebele@cox.net</a>
56
57 Some further improvements for detection of partially ocluded boards at non-ideal
58 lighting conditions have been made by Alex Bovyrin and Kurt Kolonige
59
60 \************************************************************************************/
61
62 #include "_cv.h"
63 #include <stdarg.h>
64
65 //#define DEBUG_CHESSBOARD
66 #ifdef DEBUG_CHESSBOARD
PRINTF(const char * fmt,...)67 static int PRINTF( const char* fmt, ... )
68 {
69 va_list args;
70 va_start(args, fmt);
71 return vprintf(fmt, args);
72 }
73 #include "../../otherlibs/highgui/highgui.h"
74 #else
PRINTF(const char *,...)75 static int PRINTF( const char*, ... )
76 {
77 return 0;
78 }
79 #endif
80
81
82 //=====================================================================================
83 // Implementation for the enhanced calibration object detection
84 //=====================================================================================
85
86 #define MAX_CONTOUR_APPROX 7
87
88 typedef struct CvContourEx
89 {
90 CV_CONTOUR_FIELDS()
91 int counter;
92 }
93 CvContourEx;
94
95 //=====================================================================================
96
97 /// Corner info structure
98 /** This structure stores information about the chessboard corner.*/
99 typedef struct CvCBCorner
100 {
101 CvPoint2D32f pt; // Coordinates of the corner
102 int row; // Board row index
103 int count; // Number of neighbor corners
104 struct CvCBCorner* neighbors[4]; // Neighbor corners
105 }
106 CvCBCorner;
107
108 //=====================================================================================
109 /// Quadrangle contour info structure
110 /** This structure stores information about the chessboard quadrange.*/
111 typedef struct CvCBQuad
112 {
113 int count; // Number of quad neighbors
114 int group_idx; // quad group ID
115 int row, col; // row and column of this quad
116 bool ordered; // true if corners/neighbors are ordered counter-clockwise
117 float edge_len; // quad edge len, in pix^2
118 // neighbors and corners are synced, i.e., neighbor 0 shares corner 0
119 CvCBCorner *corners[4]; // Coordinates of quad corners
120 struct CvCBQuad *neighbors[4]; // Pointers of quad neighbors
121 }
122 CvCBQuad;
123
124 //=====================================================================================
125
126 //static CvMat* debug_img = 0;
127
128 static int icvGenerateQuads( CvCBQuad **quads, CvCBCorner **corners,
129 CvMemStorage *storage, CvMat *image, int flags );
130
131 static int
132 icvGenerateQuadsEx( CvCBQuad **out_quads, CvCBCorner **out_corners,
133 CvMemStorage *storage, CvMat *image, CvMat *thresh_img, int dilation, int flags );
134
135 static void icvFindQuadNeighbors( CvCBQuad *quads, int quad_count );
136
137 static int icvFindConnectedQuads( CvCBQuad *quads, int quad_count,
138 CvCBQuad **quad_group, int group_idx,
139 CvMemStorage* storage );
140
141 static int icvCheckQuadGroup( CvCBQuad **quad_group, int count,
142 CvCBCorner **out_corners, CvSize pattern_size );
143
144 static int icvCleanFoundConnectedQuads( int quad_count,
145 CvCBQuad **quads, CvSize pattern_size );
146
147 static int icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,
148 int *all_count, CvCBQuad **all_quads, CvCBCorner **corners,
149 CvSize pattern_size, CvMemStorage* storage );
150
151 static void icvOrderQuad(CvCBQuad *quad, CvCBCorner *corner, int common);
152
153 static int icvTrimCol(CvCBQuad **quads, int count, int col, int dir);
154
155 static int icvTrimRow(CvCBQuad **quads, int count, int row, int dir);
156
157 static int icvAddOuterQuad(CvCBQuad *quad, CvCBQuad **quads, int quad_count,
158 CvCBQuad **all_quads, int all_count, CvCBCorner **corners);
159
160 static void icvRemoveQuadFromGroup(CvCBQuad **quads, int count, CvCBQuad *q0);
161
162 static int icvCheckBoardMonotony( CvPoint2D32f* corners, CvSize pattern_size );
163
164 #if 0
165 static void
166 icvCalcAffineTranf2D32f(CvPoint2D32f* pts1, CvPoint2D32f* pts2, int count, CvMat* affine_trans)
167 {
168 int i, j;
169 int real_count = 0;
170 for( j = 0; j < count; j++ )
171 {
172 if( pts1[j].x >= 0 ) real_count++;
173 }
174 if(real_count < 3) return;
175 CvMat* xy = cvCreateMat( 2*real_count, 6, CV_32FC1 );
176 CvMat* uv = cvCreateMat( 2*real_count, 1, CV_32FC1 );
177 //estimate affine transfromation
178 for( i = 0, j = 0; j < count; j++ )
179 {
180 if( pts1[j].x >= 0 )
181 {
182 CV_MAT_ELEM( *xy, float, i*2+1, 2 ) = CV_MAT_ELEM( *xy, float, i*2, 0 ) = pts2[j].x;
183 CV_MAT_ELEM( *xy, float, i*2+1, 3 ) = CV_MAT_ELEM( *xy, float, i*2, 1 ) = pts2[j].y;
184 CV_MAT_ELEM( *xy, float, i*2, 2 ) = CV_MAT_ELEM( *xy, float, i*2, 3 ) = CV_MAT_ELEM( *xy, float, i*2, 5 ) = \
185 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;
186 CV_MAT_ELEM( *xy, float, i*2, 4 ) = CV_MAT_ELEM( *xy, float, i*2+1, 5 ) = 1;
187 CV_MAT_ELEM( *uv, float, i*2, 0 ) = pts1[j].x;
188 CV_MAT_ELEM( *uv, float, i*2+1, 0 ) = pts1[j].y;
189 i++;
190 }
191 }
192
193 cvSolve( xy, uv, affine_trans, CV_SVD );
194 cvReleaseMat(&xy);
195 cvReleaseMat(&uv);
196 }
197 #endif
198
199 CV_IMPL
cvFindChessboardCorners(const void * arr,CvSize pattern_size,CvPoint2D32f * out_corners,int * out_corner_count,int flags)200 int cvFindChessboardCorners( const void* arr, CvSize pattern_size,
201 CvPoint2D32f* out_corners, int* out_corner_count,
202 int flags )
203 {
204 int k = 0;
205 const int min_dilations = 0;
206 const int max_dilations = 3;
207 int found = 0;
208 CvMat* norm_img = 0;
209 CvMat* thresh_img = 0;
210 #ifdef DEBUG_CHESSBOARD
211 IplImage *dbg_img = 0;
212 IplImage *dbg1_img = 0;
213 IplImage *dbg2_img = 0;
214 #endif
215 CvMemStorage* storage = 0;
216
217 CvCBQuad *quads = 0, **quad_group = 0;
218 CvCBCorner *corners = 0, **corner_group = 0;
219
220 int expected_corners_num = (pattern_size.width/2+1)*(pattern_size.height/2+1);
221
222 if( out_corner_count )
223 *out_corner_count = 0;
224
225 CV_FUNCNAME( "cvFindChessBoardCornerGuesses2" );
226
227 __BEGIN__;
228
229 int quad_count = 0, group_idx = 0, i = 0, dilations = 0;
230 CvMat stub, *img = (CvMat*)arr;
231
232 CV_CALL( img = cvGetMat( img, &stub ));
233 //debug_img = img;
234
235 if( CV_MAT_DEPTH( img->type ) != CV_8U || CV_MAT_CN( img->type ) == 2 )
236 CV_ERROR( CV_StsUnsupportedFormat, "Only 8-bit grayscale or color images are supported" );
237
238 if( pattern_size.width <= 2 || pattern_size.height <= 2 )
239 CV_ERROR( CV_StsOutOfRange, "Both width and height of the pattern should have bigger than 2" );
240
241 if( !out_corners )
242 CV_ERROR( CV_StsNullPtr, "Null pointer to corners" );
243
244 CV_CALL( storage = cvCreateMemStorage(0) );
245 CV_CALL( thresh_img = cvCreateMat( img->rows, img->cols, CV_8UC1 ));
246
247 #ifdef DEBUG_CHESSBOARD
248 CV_CALL( dbg_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3 ));
249 CV_CALL( dbg1_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3 ));
250 CV_CALL( dbg2_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3 ));
251 #endif
252
253 if( CV_MAT_CN(img->type) != 1 || (flags & CV_CALIB_CB_NORMALIZE_IMAGE) )
254 {
255 // equalize the input image histogram -
256 // that should make the contrast between "black" and "white" areas big enough
257 CV_CALL( norm_img = cvCreateMat( img->rows, img->cols, CV_8UC1 ));
258
259 if( CV_MAT_CN(img->type) != 1 )
260 {
261 CV_CALL( cvCvtColor( img, norm_img, CV_BGR2GRAY ));
262 img = norm_img;
263 }
264
265 if( flags & CV_CALIB_CB_NORMALIZE_IMAGE )
266 {
267 cvEqualizeHist( img, norm_img );
268 img = norm_img;
269 }
270 }
271
272 // Try our standard "1" dilation, but if the pattern is not found, iterate the whole procedure with higher dilations.
273 // This is necessary because some squares simply do not separate properly with a single dilation. However,
274 // we want to use the minimum number of dilations possible since dilations cause the squares to become smaller,
275 // making it difficult to detect smaller squares.
276 for( k = 0; k < 2; k++ )
277 {
278 for( dilations = min_dilations; dilations <= max_dilations; dilations++ )
279 {
280 if (found) break; // already found it
281
282 if( k == 1 )
283 {
284 //Pattern was not found using binarization
285 // Run multi-level quads extraction
286 // In case one-level binarization did not give enough number of quads
287 CV_CALL( quad_count = icvGenerateQuadsEx( &quads, &corners, storage, img, thresh_img, dilations, flags ));
288 PRINTF("EX quad count: %d/%d\n", quad_count, expected_corners_num);
289 }
290 else
291 {
292 // convert the input grayscale image to binary (black-n-white)
293 if( flags & CV_CALIB_CB_ADAPTIVE_THRESH )
294 {
295 int block_size = cvRound(MIN(img->cols,img->rows)*0.2)|1;
296
297 // convert to binary
298 cvAdaptiveThreshold( img, thresh_img, 255,
299 CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, block_size, 0 );
300 if (dilations > 0)
301 cvDilate( thresh_img, thresh_img, 0, dilations-1 );
302 }
303 else
304 {
305 // Make dilation before the thresholding.
306 // It splits chessboard corners
307 //cvDilate( img, thresh_img, 0, 1 );
308
309 // empiric threshold level
310 double mean = cvMean( img );
311 int thresh_level = cvRound( mean - 10 );
312 thresh_level = MAX( thresh_level, 10 );
313
314 cvThreshold( img, thresh_img, thresh_level, 255, CV_THRESH_BINARY );
315 cvDilate( thresh_img, thresh_img, 0, dilations );
316 }
317
318
319
320 #ifdef DEBUG_CHESSBOARD
321 cvCvtColor(thresh_img,dbg_img,CV_GRAY2BGR);
322 #endif
323
324 // So we can find rectangles that go to the edge, we draw a white line around the image edge.
325 // Otherwise FindContours will miss those clipped rectangle contours.
326 // The border color will be the image mean, because otherwise we risk screwing up filters like cvSmooth()...
327 cvRectangle( thresh_img, cvPoint(0,0), cvPoint(thresh_img->cols-1,
328 thresh_img->rows-1), CV_RGB(255,255,255), 3, 8);
329
330 CV_CALL( quad_count = icvGenerateQuads( &quads, &corners, storage, thresh_img, flags ));
331
332
333 PRINTF("Quad count: %d/%d\n", quad_count, expected_corners_num);
334 }
335
336
337 #ifdef DEBUG_CHESSBOARD
338 cvCopy(dbg_img, dbg1_img);
339 cvNamedWindow("all_quads", 1);
340 // copy corners to temp array
341 for( i = 0; i < quad_count; i++ )
342 {
343 for (int k=0; k<4; k++)
344 {
345 CvPoint2D32f pt1, pt2;
346 CvScalar color = CV_RGB(30,255,30);
347 pt1 = quads[i].corners[k]->pt;
348 pt2 = quads[i].corners[(k+1)%4]->pt;
349 pt2.x = (pt1.x + pt2.x)/2;
350 pt2.y = (pt1.y + pt2.y)/2;
351 if (k>0)
352 color = CV_RGB(200,200,0);
353 cvLine( dbg1_img, cvPointFrom32f(pt1), cvPointFrom32f(pt2), color, 3, 8);
354 }
355 }
356
357
358 // cvShowImage("all_quads", (IplImage*)dbg1_img);
359 cvWaitKey();
360 #endif
361
362
363 if( quad_count <= 0 )
364 continue;
365
366 // Find quad's neighbors
367 CV_CALL( icvFindQuadNeighbors( quads, quad_count ));
368
369 // allocate extra for adding in icvOrderFoundQuads
370 CV_CALL( quad_group = (CvCBQuad**)cvAlloc( sizeof(quad_group[0]) * (quad_count+quad_count / 2)));
371 CV_CALL( corner_group = (CvCBCorner**)cvAlloc( sizeof(corner_group[0]) * (quad_count+quad_count / 2)*4 ));
372
373 for( group_idx = 0; ; group_idx++ )
374 {
375 int count = 0;
376 CV_CALL( count = icvFindConnectedQuads( quads, quad_count, quad_group, group_idx, storage ));
377
378 int icount = count;
379 if( count == 0 )
380 break;
381
382 // order the quad corners globally
383 // maybe delete or add some
384 PRINTF("Starting ordering of inner quads\n");
385 count = icvOrderFoundConnectedQuads(count, quad_group, &quad_count, &quads, &corners,
386 pattern_size, storage );
387 PRINTF("Orig count: %d After ordering: %d\n", icount, count);
388
389
390 #ifdef DEBUG_CHESSBOARD
391 cvCopy(dbg_img,dbg2_img);
392 cvNamedWindow("connected_group", 1);
393 // copy corners to temp array
394 for( i = 0; i < quad_count; i++ )
395 {
396 if (quads[i].group_idx == group_idx)
397 for (int k=0; k<4; k++)
398 {
399 CvPoint2D32f pt1, pt2;
400 CvScalar color = CV_RGB(30,255,30);
401 if (quads[i].ordered)
402 color = CV_RGB(255,30,30);
403 pt1 = quads[i].corners[k]->pt;
404 pt2 = quads[i].corners[(k+1)%4]->pt;
405 pt2.x = (pt1.x + pt2.x)/2;
406 pt2.y = (pt1.y + pt2.y)/2;
407 if (k>0)
408 color = CV_RGB(200,200,0);
409 cvLine( dbg2_img, cvPointFrom32f(pt1), cvPointFrom32f(pt2), color, 3, 8);
410 }
411 }
412 // cvShowImage("connected_group", (IplImage*)dbg2_img);
413 cvWaitKey();
414 #endif
415
416 if (count == 0)
417 continue; // haven't found inner quads
418
419
420 // If count is more than it should be, this will remove those quads
421 // which cause maximum deviation from a nice square pattern.
422 CV_CALL( count = icvCleanFoundConnectedQuads( count, quad_group, pattern_size ));
423 PRINTF("Connected group: %d orig count: %d cleaned: %d\n", group_idx, icount, count);
424
425 CV_CALL( count = icvCheckQuadGroup( quad_group, count, corner_group, pattern_size ));
426 PRINTF("Connected group: %d count: %d cleaned: %d\n", group_idx, icount, count);
427
428 if( count > 0 || (out_corner_count && -count > *out_corner_count) )
429 {
430 int n = count > 0 ? pattern_size.width * pattern_size.height : -count;
431 n = MIN( n, pattern_size.width * pattern_size.height );
432
433 // copy corners to output array
434 for( i = 0; i < n; i++ )
435 out_corners[i] = corner_group[i]->pt;
436
437 if( out_corner_count )
438 *out_corner_count = n;
439
440 if( count > 0 )
441 {
442 found = 1;
443 break;
444 }
445 }
446 }
447
448 cvFree( &quads );
449 cvFree( &corners );
450 cvFree( &quad_group );
451 cvFree( &corner_group );
452 }//dilations
453 }//
454
455
456 __END__;
457
458 cvReleaseMemStorage( &storage );
459 cvReleaseMat( &norm_img );
460 cvReleaseMat( &thresh_img );
461 cvFree( &quads );
462 cvFree( &corners );
463
464 if( found )
465 found = icvCheckBoardMonotony( out_corners, pattern_size );
466
467 if( found && pattern_size.height % 2 == 0 && pattern_size.width % 2 == 0 )
468 {
469 int last_row = (pattern_size.height-1)*pattern_size.width;
470 double dy0 = out_corners[last_row].y - out_corners[0].y;
471 if( dy0 < 0 )
472 {
473 int i, n = pattern_size.width*pattern_size.height;
474 for( i = 0; i < n/2; i++ )
475 {
476 CvPoint2D32f temp;
477 CV_SWAP(out_corners[i], out_corners[n-i-1], temp);
478 }
479 }
480 }
481
482 return found;
483 }
484
485 //
486 // Checks that each board row and column is pretty much monotonous curve:
487 // It analyzes each row and each column of the chessboard as following:
488 // for each corner c lying between end points in the same row/column it checks that
489 // the point projection to the line segment (a,b) is lying between projections
490 // of the neighbor corners in the same row/column.
491 //
492 // This function has been created as temporary workaround for the bug in current implementation
493 // of cvFindChessboardCornes that produces absolutely unordered sets of corners.
494 //
495
496 static int
icvCheckBoardMonotony(CvPoint2D32f * corners,CvSize pattern_size)497 icvCheckBoardMonotony( CvPoint2D32f* corners, CvSize pattern_size )
498 {
499 int i, j, k;
500
501 for( k = 0; k < 2; k++ )
502 {
503 for( i = 0; i < (k == 0 ? pattern_size.height : pattern_size.width); i++ )
504 {
505 CvPoint2D32f a = k == 0 ? corners[i*pattern_size.width] : corners[i];
506 CvPoint2D32f b = k == 0 ? corners[(i+1)*pattern_size.width-1] :
507 corners[(pattern_size.height-1)*pattern_size.width + i];
508 float prevt = 0, dx0 = b.x - a.x, dy0 = b.y - a.y;
509 if( fabs(dx0) + fabs(dy0) < FLT_EPSILON )
510 return 0;
511 for( j = 1; j < (k == 0 ? pattern_size.width : pattern_size.height) - 1; j++ )
512 {
513 CvPoint2D32f c = k == 0 ? corners[i*pattern_size.width + j] :
514 corners[j*pattern_size.width + i];
515 float t = ((c.x - a.x)*dx0 + (c.y - a.y)*dy0)/(dx0*dx0 + dy0*dy0);
516 if( t < prevt || t > 1 )
517 return 0;
518 prevt = t;
519 }
520 }
521 }
522
523 return 1;
524 }
525
526 //
527 // order a group of connected quads
528 // order of corners:
529 // 0 is top left
530 // clockwise from there
531 // note: "top left" is nominal, depends on initial ordering of starting quad
532 // but all other quads are ordered consistently
533 //
534 // can change the number of quads in the group
535 // can add quads, so we need to have quad/corner arrays passed in
536 //
537
538 static int
icvOrderFoundConnectedQuads(int quad_count,CvCBQuad ** quads,int * all_count,CvCBQuad ** all_quads,CvCBCorner ** corners,CvSize pattern_size,CvMemStorage * storage)539 icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,
540 int *all_count, CvCBQuad **all_quads, CvCBCorner **corners,
541 CvSize pattern_size, CvMemStorage* storage )
542 {
543 CvMemStorage* temp_storage = cvCreateChildMemStorage( storage );
544 CvSeq* stack = cvCreateSeq( 0, sizeof(*stack), sizeof(void*), temp_storage );
545 int i;
546
547 // first find an interior quad
548 CvCBQuad *start = NULL;
549 for (i=0; i<quad_count; i++)
550 {
551 if (quads[i]->count == 4)
552 {
553 start = quads[i];
554 break;
555 }
556 }
557
558 if (start == NULL)
559 {
560 cvReleaseMemStorage( &temp_storage );
561 return 0; // no 4-connected quad
562 }
563
564 // start with first one, assign rows/cols
565 int row_min = 0, col_min = 0, row_max=0, col_max = 0;
566 #define HSIZE 20
567 int col_hist[HSIZE*2];
568 int row_hist[HSIZE*2]; // bad programming, should allow variable size
569
570 for (i=0; i<HSIZE*2; i++) // init to zero
571 col_hist[i] = row_hist[i] = 0;
572 cvSeqPush(stack, &start);
573 start->row = 0;
574 start->col = 0;
575 start->ordered = true;
576
577 // Recursively order the quads so that all position numbers (e.g.,
578 // 0,1,2,3) are in the at the same relative corner (e.g., lower right).
579
580 while( stack->total )
581 {
582 CvCBQuad* q;
583 cvSeqPop( stack, &q );
584 int col = q->col;
585 int row = q->row;
586 col_hist[col+HSIZE]++;
587 row_hist[row+HSIZE]++;
588
589 // check min/max
590 if (row > row_max) row_max = row;
591 if (row < row_min) row_min = row;
592 if (col > col_max) col_max = col;
593 if (col < col_min) col_min = col;
594
595 for(int i = 0; i < 4; i++ )
596 {
597 CvCBQuad *neighbor = q->neighbors[i];
598 switch(i) // adjust col, row for this quad
599 { // start at top left, go clockwise
600 case 0:
601 row--; col--; break;
602 case 1:
603 col += 2; break;
604 case 2:
605 row += 2; break;
606 case 3:
607 col -= 2; break;
608 }
609
610 // just do inside quads
611 if (neighbor && neighbor->ordered == false && neighbor->count == 4)
612 {
613 PRINTF("col: %d row: %d\n", col, row);
614 icvOrderQuad(neighbor, q->corners[i], (i+2)%4); // set in order
615 neighbor->ordered = true;
616 neighbor->row = row;
617 neighbor->col = col;
618 cvSeqPush( stack, &neighbor );
619 }
620 }
621 }
622
623 cvReleaseMemStorage( &temp_storage );
624
625 for (i=col_min; i<=col_max; i++)
626 PRINTF("HIST[%d] = %d\n", i, col_hist[i+HSIZE]);
627
628 // analyze inner quad structure
629 int w = pattern_size.width - 1;
630 int h = pattern_size.height - 1;
631 int drow = row_max - row_min + 1;
632 int dcol = col_max - col_min + 1;
633
634 // normalize pattern and found quad indices
635 if ((w > h && dcol < drow) ||
636 (w < h && drow < dcol))
637 {
638 h = pattern_size.width - 1;
639 w = pattern_size.height - 1;
640 }
641
642 PRINTF("Size: %dx%d Pattern: %dx%d\n", dcol, drow, w, h);
643
644 // check if there are enough inner quads
645 if (dcol < w || drow < h) // found enough inner quads?
646 {
647 PRINTF("Too few inner quad rows/cols\n");
648 return 0; // no, return
649 }
650
651 // too many columns, not very common
652 if (dcol == w+1) // too many, trim
653 {
654 PRINTF("Trimming cols\n");
655 if (col_hist[col_max+HSIZE] > col_hist[col_min+HSIZE])
656 {
657 PRINTF("Trimming left col\n");
658 quad_count = icvTrimCol(quads,quad_count,col_min,-1);
659 }
660 else
661 {
662 PRINTF("Trimming right col\n");
663 quad_count = icvTrimCol(quads,quad_count,col_max,+1);
664 }
665 }
666
667 // too many rows, not very common
668 if (drow == h+1) // too many, trim
669 {
670 PRINTF("Trimming rows\n");
671 if (row_hist[row_max+HSIZE] > row_hist[row_min+HSIZE])
672 {
673 PRINTF("Trimming top row\n");
674 quad_count = icvTrimRow(quads,quad_count,row_min,-1);
675 }
676 else
677 {
678 PRINTF("Trimming bottom row\n");
679 quad_count = icvTrimRow(quads,quad_count,row_max,+1);
680 }
681 }
682
683
684 // check edges of inner quads
685 // if there is an outer quad missing, fill it in
686 // first order all inner quads
687 int found = 0;
688 for (i=0; i<quad_count; i++)
689 {
690 if (quads[i]->count == 4)
691 { // ok, look at neighbors
692 int col = quads[i]->col;
693 int row = quads[i]->row;
694 for (int j=0; j<4; j++)
695 {
696 switch(j) // adjust col, row for this quad
697 { // start at top left, go clockwise
698 case 0:
699 row--; col--; break;
700 case 1:
701 col += 2; break;
702 case 2:
703 row += 2; break;
704 case 3:
705 col -= 2; break;
706 }
707 CvCBQuad *neighbor = quads[i]->neighbors[j];
708 if (neighbor && !neighbor->ordered && // is it an inner quad?
709 col <= col_max && col >= col_min &&
710 row <= row_max && row >= row_min)
711 {
712 // if so, set in order
713 PRINTF("Adding inner: col: %d row: %d\n", col, row);
714 found++;
715 icvOrderQuad(neighbor, quads[i]->corners[j], (j+2)%4);
716 neighbor->ordered = true;
717 neighbor->row = row;
718 neighbor->col = col;
719 }
720 }
721 }
722 }
723
724 // if we have found inner quads, add corresponding outer quads,
725 // which are missing
726 if (found > 0)
727 {
728 PRINTF("Found %d inner quads not connected to outer quads, repairing\n", found);
729 for (int i=0; i<quad_count; i++)
730 {
731 if (quads[i]->count < 4 && quads[i]->ordered)
732 {
733 int added = icvAddOuterQuad(quads[i],quads,quad_count,all_quads,*all_count,corners);
734 *all_count += added;
735 quad_count += added;
736 }
737 }
738 }
739
740
741 // final trimming of outer quads
742 if (dcol == w && drow == h) // found correct inner quads
743 {
744 PRINTF("Inner bounds ok, check outer quads\n");
745 int rcount = quad_count;
746 for (int i=quad_count-1; i>=0; i--) // eliminate any quad not connected to
747 // an ordered quad
748 {
749 if (quads[i]->ordered == false)
750 {
751 bool outer = false;
752 for (int j=0; j<4; j++) // any neighbors that are ordered?
753 {
754 if (quads[i]->neighbors[j] && quads[i]->neighbors[j]->ordered)
755 outer = true;
756 }
757 if (!outer) // not an outer quad, eliminate
758 {
759 PRINTF("Removing quad %d\n", i);
760 icvRemoveQuadFromGroup(quads,rcount,quads[i]);
761 rcount--;
762 }
763 }
764
765 }
766 return rcount;
767 }
768
769 return 0;
770 }
771
772
773 // add an outer quad
774 // looks for the neighbor of <quad> that isn't present,
775 // tries to add it in.
776 // <quad> is ordered
777
778 static int
icvAddOuterQuad(CvCBQuad * quad,CvCBQuad ** quads,int quad_count,CvCBQuad ** all_quads,int all_count,CvCBCorner ** corners)779 icvAddOuterQuad( CvCBQuad *quad, CvCBQuad **quads, int quad_count,
780 CvCBQuad **all_quads, int all_count, CvCBCorner **corners )
781
782 {
783 int added = 0;
784 for (int i=0; i<4; i++) // find no-neighbor corners
785 {
786 if (!quad->neighbors[i]) // ok, create and add neighbor
787 {
788 int j = (i+2)%4;
789 PRINTF("Adding quad as neighbor 2\n");
790 CvCBQuad *q = &(*all_quads)[all_count];
791 memset( q, 0, sizeof(*q) );
792 added++;
793 quads[quad_count] = q;
794 quad_count++;
795
796 // set neighbor and group id
797 quad->neighbors[i] = q;
798 quad->count += 1;
799 q->neighbors[j] = quad;
800 q->group_idx = quad->group_idx;
801 q->count = 1; // number of neighbors
802 q->ordered = false;
803 q->edge_len = quad->edge_len;
804
805 // make corners of new quad
806 // same as neighbor quad, but offset
807 CvPoint2D32f pt = quad->corners[i]->pt;
808 CvCBCorner* corner;
809 float dx = pt.x - quad->corners[j]->pt.x;
810 float dy = pt.y - quad->corners[j]->pt.y;
811 for (int k=0; k<4; k++)
812 {
813 corner = &(*corners)[all_count*4+k];
814 pt = quad->corners[k]->pt;
815 memset( corner, 0, sizeof(*corner) );
816 corner->pt = pt;
817 q->corners[k] = corner;
818 corner->pt.x += dx;
819 corner->pt.y += dy;
820 }
821 // have to set exact corner
822 q->corners[j] = quad->corners[i];
823
824 // now find other neighbor and add it, if possible
825 if (quad->neighbors[(i+3)%4] &&
826 quad->neighbors[(i+3)%4]->ordered &&
827 quad->neighbors[(i+3)%4]->neighbors[i] &&
828 quad->neighbors[(i+3)%4]->neighbors[i]->ordered )
829 {
830 CvCBQuad *qn = quad->neighbors[(i+3)%4]->neighbors[i];
831 q->count = 2;
832 q->neighbors[(j+1)%4] = qn;
833 qn->neighbors[(i+1)%4] = q;
834 qn->count += 1;
835 // have to set exact corner
836 q->corners[(j+1)%4] = qn->corners[(i+1)%4];
837 }
838
839 all_count++;
840 }
841 }
842 return added;
843 }
844
845
846 // trimming routines
847
848 static int
icvTrimCol(CvCBQuad ** quads,int count,int col,int dir)849 icvTrimCol(CvCBQuad **quads, int count, int col, int dir)
850 {
851 int rcount = count;
852 // find the right quad(s)
853 for (int i=0; i<count; i++)
854 {
855 #ifdef DEBUG_CHESSBOARD
856 if (quads[i]->ordered)
857 PRINTF("index: %d cur: %d\n", col, quads[i]->col);
858 #endif
859 if (quads[i]->ordered && quads[i]->col == col)
860 {
861 if (dir == 1)
862 {
863 if (quads[i]->neighbors[1])
864 {
865 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[1]);
866 rcount--;
867 }
868 if (quads[i]->neighbors[2])
869 {
870 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[2]);
871 rcount--;
872 }
873 }
874 else
875 {
876 if (quads[i]->neighbors[0])
877 {
878 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[0]);
879 rcount--;
880 }
881 if (quads[i]->neighbors[3])
882 {
883 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[3]);
884 rcount--;
885 }
886 }
887
888 }
889 }
890 return rcount;
891 }
892
893 static int
icvTrimRow(CvCBQuad ** quads,int count,int row,int dir)894 icvTrimRow(CvCBQuad **quads, int count, int row, int dir)
895 {
896 int i, rcount = count;
897 // find the right quad(s)
898 for (i=0; i<count; i++)
899 {
900 #ifdef DEBUG_CHESSBOARD
901 if (quads[i]->ordered)
902 PRINTF("index: %d cur: %d\n", row, quads[i]->row);
903 #endif
904 if (quads[i]->ordered && quads[i]->row == row)
905 {
906 if (dir == 1) // remove from bottom
907 {
908 if (quads[i]->neighbors[2])
909 {
910 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[2]);
911 rcount--;
912 }
913 if (quads[i]->neighbors[3])
914 {
915 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[3]);
916 rcount--;
917 }
918 }
919 else // remove from top
920 {
921 if (quads[i]->neighbors[0])
922 {
923 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[0]);
924 rcount--;
925 }
926 if (quads[i]->neighbors[1])
927 {
928 icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[1]);
929 rcount--;
930 }
931 }
932
933 }
934 }
935 return rcount;
936 }
937
938
939 //
940 // remove quad from quad group
941 //
942
943 static void
icvRemoveQuadFromGroup(CvCBQuad ** quads,int count,CvCBQuad * q0)944 icvRemoveQuadFromGroup(CvCBQuad **quads, int count, CvCBQuad *q0)
945 {
946 int i, j;
947 // remove any references to this quad as a neighbor
948 for(i = 0; i < count; i++ )
949 {
950 CvCBQuad *q = quads[i];
951 for(j = 0; j < 4; j++ )
952 {
953 if( q->neighbors[j] == q0 )
954 {
955 q->neighbors[j] = 0;
956 q->count--;
957 for(int k = 0; k < 4; k++ )
958 if( q0->neighbors[k] == q )
959 {
960 q0->neighbors[k] = 0;
961 q0->count--;
962 break;
963 }
964 break;
965 }
966 }
967 }
968
969 // remove the quad
970 for(i = 0; i < count; i++ )
971 {
972 CvCBQuad *q = quads[i];
973 if (q == q0)
974 {
975 quads[i] = quads[count-1];
976 break;
977 }
978 }
979 }
980
981 //
982 // put quad into correct order, where <corner> has value <common>
983 //
984
985 static void
icvOrderQuad(CvCBQuad * quad,CvCBCorner * corner,int common)986 icvOrderQuad(CvCBQuad *quad, CvCBCorner *corner, int common)
987 {
988 // find the corner
989 int tc;
990 for (tc=0; tc<4; tc++)
991 if (quad->corners[tc]->pt.x == corner->pt.x &&
992 quad->corners[tc]->pt.y == corner->pt.y)
993 break;
994
995 // set corner order
996 // shift
997 while (tc != common)
998 {
999 // shift by one
1000 CvCBCorner *tempc;
1001 CvCBQuad *tempq;
1002 tempc = quad->corners[3];
1003 tempq = quad->neighbors[3];
1004 for (int i=3; i>0; i--)
1005 {
1006 quad->corners[i] = quad->corners[i-1];
1007 quad->neighbors[i] = quad->neighbors[i-1];
1008 }
1009 quad->corners[0] = tempc;
1010 quad->neighbors[0] = tempq;
1011 tc++;
1012 tc = tc%4;
1013 }
1014 }
1015
1016
1017 // if we found too many connect quads, remove those which probably do not belong.
1018 static int
icvCleanFoundConnectedQuads(int quad_count,CvCBQuad ** quad_group,CvSize pattern_size)1019 icvCleanFoundConnectedQuads( int quad_count, CvCBQuad **quad_group, CvSize pattern_size )
1020 {
1021 CvMemStorage *temp_storage = 0;
1022 CvPoint2D32f *centers = 0;
1023
1024 CV_FUNCNAME( "icvCleanFoundConnectedQuads" );
1025
1026 __BEGIN__;
1027
1028 CvPoint2D32f center = {0,0};
1029 int i, j, k;
1030 // number of quads this pattern should contain
1031 int count = ((pattern_size.width + 1)*(pattern_size.height + 1) + 1)/2;
1032
1033 // if we have more quadrangles than we should,
1034 // try to eliminate duplicates or ones which don't belong to the pattern rectangle...
1035 if( quad_count <= count )
1036 EXIT;
1037
1038 // create an array of quadrangle centers
1039 CV_CALL( centers = (CvPoint2D32f *)cvAlloc( sizeof(centers[0])*quad_count ));
1040 CV_CALL( temp_storage = cvCreateMemStorage(0));
1041
1042 for( i = 0; i < quad_count; i++ )
1043 {
1044 CvPoint2D32f ci = {0,0};
1045 CvCBQuad* q = quad_group[i];
1046
1047 for( j = 0; j < 4; j++ )
1048 {
1049 CvPoint2D32f pt = q->corners[j]->pt;
1050 ci.x += pt.x;
1051 ci.y += pt.y;
1052 }
1053
1054 ci.x *= 0.25f;
1055 ci.y *= 0.25f;
1056
1057 centers[i] = ci;
1058 center.x += ci.x;
1059 center.y += ci.y;
1060 }
1061 center.x /= quad_count;
1062 center.y /= quad_count;
1063
1064 // If we still have more quadrangles than we should,
1065 // we try to eliminate bad ones based on minimizing the bounding box.
1066 // We iteratively remove the point which reduces the size of
1067 // the bounding box of the blobs the most
1068 // (since we want the rectangle to be as small as possible)
1069 // remove the quadrange that causes the biggest reduction
1070 // in pattern size until we have the correct number
1071 for( ; quad_count > count; quad_count-- )
1072 {
1073 double min_box_area = DBL_MAX;
1074 int skip, min_box_area_index = -1;
1075 CvCBQuad *q0, *q;
1076
1077 // For each point, calculate box area without that point
1078 for( skip = 0; skip < quad_count; skip++ )
1079 {
1080 // get bounding rectangle
1081 CvPoint2D32f temp = centers[skip]; // temporarily make index 'skip' the same as
1082 centers[skip] = center; // pattern center (so it is not counted for convex hull)
1083 CvMat pointMat = cvMat(1, quad_count, CV_32FC2, centers);
1084 CvSeq *hull = cvConvexHull2( &pointMat, temp_storage, CV_CLOCKWISE, 1 );
1085 centers[skip] = temp;
1086 double hull_area = fabs(cvContourArea(hull, CV_WHOLE_SEQ));
1087
1088 // remember smallest box area
1089 if( hull_area < min_box_area )
1090 {
1091 min_box_area = hull_area;
1092 min_box_area_index = skip;
1093 }
1094 cvClearMemStorage( temp_storage );
1095 }
1096
1097 q0 = quad_group[min_box_area_index];
1098
1099 // remove any references to this quad as a neighbor
1100 for( i = 0; i < quad_count; i++ )
1101 {
1102 q = quad_group[i];
1103 for( j = 0; j < 4; j++ )
1104 {
1105 if( q->neighbors[j] == q0 )
1106 {
1107 q->neighbors[j] = 0;
1108 q->count--;
1109 for( k = 0; k < 4; k++ )
1110 if( q0->neighbors[k] == q )
1111 {
1112 q0->neighbors[k] = 0;
1113 q0->count--;
1114 break;
1115 }
1116 break;
1117 }
1118 }
1119 }
1120
1121 // remove the quad
1122 quad_count--;
1123 quad_group[min_box_area_index] = quad_group[quad_count];
1124 centers[min_box_area_index] = centers[quad_count];
1125 }
1126
1127 __END__;
1128
1129 cvReleaseMemStorage( &temp_storage );
1130 cvFree( ¢ers );
1131
1132 return quad_count;
1133 }
1134
1135 //=====================================================================================
1136
1137 static int
icvFindConnectedQuads(CvCBQuad * quad,int quad_count,CvCBQuad ** out_group,int group_idx,CvMemStorage * storage)1138 icvFindConnectedQuads( CvCBQuad *quad, int quad_count, CvCBQuad **out_group,
1139 int group_idx, CvMemStorage* storage )
1140 {
1141 CvMemStorage* temp_storage = cvCreateChildMemStorage( storage );
1142 CvSeq* stack = cvCreateSeq( 0, sizeof(*stack), sizeof(void*), temp_storage );
1143 int i, count = 0;
1144
1145 // Scan the array for a first unlabeled quad
1146 for( i = 0; i < quad_count; i++ )
1147 {
1148 if( quad[i].count > 0 && quad[i].group_idx < 0)
1149 break;
1150 }
1151
1152 // Recursively find a group of connected quads starting from the seed quad[i]
1153 if( i < quad_count )
1154 {
1155 CvCBQuad* q = &quad[i];
1156 cvSeqPush( stack, &q );
1157 out_group[count++] = q;
1158 q->group_idx = group_idx;
1159 q->ordered = false;
1160
1161 while( stack->total )
1162 {
1163 cvSeqPop( stack, &q );
1164 for( i = 0; i < 4; i++ )
1165 {
1166 CvCBQuad *neighbor = q->neighbors[i];
1167 if( neighbor && neighbor->count > 0 && neighbor->group_idx < 0 )
1168 {
1169 cvSeqPush( stack, &neighbor );
1170 out_group[count++] = neighbor;
1171 neighbor->group_idx = group_idx;
1172 neighbor->ordered = false;
1173 }
1174 }
1175 }
1176 }
1177
1178 cvReleaseMemStorage( &temp_storage );
1179 return count;
1180 }
1181
1182
1183 //=====================================================================================
1184
1185 static int
icvCheckQuadGroup(CvCBQuad ** quad_group,int quad_count,CvCBCorner ** out_corners,CvSize pattern_size)1186 icvCheckQuadGroup( CvCBQuad **quad_group, int quad_count,
1187 CvCBCorner **out_corners, CvSize pattern_size )
1188 {
1189 const int ROW1 = 1000000;
1190 const int ROW2 = 2000000;
1191 const int ROW_ = 3000000;
1192 int result = 0;
1193 int i, out_corner_count = 0, corner_count = 0;
1194 CvCBCorner** corners = 0;
1195
1196 CV_FUNCNAME( "icvCheckQuadGroup" );
1197
1198 __BEGIN__;
1199
1200 int j, k, kk;
1201 int width = 0, height = 0;
1202 int hist[5] = {0,0,0,0,0};
1203 CvCBCorner* first = 0, *first2 = 0, *right, *cur, *below, *c;
1204 CV_CALL( corners = (CvCBCorner**)cvAlloc( quad_count*4*sizeof(corners[0]) ));
1205
1206 // build dual graph, which vertices are internal quad corners
1207 // and two vertices are connected iff they lie on the same quad edge
1208 for( i = 0; i < quad_count; i++ )
1209 {
1210 CvCBQuad* q = quad_group[i];
1211 /*CvScalar color = q->count == 0 ? cvScalar(0,255,255) :
1212 q->count == 1 ? cvScalar(0,0,255) :
1213 q->count == 2 ? cvScalar(0,255,0) :
1214 q->count == 3 ? cvScalar(255,255,0) :
1215 cvScalar(255,0,0);*/
1216
1217 for( j = 0; j < 4; j++ )
1218 {
1219 //cvLine( debug_img, cvPointFrom32f(q->corners[j]->pt), cvPointFrom32f(q->corners[(j+1)&3]->pt), color, 1, CV_AA, 0 );
1220 if( q->neighbors[j] )
1221 {
1222 CvCBCorner *a = q->corners[j], *b = q->corners[(j+1)&3];
1223 // mark internal corners that belong to:
1224 // - a quad with a single neighbor - with ROW1,
1225 // - a quad with two neighbors - with ROW2
1226 // make the rest of internal corners with ROW_
1227 int row_flag = q->count == 1 ? ROW1 : q->count == 2 ? ROW2 : ROW_;
1228
1229 if( a->row == 0 )
1230 {
1231 corners[corner_count++] = a;
1232 a->row = row_flag;
1233 }
1234 else if( a->row > row_flag )
1235 a->row = row_flag;
1236
1237 if( q->neighbors[(j+1)&3] )
1238 {
1239 if( a->count >= 4 || b->count >= 4 )
1240 EXIT;
1241 for( k = 0; k < 4; k++ )
1242 {
1243 if( a->neighbors[k] == b )
1244 EXIT;
1245 if( b->neighbors[k] == a )
1246 EXIT;
1247 }
1248 a->neighbors[a->count++] = b;
1249 b->neighbors[b->count++] = a;
1250 }
1251 }
1252 }
1253 }
1254
1255 if( corner_count != pattern_size.width*pattern_size.height )
1256 EXIT;
1257
1258 for( i = 0; i < corner_count; i++ )
1259 {
1260 int n = corners[i]->count;
1261 assert( 0 <= n && n <= 4 );
1262 hist[n]++;
1263 if( !first && n == 2 )
1264 {
1265 if( corners[i]->row == ROW1 )
1266 first = corners[i];
1267 else if( !first2 && corners[i]->row == ROW2 )
1268 first2 = corners[i];
1269 }
1270 }
1271
1272 // start with a corner that belongs to a quad with a signle neighbor.
1273 // if we do not have such, start with a corner of a quad with two neighbors.
1274 if( !first )
1275 first = first2;
1276
1277 if( !first || hist[0] != 0 || hist[1] != 0 || hist[2] != 4 ||
1278 hist[3] != (pattern_size.width + pattern_size.height)*2 - 8 )
1279 EXIT;
1280
1281 cur = first;
1282 right = below = 0;
1283 out_corners[out_corner_count++] = cur;
1284
1285 for( k = 0; k < 4; k++ )
1286 {
1287 c = cur->neighbors[k];
1288 if( c )
1289 {
1290 if( !right )
1291 right = c;
1292 else if( !below )
1293 below = c;
1294 }
1295 }
1296
1297 if( !right || (right->count != 2 && right->count != 3) ||
1298 !below || (below->count != 2 && below->count != 3) )
1299 EXIT;
1300
1301 cur->row = 0;
1302 //cvCircle( debug_img, cvPointFrom32f(cur->pt), 3, cvScalar(0,255,0), -1, 8, 0 );
1303
1304 first = below; // remember the first corner in the next row
1305 // find and store the first row (or column)
1306 for(j=1;;j++)
1307 {
1308 right->row = 0;
1309 out_corners[out_corner_count++] = right;
1310 //cvCircle( debug_img, cvPointFrom32f(right->pt), 3, cvScalar(0,255-j*10,0), -1, 8, 0 );
1311 if( right->count == 2 )
1312 break;
1313 if( right->count != 3 || out_corner_count >= MAX(pattern_size.width,pattern_size.height) )
1314 EXIT;
1315 cur = right;
1316 for( k = 0; k < 4; k++ )
1317 {
1318 c = cur->neighbors[k];
1319 if( c && c->row > 0 )
1320 {
1321 for( kk = 0; kk < 4; kk++ )
1322 {
1323 if( c->neighbors[kk] == below )
1324 break;
1325 }
1326 if( kk < 4 )
1327 below = c;
1328 else
1329 right = c;
1330 }
1331 }
1332 }
1333
1334 width = out_corner_count;
1335 if( width == pattern_size.width )
1336 height = pattern_size.height;
1337 else if( width == pattern_size.height )
1338 height = pattern_size.width;
1339 else
1340 EXIT;
1341
1342 // find and store all the other rows
1343 for( i = 1; ; i++ )
1344 {
1345 if( !first )
1346 break;
1347 cur = first;
1348 first = 0;
1349 for( j = 0;; j++ )
1350 {
1351 cur->row = i;
1352 out_corners[out_corner_count++] = cur;
1353 //cvCircle( debug_img, cvPointFrom32f(cur->pt), 3, cvScalar(0,0,255-j*10), -1, 8, 0 );
1354 if( cur->count == 2 + (i < height-1) && j > 0 )
1355 break;
1356
1357 right = 0;
1358
1359 // find a neighbor that has not been processed yet
1360 // and that has a neighbor from the previous row
1361 for( k = 0; k < 4; k++ )
1362 {
1363 c = cur->neighbors[k];
1364 if( c && c->row > i )
1365 {
1366 for( kk = 0; kk < 4; kk++ )
1367 {
1368 if( c->neighbors[kk] && c->neighbors[kk]->row == i-1 )
1369 break;
1370 }
1371 if( kk < 4 )
1372 {
1373 right = c;
1374 if( j > 0 )
1375 break;
1376 }
1377 else if( j == 0 )
1378 first = c;
1379 }
1380 }
1381 if( !right )
1382 EXIT;
1383 cur = right;
1384 }
1385
1386 if( j != width - 1 )
1387 EXIT;
1388 }
1389
1390 if( out_corner_count != corner_count )
1391 EXIT;
1392
1393 // check if we need to transpose the board
1394 if( width != pattern_size.width )
1395 {
1396 CV_SWAP( width, height, k );
1397
1398 memcpy( corners, out_corners, corner_count*sizeof(corners[0]) );
1399 for( i = 0; i < height; i++ )
1400 for( j = 0; j < width; j++ )
1401 out_corners[i*width + j] = corners[j*height + i];
1402 }
1403
1404 // check if we need to revert the order in each row
1405 {
1406 CvPoint2D32f p0 = out_corners[0]->pt, p1 = out_corners[pattern_size.width-1]->pt,
1407 p2 = out_corners[pattern_size.width]->pt;
1408 if( (p1.x - p0.x)*(p2.y - p1.y) - (p1.y - p0.y)*(p2.x - p1.x) < 0 )
1409 {
1410 if( width % 2 == 0 )
1411 {
1412 for( i = 0; i < height; i++ )
1413 for( j = 0; j < width/2; j++ )
1414 CV_SWAP( out_corners[i*width+j], out_corners[i*width+width-j-1], c );
1415 }
1416 else
1417 {
1418 for( j = 0; j < width; j++ )
1419 for( i = 0; i < height/2; i++ )
1420 CV_SWAP( out_corners[i*width+j], out_corners[(height - i - 1)*width+j], c );
1421 }
1422 }
1423 }
1424
1425 result = corner_count;
1426
1427 __END__;
1428
1429 if( result <= 0 && corners )
1430 {
1431 corner_count = MIN( corner_count, pattern_size.width*pattern_size.height );
1432 for( i = 0; i < corner_count; i++ )
1433 out_corners[i] = corners[i];
1434 result = -corner_count;
1435
1436 if (result == -pattern_size.width*pattern_size.height)
1437 result = -result;
1438 }
1439
1440 cvFree( &corners );
1441
1442 return result;
1443 }
1444
1445
1446
1447
1448 //=====================================================================================
1449
icvFindQuadNeighbors(CvCBQuad * quads,int quad_count)1450 static void icvFindQuadNeighbors( CvCBQuad *quads, int quad_count )
1451 {
1452 const float thresh_scale = 1.f;
1453 int idx, i, k, j;
1454 float dx, dy, dist;
1455
1456 // find quad neighbors
1457 for( idx = 0; idx < quad_count; idx++ )
1458 {
1459 CvCBQuad* cur_quad = &quads[idx];
1460
1461 // choose the points of the current quadrangle that are close to
1462 // some points of the other quadrangles
1463 // (it can happen for split corners (due to dilation) of the
1464 // checker board). Search only in other quadrangles!
1465
1466 // for each corner of this quadrangle
1467 for( i = 0; i < 4; i++ )
1468 {
1469 CvPoint2D32f pt;
1470 float min_dist = FLT_MAX;
1471 int closest_corner_idx = -1;
1472 CvCBQuad *closest_quad = 0;
1473 CvCBCorner *closest_corner = 0;
1474
1475 if( cur_quad->neighbors[i] )
1476 continue;
1477
1478 pt = cur_quad->corners[i]->pt;
1479
1480 // find the closest corner in all other quadrangles
1481 for( k = 0; k < quad_count; k++ )
1482 {
1483 if( k == idx )
1484 continue;
1485
1486 for( j = 0; j < 4; j++ )
1487 {
1488 if( quads[k].neighbors[j] )
1489 continue;
1490
1491 dx = pt.x - quads[k].corners[j]->pt.x;
1492 dy = pt.y - quads[k].corners[j]->pt.y;
1493 dist = dx * dx + dy * dy;
1494
1495 if( dist < min_dist &&
1496 dist <= cur_quad->edge_len*thresh_scale &&
1497 dist <= quads[k].edge_len*thresh_scale )
1498 {
1499 // check edge lengths, make sure they're compatible
1500 // edges that are different by more than 1:4 are rejected
1501 float ediff = cur_quad->edge_len - quads[k].edge_len;
1502 if (ediff > 32*cur_quad->edge_len ||
1503 ediff > 32*quads[k].edge_len)
1504 {
1505 PRINTF("Incompatible edge lengths\n");
1506 continue;
1507 }
1508 closest_corner_idx = j;
1509 closest_quad = &quads[k];
1510 min_dist = dist;
1511 }
1512 }
1513 }
1514
1515 // we found a matching corner point?
1516 if( closest_corner_idx >= 0 && min_dist < FLT_MAX )
1517 {
1518 // If another point from our current quad is closer to the found corner
1519 // than the current one, then we don't count this one after all.
1520 // This is necessary to support small squares where otherwise the wrong
1521 // corner will get matched to closest_quad;
1522 closest_corner = closest_quad->corners[closest_corner_idx];
1523
1524 for( j = 0; j < 4; j++ )
1525 {
1526 if( cur_quad->neighbors[j] == closest_quad )
1527 break;
1528
1529 dx = closest_corner->pt.x - cur_quad->corners[j]->pt.x;
1530 dy = closest_corner->pt.y - cur_quad->corners[j]->pt.y;
1531
1532 if( dx * dx + dy * dy < min_dist )
1533 break;
1534 }
1535
1536 if( j < 4 || cur_quad->count >= 4 || closest_quad->count >= 4 )
1537 continue;
1538
1539 // Check that each corner is a neighbor of different quads
1540 for( j = 0; j < closest_quad->count; j++ )
1541 {
1542 if( closest_quad->neighbors[j] == cur_quad )
1543 break;
1544 }
1545 if( j < closest_quad->count )
1546 continue;
1547
1548 // check whether the closest corner to closest_corner
1549 // is different from cur_quad->corners[i]->pt
1550 for( k = 0; k < quad_count; k++ )
1551 {
1552 CvCBQuad* q = &quads[k];
1553 if( k == idx || q == closest_quad )
1554 continue;
1555
1556 for( j = 0; j < 4; j++ )
1557 if( !q->neighbors[j] )
1558 {
1559 dx = closest_corner->pt.x - q->corners[j]->pt.x;
1560 dy = closest_corner->pt.y - q->corners[j]->pt.y;
1561 dist = dx*dx + dy*dy;
1562 if( dist < min_dist )
1563 break;
1564 }
1565 if( j < 4 )
1566 break;
1567 }
1568
1569 if( k < quad_count )
1570 continue;
1571
1572 closest_corner->pt.x = (pt.x + closest_corner->pt.x) * 0.5f;
1573 closest_corner->pt.y = (pt.y + closest_corner->pt.y) * 0.5f;
1574
1575 // We've found one more corner - remember it
1576 cur_quad->count++;
1577 cur_quad->neighbors[i] = closest_quad;
1578 cur_quad->corners[i] = closest_corner;
1579
1580 closest_quad->count++;
1581 closest_quad->neighbors[closest_corner_idx] = cur_quad;
1582 }
1583 }
1584 }
1585 }
1586
1587 //=====================================================================================
1588
1589 // returns corners in clockwise order
1590 // corners don't necessarily start at same position on quad (e.g.,
1591 // top left corner)
1592
1593 static int
icvGenerateQuads(CvCBQuad ** out_quads,CvCBCorner ** out_corners,CvMemStorage * storage,CvMat * image,int flags)1594 icvGenerateQuads( CvCBQuad **out_quads, CvCBCorner **out_corners,
1595 CvMemStorage *storage, CvMat *image, int flags )
1596 {
1597 int quad_count = 0;
1598 CvMemStorage *temp_storage = 0;
1599
1600 if( out_quads )
1601 *out_quads = 0;
1602
1603 if( out_corners )
1604 *out_corners = 0;
1605
1606 CV_FUNCNAME( "icvGenerateQuads" );
1607
1608 __BEGIN__;
1609
1610 CvSeq *src_contour = 0;
1611 CvSeq *root;
1612 CvContourEx* board = 0;
1613 CvContourScanner scanner;
1614 int i, idx, min_size;
1615
1616 CV_ASSERT( out_corners && out_quads );
1617
1618 // empiric bound for minimal allowed perimeter for squares
1619 min_size = cvRound( image->cols * image->rows * .03 * 0.01 * 0.92 );
1620
1621 // create temporary storage for contours and the sequence of pointers to found quadrangles
1622 CV_CALL( temp_storage = cvCreateChildMemStorage( storage ));
1623 CV_CALL( root = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSeq*), temp_storage ));
1624
1625 // initialize contour retrieving routine
1626 CV_CALL( scanner = cvStartFindContours( image, temp_storage, sizeof(CvContourEx),
1627 CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE ));
1628
1629 // get all the contours one by one
1630 while( (src_contour = cvFindNextContour( scanner )) != 0 )
1631 {
1632 CvSeq *dst_contour = 0;
1633 CvRect rect = ((CvContour*)src_contour)->rect;
1634
1635 // reject contours with too small perimeter
1636 if( CV_IS_SEQ_HOLE(src_contour) && rect.width*rect.height >= min_size )
1637 {
1638 const int min_approx_level = 2, max_approx_level = MAX_CONTOUR_APPROX;
1639 int approx_level;
1640 for( approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++ )
1641 {
1642 dst_contour = cvApproxPoly( src_contour, sizeof(CvContour), temp_storage,
1643 CV_POLY_APPROX_DP, (float)approx_level );
1644 // we call this again on its own output, because sometimes
1645 // cvApproxPoly() does not simplify as much as it should.
1646 dst_contour = cvApproxPoly( dst_contour, sizeof(CvContour), temp_storage,
1647 CV_POLY_APPROX_DP, (float)approx_level );
1648
1649 if( dst_contour->total == 4 )
1650 break;
1651 }
1652
1653 // reject non-quadrangles
1654 if( dst_contour->total == 4 && cvCheckContourConvexity(dst_contour) )
1655 {
1656 CvPoint pt[4];
1657 double d1, d2, p = cvContourPerimeter(dst_contour);
1658 double area = fabs(cvContourArea(dst_contour, CV_WHOLE_SEQ));
1659 double dx, dy;
1660
1661 for( i = 0; i < 4; i++ )
1662 pt[i] = *(CvPoint*)cvGetSeqElem(dst_contour, i);
1663
1664 dx = pt[0].x - pt[2].x;
1665 dy = pt[0].y - pt[2].y;
1666 d1 = sqrt(dx*dx + dy*dy);
1667
1668 dx = pt[1].x - pt[3].x;
1669 dy = pt[1].y - pt[3].y;
1670 d2 = sqrt(dx*dx + dy*dy);
1671
1672 // philipg. Only accept those quadrangles which are more square
1673 // than rectangular and which are big enough
1674 double d3, d4;
1675 dx = pt[0].x - pt[1].x;
1676 dy = pt[0].y - pt[1].y;
1677 d3 = sqrt(dx*dx + dy*dy);
1678 dx = pt[1].x - pt[2].x;
1679 dy = pt[1].y - pt[2].y;
1680 d4 = sqrt(dx*dx + dy*dy);
1681 if( !(flags & CV_CALIB_CB_FILTER_QUADS) ||
1682 (d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > min_size &&
1683 d1 >= 0.15 * p && d2 >= 0.15 * p) )
1684 {
1685 CvContourEx* parent = (CvContourEx*)(src_contour->v_prev);
1686 parent->counter++;
1687 if( !board || board->counter < parent->counter )
1688 board = parent;
1689 dst_contour->v_prev = (CvSeq*)parent;
1690 //for( i = 0; i < 4; i++ ) cvLine( debug_img, pt[i], pt[(i+1)&3], cvScalar(200,255,255), 1, CV_AA, 0 );
1691 cvSeqPush( root, &dst_contour );
1692 }
1693 }
1694 }
1695 }
1696
1697 // finish contour retrieving
1698 cvEndFindContours( &scanner );
1699
1700 // allocate quad & corner buffers
1701 CV_CALL( *out_quads = (CvCBQuad*)cvAlloc((root->total+root->total / 2) * sizeof((*out_quads)[0])));
1702 CV_CALL( *out_corners = (CvCBCorner*)cvAlloc((root->total+root->total / 2) * 4 * sizeof((*out_corners)[0])));
1703
1704 // Create array of quads structures
1705 for( idx = 0; idx < root->total; idx++ )
1706 {
1707 CvCBQuad* q = &(*out_quads)[quad_count];
1708 src_contour = *(CvSeq**)cvGetSeqElem( root, idx );
1709 if( (flags & CV_CALIB_CB_FILTER_QUADS) && src_contour->v_prev != (CvSeq*)board )
1710 continue;
1711
1712 // reset group ID
1713 memset( q, 0, sizeof(*q) );
1714 q->group_idx = -1;
1715 assert( src_contour->total == 4 );
1716 for( i = 0; i < 4; i++ )
1717 {
1718 CvPoint2D32f pt = cvPointTo32f(*(CvPoint*)cvGetSeqElem(src_contour, i));
1719 CvCBCorner* corner = &(*out_corners)[quad_count*4 + i];
1720
1721 memset( corner, 0, sizeof(*corner) );
1722 corner->pt = pt;
1723 q->corners[i] = corner;
1724 }
1725 q->edge_len = FLT_MAX;
1726 for( i = 0; i < 4; i++ )
1727 {
1728 float dx = q->corners[i]->pt.x - q->corners[(i+1)&3]->pt.x;
1729 float dy = q->corners[i]->pt.y - q->corners[(i+1)&3]->pt.y;
1730 float d = dx*dx + dy*dy;
1731 if( q->edge_len > d )
1732 q->edge_len = d;
1733 }
1734 quad_count++;
1735 }
1736
1737 __END__;
1738
1739 if( cvGetErrStatus() < 0 )
1740 {
1741 if( out_quads )
1742 cvFree( out_quads );
1743 if( out_corners )
1744 cvFree( out_corners );
1745 quad_count = 0;
1746 }
1747
1748 cvReleaseMemStorage( &temp_storage );
1749 return quad_count;
1750 }
1751
1752
1753 //=====================================================================================
1754
is_equal_quad(const void * _a,const void * _b,void *)1755 static int is_equal_quad( const void* _a, const void* _b, void* )
1756 {
1757 CvRect a = (*((CvContour**)_a))->rect;
1758 CvRect b = (*((CvContour**)_b))->rect;
1759
1760 int dx = MIN( b.x + b.width - 1, a.x + a.width - 1) - MAX( b.x, a.x);
1761 int dy = MIN( b.y + b.height - 1, a.y + a.height - 1) - MAX( b.y, a.y);
1762 int w = (a.width + b.width)>>1;
1763 int h = (a.height + b.height)>>1;
1764
1765 if( dx > w*0.75 && dy > h*0.75 && dx < w*1.25 && dy < h*1.25 ) return 1;
1766
1767 return 0;
1768 }
1769
1770 static int
icvGenerateQuadsEx(CvCBQuad ** out_quads,CvCBCorner ** out_corners,CvMemStorage * storage,CvMat * image,CvMat * thresh_img,int dilations,int flags)1771 icvGenerateQuadsEx( CvCBQuad **out_quads, CvCBCorner **out_corners,
1772 CvMemStorage *storage, CvMat *image, CvMat *thresh_img, int dilations, int flags )
1773 {
1774 int l;
1775 int quad_count = 0;
1776 CvMemStorage *temp_storage = 0;
1777
1778 if( out_quads )
1779 *out_quads = 0;
1780
1781 if( out_corners )
1782 *out_corners = 0;
1783
1784 CV_FUNCNAME( "icvGenerateQuads" );
1785
1786 __BEGIN__;
1787
1788 CvSeq *src_contour = 0;
1789 CvSeq *root, *root_tmp;
1790 CvContourEx* board = 0;
1791 CvContourScanner scanner;
1792 int i, idx, min_size;
1793 int step_level = 25;
1794
1795 CV_ASSERT( out_corners && out_quads );
1796
1797 // empiric bound for minimal allowed perimeter for squares
1798 min_size = cvRound( image->cols * image->rows * .03 * 0.01 * 0.92 );
1799
1800 // create temporary storage for contours and the sequence of pointers to found quadrangles
1801 CV_CALL( temp_storage = cvCreateChildMemStorage( storage ));
1802 CV_CALL( root_tmp = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSeq*), temp_storage ));
1803 CV_CALL( root = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSeq*), temp_storage ));
1804
1805 //perform contours slicing
1806 cvEqualizeHist(image,image);
1807 for(l = step_level; l < 256-step_level; l+= step_level)
1808 {
1809 cvThreshold( image, thresh_img, l, 255, CV_THRESH_BINARY );
1810 cvDilate( thresh_img, thresh_img, 0, dilations );
1811
1812 //draw frame to extract edge quads
1813 cvRectangle( thresh_img, cvPoint(0,0), cvPoint(thresh_img->cols-1,
1814 thresh_img->rows-1), CV_RGB(255,255,255), 3, 8);
1815
1816 // initialize contour retrieving routine
1817 CV_CALL( scanner = cvStartFindContours( thresh_img, temp_storage, sizeof(CvContourEx),
1818 CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE ));
1819
1820 // get all the contours one by one
1821 while( (src_contour = cvFindNextContour( scanner )) != 0 )
1822 {
1823 CvSeq *dst_contour = 0;
1824 CvRect rect = ((CvContour*)src_contour)->rect;
1825
1826 // reject contours with too small perimeter
1827 if( CV_IS_SEQ_HOLE(src_contour) && rect.width*rect.height >= min_size )
1828 {
1829 const int min_approx_level = 2, max_approx_level = MAX_CONTOUR_APPROX;
1830 int approx_level;
1831 for( approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++ )
1832 {
1833 dst_contour = cvApproxPoly( src_contour, sizeof(CvContour), temp_storage,
1834 CV_POLY_APPROX_DP, (float)approx_level );
1835 // we call this again on its own output, because sometimes
1836 // cvApproxPoly() does not simplify as much as it should.
1837 dst_contour = cvApproxPoly( dst_contour, sizeof(CvContour), temp_storage,
1838 CV_POLY_APPROX_DP, (float)approx_level );
1839
1840 if( dst_contour->total == 4 )
1841 break;
1842 }
1843
1844 // reject non-quadrangles
1845 if( dst_contour->total == 4 && cvCheckContourConvexity(dst_contour) )
1846 {
1847 CvPoint pt[4];
1848 double d1, d2, p = cvContourPerimeter(dst_contour);
1849 double area = fabs(cvContourArea(dst_contour, CV_WHOLE_SEQ));
1850 double dx, dy;
1851
1852 for( i = 0; i < 4; i++ )
1853 pt[i] = *(CvPoint*)cvGetSeqElem(dst_contour, i);
1854
1855 //check border condition. if this is edge square we will add this as is
1856 int edge_flag = 0, eps = 2;
1857 for( i = 0; i < 4; i++ )
1858 if( pt[i].x <= eps || pt[i].y <= eps ||
1859 pt[i].x >= image->width - eps ||
1860 pt[i].y >= image->height - eps ) edge_flag = 1;
1861
1862 dx = pt[0].x - pt[2].x;
1863 dy = pt[0].y - pt[2].y;
1864 d1 = sqrt(dx*dx + dy*dy);
1865
1866 dx = pt[1].x - pt[3].x;
1867 dy = pt[1].y - pt[3].y;
1868 d2 = sqrt(dx*dx + dy*dy);
1869
1870 // philipg. Only accept those quadrangles which are more square
1871 // than rectangular and which are big enough
1872 double d3, d4;
1873 dx = pt[0].x - pt[1].x;
1874 dy = pt[0].y - pt[1].y;
1875 d3 = sqrt(dx*dx + dy*dy);
1876 dx = pt[1].x - pt[2].x;
1877 dy = pt[1].y - pt[2].y;
1878 d4 = sqrt(dx*dx + dy*dy);
1879 if( edge_flag ||
1880 (!(flags & CV_CALIB_CB_FILTER_QUADS) ||
1881 (d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > min_size &&
1882 d1 >= 0.15 * p && d2 >= 0.15 * p)) )
1883 {
1884 CvContourEx* parent = (CvContourEx*)(src_contour->v_prev);
1885 parent->counter++;
1886 if( !board || board->counter < parent->counter )
1887 board = parent;
1888 dst_contour->v_prev = (CvSeq*)parent;
1889 //for( i = 0; i < 4; i++ ) cvLine( debug_img, pt[i], pt[(i+1)&3], cvScalar(200,255,255), 1, CV_AA, 0 );
1890 cvSeqPush( root_tmp, &dst_contour );
1891 }
1892 }
1893 }
1894 }
1895 // finish contour retrieving
1896 cvEndFindContours( &scanner );
1897 }
1898
1899
1900 // Perform clustering of extracted quads
1901 // Same quad can be extracted from different binarization levels
1902 if( root_tmp->total )
1903 {
1904 CvSeq* idx_seq = 0;
1905 int n_quads = cvSeqPartition( root_tmp, temp_storage, &idx_seq, is_equal_quad, 0 );
1906 for( i = 0; i < n_quads; i++ )
1907 {
1908 //extract biggest quad in group
1909 int max_size = 0;
1910 CvSeq* max_seq = 0;
1911 for( int j = 0; j < root_tmp->total; j++ )
1912 {
1913 int index = *(int*)cvGetSeqElem(idx_seq, j);
1914 if(index!=i) continue;
1915 CvContour* cnt = *(CvContour**)cvGetSeqElem(root_tmp, j);
1916 if(cnt->rect.width > max_size)
1917 {
1918 max_size = cnt->rect.width;
1919 max_seq = (CvSeq*)cnt;
1920 }
1921 }
1922 cvSeqPush( root, &max_seq);
1923 }
1924 }
1925
1926 // allocate quad & corner buffers
1927 CV_CALL( *out_quads = (CvCBQuad*)cvAlloc((root->total+root->total / 2) * sizeof((*out_quads)[0])));
1928 CV_CALL( *out_corners = (CvCBCorner*)cvAlloc((root->total+root->total / 2) * 4 * sizeof((*out_corners)[0])));
1929
1930 // Create array of quads structures
1931 for( idx = 0; idx < root->total; idx++ )
1932 {
1933 CvCBQuad* q = &(*out_quads)[quad_count];
1934 src_contour = *(CvSeq**)cvGetSeqElem( root, idx );
1935 if( (flags & CV_CALIB_CB_FILTER_QUADS) && src_contour->v_prev != (CvSeq*)board )
1936 continue;
1937
1938 // reset group ID
1939 memset( q, 0, sizeof(*q) );
1940 q->group_idx = -1;
1941 assert( src_contour->total == 4 );
1942 for( i = 0; i < 4; i++ )
1943 {
1944 CvPoint2D32f pt = cvPointTo32f(*(CvPoint*)cvGetSeqElem(src_contour, i));
1945 CvCBCorner* corner = &(*out_corners)[quad_count*4 + i];
1946
1947 memset( corner, 0, sizeof(*corner) );
1948 corner->pt = pt;
1949 q->corners[i] = corner;
1950 }
1951 q->edge_len = FLT_MAX;
1952 for( i = 0; i < 4; i++ )
1953 {
1954 float dx = q->corners[i]->pt.x - q->corners[(i+1)&3]->pt.x;
1955 float dy = q->corners[i]->pt.y - q->corners[(i+1)&3]->pt.y;
1956 float d = dx*dx + dy*dy;
1957 if( q->edge_len > d )
1958 q->edge_len = d;
1959 }
1960 quad_count++;
1961 }
1962
1963 __END__;
1964
1965 if( cvGetErrStatus() < 0 )
1966 {
1967 if( out_quads )
1968 cvFree( out_quads );
1969 if( out_corners )
1970 cvFree( out_corners );
1971 quad_count = 0;
1972 }
1973
1974 cvReleaseMemStorage( &temp_storage );
1975 return quad_count;
1976 }
1977
1978
1979 CV_IMPL void
cvDrawChessboardCorners(CvArr * _image,CvSize pattern_size,CvPoint2D32f * corners,int count,int found)1980 cvDrawChessboardCorners( CvArr* _image, CvSize pattern_size,
1981 CvPoint2D32f* corners, int count, int found )
1982 {
1983 CV_FUNCNAME( "cvDrawChessboardCorners" );
1984
1985 __BEGIN__;
1986
1987 const int shift = 0;
1988 const int radius = 4;
1989 const int r = radius*(1 << shift);
1990 int i;
1991 CvMat stub, *image;
1992 double scale = 1;
1993 int type, cn, line_type;
1994
1995 CV_CALL( image = cvGetMat( _image, &stub ));
1996
1997 type = CV_MAT_TYPE(image->type);
1998 cn = CV_MAT_CN(type);
1999 if( cn != 1 && cn != 3 && cn != 4 )
2000 CV_ERROR( CV_StsUnsupportedFormat, "Number of channels must be 1, 3 or 4" );
2001
2002 switch( CV_MAT_DEPTH(image->type) )
2003 {
2004 case CV_8U:
2005 scale = 1;
2006 break;
2007 case CV_16U:
2008 scale = 256;
2009 break;
2010 case CV_32F:
2011 scale = 1./255;
2012 break;
2013 default:
2014 CV_ERROR( CV_StsUnsupportedFormat,
2015 "Only 8-bit, 16-bit or floating-point 32-bit images are supported" );
2016 }
2017
2018 line_type = type == CV_8UC1 || type == CV_8UC3 ? CV_AA : 8;
2019
2020 if( !found )
2021 {
2022 CvScalar color = {{0,0,255}};
2023 if( cn == 1 )
2024 color = cvScalarAll(200);
2025 color.val[0] *= scale;
2026 color.val[1] *= scale;
2027 color.val[2] *= scale;
2028 color.val[3] *= scale;
2029
2030 for( i = 0; i < count; i++ )
2031 {
2032 CvPoint pt;
2033 pt.x = cvRound(corners[i].x*(1 << shift));
2034 pt.y = cvRound(corners[i].y*(1 << shift));
2035 cvLine( image, cvPoint( pt.x - r, pt.y - r ),
2036 cvPoint( pt.x + r, pt.y + r ), color, 1, line_type, shift );
2037 cvLine( image, cvPoint( pt.x - r, pt.y + r),
2038 cvPoint( pt.x + r, pt.y - r), color, 1, line_type, shift );
2039 cvCircle( image, pt, r+(1<<shift), color, 1, line_type, shift );
2040 }
2041 }
2042 else
2043 {
2044 int x, y;
2045 CvPoint prev_pt = {0, 0};
2046 const int line_max = 7;
2047 static const CvScalar line_colors[line_max] =
2048 {
2049 {{0,0,255}},
2050 {{0,128,255}},
2051 {{0,200,200}},
2052 {{0,255,0}},
2053 {{200,200,0}},
2054 {{255,0,0}},
2055 {{255,0,255}}
2056 };
2057
2058 for( y = 0, i = 0; y < pattern_size.height; y++ )
2059 {
2060 CvScalar color = line_colors[y % line_max];
2061 if( cn == 1 )
2062 color = cvScalarAll(200);
2063 color.val[0] *= scale;
2064 color.val[1] *= scale;
2065 color.val[2] *= scale;
2066 color.val[3] *= scale;
2067
2068 for( x = 0; x < pattern_size.width; x++, i++ )
2069 {
2070 CvPoint pt;
2071 pt.x = cvRound(corners[i].x*(1 << shift));
2072 pt.y = cvRound(corners[i].y*(1 << shift));
2073
2074 if( i != 0 )
2075 cvLine( image, prev_pt, pt, color, 1, line_type, shift );
2076
2077 cvLine( image, cvPoint(pt.x - r, pt.y - r),
2078 cvPoint(pt.x + r, pt.y + r), color, 1, line_type, shift );
2079 cvLine( image, cvPoint(pt.x - r, pt.y + r),
2080 cvPoint(pt.x + r, pt.y - r), color, 1, line_type, shift );
2081 cvCircle( image, pt, r+(1<<shift), color, 1, line_type, shift );
2082 prev_pt = pt;
2083 }
2084 }
2085 }
2086
2087 __END__;
2088 }
2089
2090
2091 /* End of file. */
2092