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 (at) 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 (at) cox.net">pgruebele (at) 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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
