vcs.maemo.org Git - opencv/blob - src/cv/cvcalibinit.cpp

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