--- /dev/null
+//M*//////////////////////////////////////////////////////////////////////////////////////\r
+//\r
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.\r
+//\r
+// By downloading, copying, installing or using the software you agree to this license.\r
+// If you do not agree to this license, do not download, install,\r
+// copy or use the software.\r
+//\r
+//\r
+// Intel License Agreement\r
+// For Open Source Computer Vision Library\r
+//\r
+// Copyright (C) 2000, Intel Corporation, all rights reserved.\r
+// Third party copyrights are property of their respective owners.\r
+//\r
+// Redistribution and use in source and binary forms, with or without modification,\r
+// are permitted provided that the following conditions are met:\r
+//\r
+// * Redistribution's of source code must retain the above copyright notice,\r
+// this list of conditions and the following disclaimer.\r
+//\r
+// * Redistribution's in binary form must reproduce the above copyright notice,\r
+// this list of conditions and the following disclaimer in the documentation\r
+// and/or other materials provided with the distribution.\r
+//\r
+// * The name of Intel Corporation may not be used to endorse or promote products\r
+// derived from this software without specific prior written permission.\r
+//\r
+// This software is provided by the copyright holders and contributors "as is" and\r
+// any express or implied warranties, including, but not limited to, the implied\r
+// warranties of merchantability and fitness for a particular purpose are disclaimed.\r
+// In no event shall the Intel Corporation or contributors be liable for any direct,\r
+// indirect, incidental, special, exemplary, or consequential damages\r
+// (including, but not limited to, procurement of substitute goods or services;\r
+// loss of use, data, or profits; or business interruption) however caused\r
+// and on any theory of liability, whether in contract, strict liability,\r
+// or tort (including negligence or otherwise) arising in any way out of\r
+// the use of this software, even if advised of the possibility of such damage.\r
+//\r
+//M*/\r
+\r
+/************************************************************************************\\r
+ This is improved variant of chessboard corner detection algorithm that\r
+ uses a graph of connected quads. It is based on the code contributed\r
+ by Vladimir Vezhnevets and Philip Gruebele.\r
+ Here is the copyright notice from the original Vladimir's code:\r
+ ===============================================================\r
+\r
+ The algorithms developed and implemented by Vezhnevets Vldimir\r
+ aka Dead Moroz (vvp@graphics.cs.msu.ru)\r
+ See http://graphics.cs.msu.su/en/research/calibration/opencv.html\r
+ for detailed information.\r
+\r
+ Reliability additions and modifications made by Philip Gruebele.\r
+ <a href="mailto:pgruebele@cox.net">pgruebele@cox.net</a>\r
+\r
+ Some further improvements for detection of partially ocluded boards at non-ideal\r
+ lighting conditions have been made by Alex Bovyrin and Kurt Kolonige\r
+\r
+\************************************************************************************/\r
+\r
+#include "_cv.h"\r
+#include <stdarg.h>\r
+\r
+//#define DEBUG_CHESSBOARD\r
+#ifdef DEBUG_CHESSBOARD\r
+static int PRINTF( const char* fmt, ... )\r
+{\r
+ va_list args;\r
+ va_start(args, fmt);\r
+ return vprintf(fmt, args);\r
+}\r
+#include "..//..//include/opencv/highgui.h"\r
+#else\r
+static int PRINTF( const char*, ... )\r
+{\r
+ return 0;\r
+}\r
+#endif\r
+\r
+\r
+//=====================================================================================\r
+// Implementation for the enhanced calibration object detection\r
+//=====================================================================================\r
+\r
+#define MAX_CONTOUR_APPROX 7\r
+\r
+struct CvContourEx\r
+{\r
+ CV_CONTOUR_FIELDS()\r
+ int counter;\r
+};\r
+\r
+//=====================================================================================\r
+\r
+/// Corner info structure\r
+/** This structure stores information about the chessboard corner.*/\r
+struct CvCBCorner\r
+{\r
+ CvPoint2D32f pt; // Coordinates of the corner\r
+ int row; // Board row index\r
+ int count; // Number of neighbor corners\r
+ struct CvCBCorner* neighbors[4]; // Neighbor corners\r
+\r
+ float meanDist(int *_n) const\r
+ {\r
+ float sum = 0;\r
+ int n = 0;\r
+ for( int i = 0; i < 4; i++ )\r
+ {\r
+ if( neighbors[i] )\r
+ {\r
+ float dx = neighbors[i]->pt.x - pt.x;\r
+ float dy = neighbors[i]->pt.y - pt.y;\r
+ sum += sqrt(dx*dx + dy*dy);\r
+ n++;\r
+ }\r
+ }\r
+ if(_n)\r
+ *_n = n;\r
+ return sum/MAX(n,1);\r
+ }\r
+};\r
+\r
+//=====================================================================================\r
+/// Quadrangle contour info structure\r
+/** This structure stores information about the chessboard quadrange.*/\r
+struct CvCBQuad\r
+{\r
+ int count; // Number of quad neighbors\r
+ int group_idx; // quad group ID\r
+ int row, col; // row and column of this quad\r
+ bool ordered; // true if corners/neighbors are ordered counter-clockwise\r
+ float edge_len; // quad edge len, in pix^2\r
+ // neighbors and corners are synced, i.e., neighbor 0 shares corner 0\r
+ CvCBCorner *corners[4]; // Coordinates of quad corners\r
+ struct CvCBQuad *neighbors[4]; // Pointers of quad neighbors\r
+};\r
+\r
+//=====================================================================================\r
+\r
+//static CvMat* debug_img = 0;\r
+\r
+static int icvGenerateQuads( CvCBQuad **quads, CvCBCorner **corners,\r
+ CvMemStorage *storage, CvMat *image, int flags );\r
+\r
+/*static int\r
+icvGenerateQuadsEx( CvCBQuad **out_quads, CvCBCorner **out_corners,\r
+ CvMemStorage *storage, CvMat *image, CvMat *thresh_img, int dilation, int flags );*/\r
+\r
+static void icvFindQuadNeighbors( CvCBQuad *quads, int quad_count );\r
+\r
+static int icvFindConnectedQuads( CvCBQuad *quads, int quad_count,\r
+ CvCBQuad **quad_group, int group_idx,\r
+ CvMemStorage* storage );\r
+\r
+static int icvCheckQuadGroup( CvCBQuad **quad_group, int count,\r
+ CvCBCorner **out_corners, CvSize pattern_size );\r
+\r
+static int icvCleanFoundConnectedQuads( int quad_count,\r
+ CvCBQuad **quads, CvSize pattern_size );\r
+\r
+static int icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,\r
+ int *all_count, CvCBQuad **all_quads, CvCBCorner **corners,\r
+ CvSize pattern_size, CvMemStorage* storage );\r
+\r
+static void icvOrderQuad(CvCBQuad *quad, CvCBCorner *corner, int common);\r
+\r
+static int icvTrimCol(CvCBQuad **quads, int count, int col, int dir);\r
+\r
+static int icvTrimRow(CvCBQuad **quads, int count, int row, int dir);\r
+\r
+static int icvAddOuterQuad(CvCBQuad *quad, CvCBQuad **quads, int quad_count,\r
+ CvCBQuad **all_quads, int all_count, CvCBCorner **corners);\r
+\r
+static void icvRemoveQuadFromGroup(CvCBQuad **quads, int count, CvCBQuad *q0);\r
+\r
+static int icvCheckBoardMonotony( CvPoint2D32f* corners, CvSize pattern_size );\r
+\r
+#if 0\r
+static void\r
+icvCalcAffineTranf2D32f(CvPoint2D32f* pts1, CvPoint2D32f* pts2, int count, CvMat* affine_trans)\r
+{\r
+ int i, j;\r
+ int real_count = 0;\r
+ for( j = 0; j < count; j++ )\r
+ {\r
+ if( pts1[j].x >= 0 ) real_count++;\r
+ }\r
+ if(real_count < 3) return;\r
+ CvMat* xy = cvCreateMat( 2*real_count, 6, CV_32FC1 );\r
+ CvMat* uv = cvCreateMat( 2*real_count, 1, CV_32FC1 );\r
+ //estimate affine transfromation\r
+ for( i = 0, j = 0; j < count; j++ )\r
+ {\r
+ if( pts1[j].x >= 0 )\r
+ {\r
+ CV_MAT_ELEM( *xy, float, i*2+1, 2 ) = CV_MAT_ELEM( *xy, float, i*2, 0 ) = pts2[j].x;\r
+ CV_MAT_ELEM( *xy, float, i*2+1, 3 ) = CV_MAT_ELEM( *xy, float, i*2, 1 ) = pts2[j].y;\r
+ 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
+ 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
+ CV_MAT_ELEM( *xy, float, i*2, 4 ) = CV_MAT_ELEM( *xy, float, i*2+1, 5 ) = 1;\r
+ CV_MAT_ELEM( *uv, float, i*2, 0 ) = pts1[j].x;\r
+ CV_MAT_ELEM( *uv, float, i*2+1, 0 ) = pts1[j].y;\r
+ i++;\r
+ }\r
+ }\r
+\r
+ cvSolve( xy, uv, affine_trans, CV_SVD );\r
+ cvReleaseMat(&xy);\r
+ cvReleaseMat(&uv);\r
+}\r
+#endif\r
+\r
+CV_IMPL\r
+int cvFindChessboardCorners( const void* arr, CvSize pattern_size,\r
+ CvPoint2D32f* out_corners, int* out_corner_count,\r
+ int flags )\r
+{\r
+ int k = 0;\r
+ const int min_dilations = 0;\r
+ const int max_dilations = 7;\r
+ int found = 0;\r
+ CvMat* norm_img = 0;\r
+ CvMat* thresh_img = 0;\r
+#ifdef DEBUG_CHESSBOARD\r
+ IplImage *dbg_img = 0;\r
+ IplImage *dbg1_img = 0;\r
+ IplImage *dbg2_img = 0;\r
+#endif\r
+ CvMemStorage* storage = 0;\r
+\r
+ CvCBQuad *quads = 0, **quad_group = 0;\r
+ CvCBCorner *corners = 0, **corner_group = 0;\r
+ CvMat stub, *img = (CvMat*)arr;\r
+\r
+ int expected_corners_num = (pattern_size.width/2+1)*(pattern_size.height/2+1);\r
+\r
+ int prev_sqr_size = 0;\r
+\r
+ if( out_corner_count )\r
+ *out_corner_count = 0;\r
+\r
+ CV_FUNCNAME( "cvFindChessBoardCornerGuesses2" );\r
+\r
+ __BEGIN__;\r
+\r
+ int quad_count = 0, group_idx = 0, i = 0, dilations = 0;\r
+ \r
+ CV_CALL( img = cvGetMat( img, &stub ));\r
+ //debug_img = img;\r
+\r
+ if( CV_MAT_DEPTH( img->type ) != CV_8U || CV_MAT_CN( img->type ) == 2 )\r
+ CV_ERROR( CV_StsUnsupportedFormat, "Only 8-bit grayscale or color images are supported" );\r
+\r
+ if( pattern_size.width <= 2 || pattern_size.height <= 2 )\r
+ CV_ERROR( CV_StsOutOfRange, "Both width and height of the pattern should have bigger than 2" );\r
+\r
+ if( !out_corners )\r
+ CV_ERROR( CV_StsNullPtr, "Null pointer to corners" );\r
+\r
+ CV_CALL( storage = cvCreateMemStorage(0) );\r
+ CV_CALL( thresh_img = cvCreateMat( img->rows, img->cols, CV_8UC1 ));\r
+\r
+#ifdef DEBUG_CHESSBOARD\r
+ CV_CALL( dbg_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3 ));\r
+ CV_CALL( dbg1_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3 ));\r
+ CV_CALL( dbg2_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3 ));\r
+#endif\r
+\r
+ if( CV_MAT_CN(img->type) != 1 || (flags & CV_CALIB_CB_NORMALIZE_IMAGE) )\r
+ {\r
+ // equalize the input image histogram -\r
+ // that should make the contrast between "black" and "white" areas big enough\r
+ CV_CALL( norm_img = cvCreateMat( img->rows, img->cols, CV_8UC1 ));\r
+\r
+ if( CV_MAT_CN(img->type) != 1 )\r
+ {\r
+ CV_CALL( cvCvtColor( img, norm_img, CV_BGR2GRAY ));\r
+ img = norm_img;\r
+ }\r
+\r
+ if( flags & CV_CALIB_CB_NORMALIZE_IMAGE )\r
+ {\r
+ cvEqualizeHist( img, norm_img );\r
+ img = norm_img;\r
+ }\r
+ }\r
+\r
+ // Try our standard "1" dilation, but if the pattern is not found, iterate the whole procedure with higher dilations.\r
+ // This is necessary because some squares simply do not separate properly with a single dilation. However,\r
+ // we want to use the minimum number of dilations possible since dilations cause the squares to become smaller,\r
+ // making it difficult to detect smaller squares.\r
+ for( k = 0; k < 3; k++ )\r
+ {\r
+ for( dilations = min_dilations; dilations <= max_dilations; dilations++ )\r
+ {\r
+ if (found)\r
+ break; // already found it\r
+\r
+ /*if( k == 1 )\r
+ {\r
+ //Pattern was not found using binarization\r
+ // Run multi-level quads extraction\r
+ // In case one-level binarization did not give enough number of quads\r
+ CV_CALL( quad_count = icvGenerateQuadsEx( &quads, &corners, storage, img, thresh_img, dilations, flags ));\r
+ PRINTF("EX quad count: %d/%d\n", quad_count, expected_corners_num);\r
+ }\r
+ else*/\r
+ {\r
+ // convert the input grayscale image to binary (black-n-white)\r
+ if( flags & CV_CALIB_CB_ADAPTIVE_THRESH )\r
+ {\r
+ int block_size = cvRound(prev_sqr_size == 0 ?\r
+ MIN(img->cols,img->rows)*0.2 : prev_sqr_size*2.)|1;\r
+\r
+ // convert to binary\r
+ cvAdaptiveThreshold( img, thresh_img, 255,\r
+ CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, block_size, k*5 );\r
+ if (dilations > 0)\r
+ cvDilate( thresh_img, thresh_img, 0, dilations-1 );\r
+ }\r
+ else\r
+ {\r
+ // Make dilation before the thresholding.\r
+ // It splits chessboard corners\r
+ //cvDilate( img, thresh_img, 0, 1 );\r
+\r
+ // empiric threshold level\r
+ double mean = cvMean( img );\r
+ int thresh_level = cvRound( mean - 10 );\r
+ thresh_level = MAX( thresh_level, 10 );\r
+\r
+ cvThreshold( img, thresh_img, thresh_level, 255, CV_THRESH_BINARY );\r
+ cvDilate( thresh_img, thresh_img, 0, dilations );\r
+ }\r
+\r
+#ifdef DEBUG_CHESSBOARD\r
+ cvCvtColor(thresh_img,dbg_img,CV_GRAY2BGR);\r
+#endif\r
+\r
+ // So we can find rectangles that go to the edge, we draw a white line around the image edge.\r
+ // Otherwise FindContours will miss those clipped rectangle contours.\r
+ // The border color will be the image mean, because otherwise we risk screwing up filters like cvSmooth()...\r
+ cvRectangle( thresh_img, cvPoint(0,0), cvPoint(thresh_img->cols-1,\r
+ thresh_img->rows-1), CV_RGB(255,255,255), 3, 8);\r
+\r
+ CV_CALL( quad_count = icvGenerateQuads( &quads, &corners, storage, thresh_img, flags ));\r
+\r
+\r
+ PRINTF("Quad count: %d/%d\n", quad_count, expected_corners_num);\r
+ }\r
+\r
+\r
+#ifdef DEBUG_CHESSBOARD\r
+ cvCopy(dbg_img, dbg1_img);\r
+ cvNamedWindow("all_quads", 1);\r
+ // copy corners to temp array\r
+ for( i = 0; i < quad_count; i++ )\r
+ {\r
+ for (int k=0; k<4; k++)\r
+ {\r
+ CvPoint2D32f pt1, pt2;\r
+ CvScalar color = CV_RGB(30,255,30);\r
+ pt1 = quads[i].corners[k]->pt;\r
+ pt2 = quads[i].corners[(k+1)%4]->pt;\r
+ pt2.x = (pt1.x + pt2.x)/2;\r
+ pt2.y = (pt1.y + pt2.y)/2;\r
+ if (k>0)\r
+ color = CV_RGB(200,200,0);\r
+ cvLine( dbg1_img, cvPointFrom32f(pt1), cvPointFrom32f(pt2), color, 3, 8);\r
+ }\r
+ }\r
+\r
+\r
+ cvShowImage("all_quads", (IplImage*)dbg1_img);\r
+ cvWaitKey();\r
+#endif\r
+\r
+ if( quad_count <= 0 )\r
+ continue;\r
+\r
+ // Find quad's neighbors\r
+ CV_CALL( icvFindQuadNeighbors( quads, quad_count ));\r
+\r
+ // allocate extra for adding in icvOrderFoundQuads\r
+ CV_CALL( quad_group = (CvCBQuad**)cvAlloc( sizeof(quad_group[0]) * (quad_count+quad_count / 2)));\r
+ CV_CALL( corner_group = (CvCBCorner**)cvAlloc( sizeof(corner_group[0]) * (quad_count+quad_count / 2)*4 ));\r
+\r
+ for( group_idx = 0; ; group_idx++ )\r
+ {\r
+ int count = 0;\r
+ CV_CALL( count = icvFindConnectedQuads( quads, quad_count, quad_group, group_idx, storage ));\r
+\r
+ int icount = count;\r
+ if( count == 0 )\r
+ break;\r
+\r
+ // order the quad corners globally\r
+ // maybe delete or add some\r
+ PRINTF("Starting ordering of inner quads\n");\r
+ count = icvOrderFoundConnectedQuads(count, quad_group, &quad_count, &quads, &corners,\r
+ pattern_size, storage );\r
+ PRINTF("Orig count: %d After ordering: %d\n", icount, count);\r
+\r
+\r
+#ifdef DEBUG_CHESSBOARD\r
+ cvCopy(dbg_img,dbg2_img);\r
+ cvNamedWindow("connected_group", 1);\r
+ // copy corners to temp array\r
+ for( i = 0; i < quad_count; i++ )\r
+ {\r
+ if (quads[i].group_idx == group_idx)\r
+ for (int k=0; k<4; k++)\r
+ {\r
+ CvPoint2D32f pt1, pt2;\r
+ CvScalar color = CV_RGB(30,255,30);\r
+ if (quads[i].ordered)\r
+ color = CV_RGB(255,30,30);\r
+ pt1 = quads[i].corners[k]->pt;\r
+ pt2 = quads[i].corners[(k+1)%4]->pt;\r
+ pt2.x = (pt1.x + pt2.x)/2;\r
+ pt2.y = (pt1.y + pt2.y)/2;\r
+ if (k>0)\r
+ color = CV_RGB(200,200,0);\r
+ cvLine( dbg2_img, cvPointFrom32f(pt1), cvPointFrom32f(pt2), color, 3, 8);\r
+ }\r
+ }\r
+ cvShowImage("connected_group", (IplImage*)dbg2_img);\r
+ cvWaitKey();\r
+#endif\r
+\r
+ if (count == 0)\r
+ continue; // haven't found inner quads\r
+\r
+\r
+ // If count is more than it should be, this will remove those quads\r
+ // which cause maximum deviation from a nice square pattern.\r
+ CV_CALL( count = icvCleanFoundConnectedQuads( count, quad_group, pattern_size ));\r
+ PRINTF("Connected group: %d orig count: %d cleaned: %d\n", group_idx, icount, count);\r
+\r
+ CV_CALL( count = icvCheckQuadGroup( quad_group, count, corner_group, pattern_size ));\r
+ PRINTF("Connected group: %d count: %d cleaned: %d\n", group_idx, icount, count);\r
+\r
+ {\r
+ int n = count > 0 ? pattern_size.width * pattern_size.height : -count;\r
+ n = MIN( n, pattern_size.width * pattern_size.height );\r
+ float sum_dist = 0;\r
+ int total = 0;\r
+\r
+ for( i = 0; i < n; i++ )\r
+ {\r
+ int ni = 0;\r
+ float avgi = corner_group[i]->meanDist(&ni);\r
+ sum_dist += avgi*ni;\r
+ total += ni;\r
+ }\r
+ prev_sqr_size = cvRound(sum_dist/MAX(total, 1));\r
+\r
+ if( count > 0 || (out_corner_count && -count > *out_corner_count) )\r
+ {\r
+ // copy corners to output array\r
+ for( i = 0; i < n; i++ )\r
+ out_corners[i] = corner_group[i]->pt;\r
+\r
+ if( out_corner_count )\r
+ *out_corner_count = n;\r
+\r
+ if( count == pattern_size.width*pattern_size.height &&\r
+ icvCheckBoardMonotony( out_corners, pattern_size ))\r
+ {\r
+ found = 1;\r
+ break;\r
+ }\r
+ }\r
+ }\r
+ }\r
+\r
+ cvFree( &quads );\r
+ cvFree( &corners );\r
+ cvFree( &quad_group );\r
+ cvFree( &corner_group );\r
+ }//dilations\r
+ }//\r
+\r
+\r
+ __END__;\r
+\r
+ if( found )\r
+ found = icvCheckBoardMonotony( out_corners, pattern_size );\r
+\r
+ if( found && pattern_size.height % 2 == 0 && pattern_size.width % 2 == 0 )\r
+ {\r
+ int last_row = (pattern_size.height-1)*pattern_size.width;\r
+ double dy0 = out_corners[last_row].y - out_corners[0].y;\r
+ if( dy0 < 0 )\r
+ {\r
+ int i, n = pattern_size.width*pattern_size.height;\r
+ for( i = 0; i < n/2; i++ )\r
+ {\r
+ CvPoint2D32f temp;\r
+ CV_SWAP(out_corners[i], out_corners[n-i-1], temp);\r
+ }\r
+ }\r
+ }\r
+\r
+ if( found )\r
+ {\r
+ CvMat* gray = 0;\r
+ if( CV_MAT_CN(img->type) != 1 )\r
+ {\r
+ gray = cvCreateMat(img->rows, img->cols, CV_8UC1);\r
+ cvCvtColor(img, gray, CV_BGR2GRAY);\r
+ }\r
+ else\r
+ gray = img;\r
+ int wsize = 2;\r
+ cvFindCornerSubPix( gray, out_corners, pattern_size.width*pattern_size.height,\r
+ cvSize(wsize, wsize), cvSize(-1,-1), cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 15, 0.1));\r
+ if( gray != img )\r
+ cvReleaseMat( &gray );\r
+ }\r
+\r
+ cvReleaseMemStorage( &storage );\r
+ cvReleaseMat( &norm_img );\r
+ cvReleaseMat( &thresh_img );\r
+ cvFree( &quads );\r
+ cvFree( &corners );\r
+\r
+ return found;\r
+}\r
+\r
+//\r
+// Checks that each board row and column is pretty much monotonous curve:\r
+// It analyzes each row and each column of the chessboard as following:\r
+// for each corner c lying between end points in the same row/column it checks that\r
+// the point projection to the line segment (a,b) is lying between projections\r
+// of the neighbor corners in the same row/column.\r
+//\r
+// This function has been created as temporary workaround for the bug in current implementation\r
+// of cvFindChessboardCornes that produces absolutely unordered sets of corners.\r
+//\r
+\r
+static int\r
+icvCheckBoardMonotony( CvPoint2D32f* corners, CvSize pattern_size )\r
+{\r
+ int i, j, k;\r
+ \r
+ for( k = 0; k < 2; k++ )\r
+ {\r
+ for( i = 0; i < (k == 0 ? pattern_size.height : pattern_size.width); i++ )\r
+ {\r
+ CvPoint2D32f a = k == 0 ? corners[i*pattern_size.width] : corners[i];\r
+ CvPoint2D32f b = k == 0 ? corners[(i+1)*pattern_size.width-1] :\r
+ corners[(pattern_size.height-1)*pattern_size.width + i];\r
+ float prevt = 0, dx0 = b.x - a.x, dy0 = b.y - a.y;\r
+ if( fabs(dx0) + fabs(dy0) < FLT_EPSILON )\r
+ return 0;\r
+ for( j = 1; j < (k == 0 ? pattern_size.width : pattern_size.height) - 1; j++ )\r
+ {\r
+ CvPoint2D32f c = k == 0 ? corners[i*pattern_size.width + j] :\r
+ corners[j*pattern_size.width + i];\r
+ float t = ((c.x - a.x)*dx0 + (c.y - a.y)*dy0)/(dx0*dx0 + dy0*dy0);\r
+ if( t < prevt || t > 1 )\r
+ return 0;\r
+ prevt = t;\r
+ }\r
+ }\r
+ }\r
+\r
+ return 1;\r
+}\r
+\r
+//\r
+// order a group of connected quads\r
+// order of corners:\r
+// 0 is top left\r
+// clockwise from there\r
+// note: "top left" is nominal, depends on initial ordering of starting quad\r
+// but all other quads are ordered consistently\r
+//\r
+// can change the number of quads in the group\r
+// can add quads, so we need to have quad/corner arrays passed in\r
+//\r
+\r
+static int\r
+icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,\r
+ int *all_count, CvCBQuad **all_quads, CvCBCorner **corners,\r
+ CvSize pattern_size, CvMemStorage* storage )\r
+{\r
+ CvMemStorage* temp_storage = cvCreateChildMemStorage( storage );\r
+ CvSeq* stack = cvCreateSeq( 0, sizeof(*stack), sizeof(void*), temp_storage );\r
+ int i;\r
+\r
+ // first find an interior quad\r
+ CvCBQuad *start = NULL;\r
+ for (i=0; i<quad_count; i++)\r
+ {\r
+ if (quads[i]->count == 4)\r
+ {\r
+ start = quads[i];\r
+ break;\r
+ }\r
+ }\r
+\r
+ if (start == NULL)\r
+ {\r
+ cvReleaseMemStorage( &temp_storage );\r
+ return 0; // no 4-connected quad\r
+ }\r
+\r
+ // start with first one, assign rows/cols\r
+ int row_min = 0, col_min = 0, row_max=0, col_max = 0;\r
+\r
+ std::map<int, int> col_hist;\r
+ std::map<int, int> row_hist;\r
+\r
+ cvSeqPush(stack, &start);\r
+ start->row = 0;\r
+ start->col = 0;\r
+ start->ordered = true;\r
+\r
+ // Recursively order the quads so that all position numbers (e.g.,\r
+ // 0,1,2,3) are in the at the same relative corner (e.g., lower right).\r
+\r
+ while( stack->total )\r
+ {\r
+ CvCBQuad* q;\r
+ cvSeqPop( stack, &q );\r
+ int col = q->col;\r
+ int row = q->row;\r
+ col_hist[col]++;\r
+ row_hist[row]++;\r
+\r
+ // check min/max\r
+ if (row > row_max) row_max = row;\r
+ if (row < row_min) row_min = row;\r
+ if (col > col_max) col_max = col;\r
+ if (col < col_min) col_min = col;\r
+\r
+ for(int i = 0; i < 4; i++ )\r
+ {\r
+ CvCBQuad *neighbor = q->neighbors[i];\r
+ switch(i) // adjust col, row for this quad\r
+ { // start at top left, go clockwise\r
+ case 0:\r
+ row--; col--; break;\r
+ case 1:\r
+ col += 2; break;\r
+ case 2:\r
+ row += 2; break;\r
+ case 3:\r
+ col -= 2; break;\r
+ }\r
+\r
+ // just do inside quads\r
+ if (neighbor && neighbor->ordered == false && neighbor->count == 4)\r
+ {\r
+ PRINTF("col: %d row: %d\n", col, row);\r
+ icvOrderQuad(neighbor, q->corners[i], (i+2)%4); // set in order\r
+ neighbor->ordered = true;\r
+ neighbor->row = row;\r
+ neighbor->col = col;\r
+ cvSeqPush( stack, &neighbor );\r
+ }\r
+ }\r
+ }\r
+\r
+ cvReleaseMemStorage( &temp_storage );\r
+\r
+ for (i=col_min; i<=col_max; i++)\r
+ PRINTF("HIST[%d] = %d\n", i, col_hist[i]);\r
+\r
+ // analyze inner quad structure\r
+ int w = pattern_size.width - 1;\r
+ int h = pattern_size.height - 1;\r
+ int drow = row_max - row_min + 1;\r
+ int dcol = col_max - col_min + 1;\r
+\r
+ // normalize pattern and found quad indices\r
+ if ((w > h && dcol < drow) ||\r
+ (w < h && drow < dcol))\r
+ {\r
+ h = pattern_size.width - 1;\r
+ w = pattern_size.height - 1;\r
+ }\r
+\r
+ PRINTF("Size: %dx%d Pattern: %dx%d\n", dcol, drow, w, h);\r
+\r
+ // check if there are enough inner quads\r
+ if (dcol < w || drow < h) // found enough inner quads?\r
+ {\r
+ PRINTF("Too few inner quad rows/cols\n");\r
+ return 0; // no, return\r
+ }\r
+\r
+ // too many columns, not very common\r
+ if (dcol == w+1) // too many, trim\r
+ {\r
+ PRINTF("Trimming cols\n");\r
+ if (col_hist[col_max] > col_hist[col_min])\r
+ {\r
+ PRINTF("Trimming left col\n");\r
+ quad_count = icvTrimCol(quads,quad_count,col_min,-1);\r
+ }\r
+ else\r
+ {\r
+ PRINTF("Trimming right col\n");\r
+ quad_count = icvTrimCol(quads,quad_count,col_max,+1);\r
+ }\r
+ }\r
+\r
+ // too many rows, not very common\r
+ if (drow == h+1) // too many, trim\r
+ {\r
+ PRINTF("Trimming rows\n");\r
+ if (row_hist[row_max] > row_hist[row_min])\r
+ {\r
+ PRINTF("Trimming top row\n");\r
+ quad_count = icvTrimRow(quads,quad_count,row_min,-1);\r
+ }\r
+ else\r
+ {\r
+ PRINTF("Trimming bottom row\n");\r
+ quad_count = icvTrimRow(quads,quad_count,row_max,+1);\r
+ }\r
+ }\r
+\r
+\r
+ // check edges of inner quads\r
+ // if there is an outer quad missing, fill it in\r
+ // first order all inner quads\r
+ int found = 0;\r
+ for (i=0; i<quad_count; i++)\r
+ {\r
+ if (quads[i]->count == 4)\r
+ { // ok, look at neighbors\r
+ int col = quads[i]->col;\r
+ int row = quads[i]->row;\r
+ for (int j=0; j<4; j++)\r
+ {\r
+ switch(j) // adjust col, row for this quad\r
+ { // start at top left, go clockwise\r
+ case 0:\r
+ row--; col--; break;\r
+ case 1:\r
+ col += 2; break;\r
+ case 2:\r
+ row += 2; break;\r
+ case 3:\r
+ col -= 2; break;\r
+ }\r
+ CvCBQuad *neighbor = quads[i]->neighbors[j];\r
+ if (neighbor && !neighbor->ordered && // is it an inner quad?\r
+ col <= col_max && col >= col_min &&\r
+ row <= row_max && row >= row_min)\r
+ {\r
+ // if so, set in order\r
+ PRINTF("Adding inner: col: %d row: %d\n", col, row);\r
+ found++;\r
+ icvOrderQuad(neighbor, quads[i]->corners[j], (j+2)%4);\r
+ neighbor->ordered = true;\r
+ neighbor->row = row;\r
+ neighbor->col = col;\r
+ }\r
+ }\r
+ }\r
+ }\r
+\r
+ // if we have found inner quads, add corresponding outer quads,\r
+ // which are missing\r
+ if (found > 0)\r
+ {\r
+ PRINTF("Found %d inner quads not connected to outer quads, repairing\n", found);\r
+ for (int i=0; i<quad_count; i++)\r
+ {\r
+ if (quads[i]->count < 4 && quads[i]->ordered)\r
+ {\r
+ int added = icvAddOuterQuad(quads[i],quads,quad_count,all_quads,*all_count,corners);\r
+ *all_count += added;\r
+ quad_count += added;\r
+ }\r
+ }\r
+ }\r
+\r
+\r
+ // final trimming of outer quads\r
+ if (dcol == w && drow == h) // found correct inner quads\r
+ {\r
+ PRINTF("Inner bounds ok, check outer quads\n");\r
+ int rcount = quad_count;\r
+ for (int i=quad_count-1; i>=0; i--) // eliminate any quad not connected to\r
+ // an ordered quad\r
+ {\r
+ if (quads[i]->ordered == false)\r
+ {\r
+ bool outer = false;\r
+ for (int j=0; j<4; j++) // any neighbors that are ordered?\r
+ {\r
+ if (quads[i]->neighbors[j] && quads[i]->neighbors[j]->ordered)\r
+ outer = true;\r
+ }\r
+ if (!outer) // not an outer quad, eliminate\r
+ {\r
+ PRINTF("Removing quad %d\n", i);\r
+ icvRemoveQuadFromGroup(quads,rcount,quads[i]);\r
+ rcount--;\r
+ }\r
+ }\r
+\r
+ }\r
+ return rcount;\r
+ }\r
+\r
+ return 0;\r
+}\r
+\r
+\r
+// add an outer quad\r
+// looks for the neighbor of <quad> that isn't present,\r
+// tries to add it in.\r
+// <quad> is ordered\r
+\r
+static int\r
+icvAddOuterQuad( CvCBQuad *quad, CvCBQuad **quads, int quad_count,\r
+ CvCBQuad **all_quads, int all_count, CvCBCorner **corners )\r
+\r
+{\r
+ int added = 0;\r
+ for (int i=0; i<4; i++) // find no-neighbor corners\r
+ {\r
+ if (!quad->neighbors[i]) // ok, create and add neighbor\r
+ {\r
+ int j = (i+2)%4;\r
+ PRINTF("Adding quad as neighbor 2\n");\r
+ CvCBQuad *q = &(*all_quads)[all_count];\r
+ memset( q, 0, sizeof(*q) );\r
+ added++;\r
+ quads[quad_count] = q;\r
+ quad_count++;\r
+\r
+ // set neighbor and group id\r
+ quad->neighbors[i] = q;\r
+ quad->count += 1;\r
+ q->neighbors[j] = quad;\r
+ q->group_idx = quad->group_idx;\r
+ q->count = 1; // number of neighbors\r
+ q->ordered = false;\r
+ q->edge_len = quad->edge_len;\r
+\r
+ // make corners of new quad\r
+ // same as neighbor quad, but offset\r
+ CvPoint2D32f pt = quad->corners[i]->pt;\r
+ CvCBCorner* corner;\r
+ float dx = pt.x - quad->corners[j]->pt.x;\r
+ float dy = pt.y - quad->corners[j]->pt.y;\r
+ for (int k=0; k<4; k++)\r
+ {\r
+ corner = &(*corners)[all_count*4+k];\r
+ pt = quad->corners[k]->pt;\r
+ memset( corner, 0, sizeof(*corner) );\r
+ corner->pt = pt;\r
+ q->corners[k] = corner;\r
+ corner->pt.x += dx;\r
+ corner->pt.y += dy;\r
+ }\r
+ // have to set exact corner\r
+ q->corners[j] = quad->corners[i];\r
+\r
+ // now find other neighbor and add it, if possible\r
+ if (quad->neighbors[(i+3)%4] &&\r
+ quad->neighbors[(i+3)%4]->ordered &&\r
+ quad->neighbors[(i+3)%4]->neighbors[i] &&\r
+ quad->neighbors[(i+3)%4]->neighbors[i]->ordered )\r
+ {\r
+ CvCBQuad *qn = quad->neighbors[(i+3)%4]->neighbors[i];\r
+ q->count = 2;\r
+ q->neighbors[(j+1)%4] = qn;\r
+ qn->neighbors[(i+1)%4] = q;\r
+ qn->count += 1;\r
+ // have to set exact corner\r
+ q->corners[(j+1)%4] = qn->corners[(i+1)%4];\r
+ }\r
+\r
+ all_count++;\r
+ }\r
+ }\r
+ return added;\r
+}\r
+\r
+\r
+// trimming routines\r
+\r
+static int\r
+icvTrimCol(CvCBQuad **quads, int count, int col, int dir)\r
+{\r
+ int rcount = count;\r
+ // find the right quad(s)\r
+ for (int i=0; i<count; i++)\r
+ {\r
+#ifdef DEBUG_CHESSBOARD\r
+ if (quads[i]->ordered)\r
+ PRINTF("index: %d cur: %d\n", col, quads[i]->col);\r
+#endif\r
+ if (quads[i]->ordered && quads[i]->col == col)\r
+ {\r
+ if (dir == 1)\r
+ {\r
+ if (quads[i]->neighbors[1])\r
+ {\r
+ icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[1]);\r
+ rcount--;\r
+ }\r
+ if (quads[i]->neighbors[2])\r
+ {\r
+ icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[2]);\r
+ rcount--;\r
+ }\r
+ }\r
+ else\r
+ {\r
+ if (quads[i]->neighbors[0])\r
+ {\r
+ icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[0]);\r
+ rcount--;\r
+ }\r
+ if (quads[i]->neighbors[3])\r
+ {\r
+ icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[3]);\r
+ rcount--;\r
+ }\r
+ }\r
+\r
+ }\r
+ }\r
+ return rcount;\r
+}\r
+\r
+static int\r
+icvTrimRow(CvCBQuad **quads, int count, int row, int dir)\r
+{\r
+ int i, rcount = count;\r
+ // find the right quad(s)\r
+ for (i=0; i<count; i++)\r
+ {\r
+#ifdef DEBUG_CHESSBOARD\r
+ if (quads[i]->ordered)\r
+ PRINTF("index: %d cur: %d\n", row, quads[i]->row);\r
+#endif\r
+ if (quads[i]->ordered && quads[i]->row == row)\r
+ {\r
+ if (dir == 1) // remove from bottom\r
+ {\r
+ if (quads[i]->neighbors[2])\r
+ {\r
+ icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[2]);\r
+ rcount--;\r
+ }\r
+ if (quads[i]->neighbors[3])\r
+ {\r
+ icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[3]);\r
+ rcount--;\r
+ }\r
+ }\r
+ else // remove from top\r
+ {\r
+ if (quads[i]->neighbors[0])\r
+ {\r
+ icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[0]);\r
+ rcount--;\r
+ }\r
+ if (quads[i]->neighbors[1])\r
+ {\r
+ icvRemoveQuadFromGroup(quads,rcount,quads[i]->neighbors[1]);\r
+ rcount--;\r
+ }\r
+ }\r
+\r
+ }\r
+ }\r
+ return rcount;\r
+}\r
+\r
+\r
+//\r
+// remove quad from quad group\r
+//\r
+\r
+static void\r
+icvRemoveQuadFromGroup(CvCBQuad **quads, int count, CvCBQuad *q0)\r
+{\r
+ int i, j;\r
+ // remove any references to this quad as a neighbor\r
+ for(i = 0; i < count; i++ )\r
+ {\r
+ CvCBQuad *q = quads[i];\r
+ for(j = 0; j < 4; j++ )\r
+ {\r
+ if( q->neighbors[j] == q0 )\r
+ {\r
+ q->neighbors[j] = 0;\r
+ q->count--;\r
+ for(int k = 0; k < 4; k++ )\r
+ if( q0->neighbors[k] == q )\r
+ {\r
+ q0->neighbors[k] = 0;\r
+ q0->count--;\r
+ break;\r
+ }\r
+ break;\r
+ }\r
+ }\r
+ }\r
+\r
+ // remove the quad\r
+ for(i = 0; i < count; i++ )\r
+ {\r
+ CvCBQuad *q = quads[i];\r
+ if (q == q0)\r
+ {\r
+ quads[i] = quads[count-1];\r
+ break;\r
+ }\r
+ }\r
+}\r
+\r
+//\r
+// put quad into correct order, where <corner> has value <common>\r
+//\r
+\r
+static void\r
+icvOrderQuad(CvCBQuad *quad, CvCBCorner *corner, int common)\r
+{\r
+ // find the corner\r
+ int tc;\r
+ for (tc=0; tc<4; tc++)\r
+ if (quad->corners[tc]->pt.x == corner->pt.x &&\r
+ quad->corners[tc]->pt.y == corner->pt.y)\r
+ break;\r
+\r
+ // set corner order\r
+ // shift\r
+ while (tc != common)\r
+ {\r
+ // shift by one\r
+ CvCBCorner *tempc;\r
+ CvCBQuad *tempq;\r
+ tempc = quad->corners[3];\r
+ tempq = quad->neighbors[3];\r
+ for (int i=3; i>0; i--)\r
+ {\r
+ quad->corners[i] = quad->corners[i-1];\r
+ quad->neighbors[i] = quad->neighbors[i-1];\r
+ }\r
+ quad->corners[0] = tempc;\r
+ quad->neighbors[0] = tempq;\r
+ tc++;\r
+ tc = tc%4;\r
+ }\r
+}\r
+\r
+\r
+// if we found too many connect quads, remove those which probably do not belong.\r
+static int\r
+icvCleanFoundConnectedQuads( int quad_count, CvCBQuad **quad_group, CvSize pattern_size )\r
+{\r
+ CvMemStorage *temp_storage = 0;\r
+ CvPoint2D32f *centers = 0;\r
+\r
+ CV_FUNCNAME( "icvCleanFoundConnectedQuads" );\r
+\r
+ __BEGIN__;\r
+\r
+ CvPoint2D32f center = {0,0};\r
+ int i, j, k;\r
+ // number of quads this pattern should contain\r
+ int count = ((pattern_size.width + 1)*(pattern_size.height + 1) + 1)/2;\r
+\r
+ // if we have more quadrangles than we should,\r
+ // try to eliminate duplicates or ones which don't belong to the pattern rectangle...\r
+ if( quad_count <= count )\r
+ EXIT;\r
+\r
+ // create an array of quadrangle centers\r
+ CV_CALL( centers = (CvPoint2D32f *)cvAlloc( sizeof(centers[0])*quad_count ));\r
+ CV_CALL( temp_storage = cvCreateMemStorage(0));\r
+\r
+ for( i = 0; i < quad_count; i++ )\r
+ {\r
+ CvPoint2D32f ci = {0,0};\r
+ CvCBQuad* q = quad_group[i];\r
+\r
+ for( j = 0; j < 4; j++ )\r
+ {\r
+ CvPoint2D32f pt = q->corners[j]->pt;\r
+ ci.x += pt.x;\r
+ ci.y += pt.y;\r
+ }\r
+\r
+ ci.x *= 0.25f;\r
+ ci.y *= 0.25f;\r
+\r
+ centers[i] = ci;\r
+ center.x += ci.x;\r
+ center.y += ci.y;\r
+ }\r
+ center.x /= quad_count;\r
+ center.y /= quad_count;\r
+\r
+ // If we still have more quadrangles than we should,\r
+ // we try to eliminate bad ones based on minimizing the bounding box.\r
+ // We iteratively remove the point which reduces the size of\r
+ // the bounding box of the blobs the most\r
+ // (since we want the rectangle to be as small as possible)\r
+ // remove the quadrange that causes the biggest reduction\r
+ // in pattern size until we have the correct number\r
+ for( ; quad_count > count; quad_count-- )\r
+ {\r
+ double min_box_area = DBL_MAX;\r
+ int skip, min_box_area_index = -1;\r
+ CvCBQuad *q0, *q;\r
+\r
+ // For each point, calculate box area without that point\r
+ for( skip = 0; skip < quad_count; skip++ )\r
+ {\r
+ // get bounding rectangle\r
+ CvPoint2D32f temp = centers[skip]; // temporarily make index 'skip' the same as\r
+ centers[skip] = center; // pattern center (so it is not counted for convex hull)\r
+ CvMat pointMat = cvMat(1, quad_count, CV_32FC2, centers);\r
+ CvSeq *hull = cvConvexHull2( &pointMat, temp_storage, CV_CLOCKWISE, 1 );\r
+ centers[skip] = temp;\r
+ double hull_area = fabs(cvContourArea(hull, CV_WHOLE_SEQ));\r
+\r
+ // remember smallest box area\r
+ if( hull_area < min_box_area )\r
+ {\r
+ min_box_area = hull_area;\r
+ min_box_area_index = skip;\r
+ }\r
+ cvClearMemStorage( temp_storage );\r
+ }\r
+\r
+ q0 = quad_group[min_box_area_index];\r
+\r
+ // remove any references to this quad as a neighbor\r
+ for( i = 0; i < quad_count; i++ )\r
+ {\r
+ q = quad_group[i];\r
+ for( j = 0; j < 4; j++ )\r
+ {\r
+ if( q->neighbors[j] == q0 )\r
+ {\r
+ q->neighbors[j] = 0;\r
+ q->count--;\r
+ for( k = 0; k < 4; k++ )\r
+ if( q0->neighbors[k] == q )\r
+ {\r
+ q0->neighbors[k] = 0;\r
+ q0->count--;\r
+ break;\r
+ }\r
+ break;\r
+ }\r
+ }\r
+ }\r
+\r
+ // remove the quad\r
+ quad_count--;\r
+ quad_group[min_box_area_index] = quad_group[quad_count];\r
+ centers[min_box_area_index] = centers[quad_count];\r
+ }\r
+\r
+ __END__;\r
+\r
+ cvReleaseMemStorage( &temp_storage );\r
+ cvFree( ¢ers );\r
+\r
+ return quad_count;\r
+}\r
+\r
+//=====================================================================================\r
+\r
+static int\r
+icvFindConnectedQuads( CvCBQuad *quad, int quad_count, CvCBQuad **out_group,\r
+ int group_idx, CvMemStorage* storage )\r
+{\r
+ CvMemStorage* temp_storage = cvCreateChildMemStorage( storage );\r
+ CvSeq* stack = cvCreateSeq( 0, sizeof(*stack), sizeof(void*), temp_storage );\r
+ int i, count = 0;\r
+\r
+ // Scan the array for a first unlabeled quad\r
+ for( i = 0; i < quad_count; i++ )\r
+ {\r
+ if( quad[i].count > 0 && quad[i].group_idx < 0)\r
+ break;\r
+ }\r
+\r
+ // Recursively find a group of connected quads starting from the seed quad[i]\r
+ if( i < quad_count )\r
+ {\r
+ CvCBQuad* q = &quad[i];\r
+ cvSeqPush( stack, &q );\r
+ out_group[count++] = q;\r
+ q->group_idx = group_idx;\r
+ q->ordered = false;\r
+\r
+ while( stack->total )\r
+ {\r
+ cvSeqPop( stack, &q );\r
+ for( i = 0; i < 4; i++ )\r
+ {\r
+ CvCBQuad *neighbor = q->neighbors[i];\r
+ if( neighbor && neighbor->count > 0 && neighbor->group_idx < 0 )\r
+ {\r
+ cvSeqPush( stack, &neighbor );\r
+ out_group[count++] = neighbor;\r
+ neighbor->group_idx = group_idx;\r
+ neighbor->ordered = false;\r
+ }\r
+ }\r
+ }\r
+ }\r
+\r
+ cvReleaseMemStorage( &temp_storage );\r
+ return count;\r
+}\r
+\r
+\r
+//=====================================================================================\r
+\r
+static int\r
+icvCheckQuadGroup( CvCBQuad **quad_group, int quad_count,\r
+ CvCBCorner **out_corners, CvSize pattern_size )\r
+{\r
+ const int ROW1 = 1000000;\r
+ const int ROW2 = 2000000;\r
+ const int ROW_ = 3000000;\r
+ int result = 0;\r
+ int i, out_corner_count = 0, corner_count = 0;\r
+ CvCBCorner** corners = 0;\r
+\r
+ CV_FUNCNAME( "icvCheckQuadGroup" );\r
+\r
+ __BEGIN__;\r
+\r
+ int j, k, kk;\r
+ int width = 0, height = 0;\r
+ int hist[5] = {0,0,0,0,0};\r
+ CvCBCorner* first = 0, *first2 = 0, *right, *cur, *below, *c;\r
+ CV_CALL( corners = (CvCBCorner**)cvAlloc( quad_count*4*sizeof(corners[0]) ));\r
+\r
+ // build dual graph, which vertices are internal quad corners\r
+ // and two vertices are connected iff they lie on the same quad edge\r
+ for( i = 0; i < quad_count; i++ )\r
+ {\r
+ CvCBQuad* q = quad_group[i];\r
+ /*CvScalar color = q->count == 0 ? cvScalar(0,255,255) :\r
+ q->count == 1 ? cvScalar(0,0,255) :\r
+ q->count == 2 ? cvScalar(0,255,0) :\r
+ q->count == 3 ? cvScalar(255,255,0) :\r
+ cvScalar(255,0,0);*/\r
+\r
+ for( j = 0; j < 4; j++ )\r
+ {\r
+ //cvLine( debug_img, cvPointFrom32f(q->corners[j]->pt), cvPointFrom32f(q->corners[(j+1)&3]->pt), color, 1, CV_AA, 0 );\r
+ if( q->neighbors[j] )\r
+ {\r
+ CvCBCorner *a = q->corners[j], *b = q->corners[(j+1)&3];\r
+ // mark internal corners that belong to:\r
+ // - a quad with a single neighbor - with ROW1,\r
+ // - a quad with two neighbors - with ROW2\r
+ // make the rest of internal corners with ROW_\r
+ int row_flag = q->count == 1 ? ROW1 : q->count == 2 ? ROW2 : ROW_;\r
+\r
+ if( a->row == 0 )\r
+ {\r
+ corners[corner_count++] = a;\r
+ a->row = row_flag;\r
+ }\r
+ else if( a->row > row_flag )\r
+ a->row = row_flag;\r
+\r
+ if( q->neighbors[(j+1)&3] )\r
+ {\r
+ if( a->count >= 4 || b->count >= 4 )\r
+ EXIT;\r
+ for( k = 0; k < 4; k++ )\r
+ {\r
+ if( a->neighbors[k] == b )\r
+ EXIT;\r
+ if( b->neighbors[k] == a )\r
+ EXIT;\r
+ }\r
+ a->neighbors[a->count++] = b;\r
+ b->neighbors[b->count++] = a;\r
+ }\r
+ }\r
+ }\r
+ }\r
+\r
+ if( corner_count != pattern_size.width*pattern_size.height )\r
+ EXIT;\r
+\r
+ for( i = 0; i < corner_count; i++ )\r
+ {\r
+ int n = corners[i]->count;\r
+ assert( 0 <= n && n <= 4 );\r
+ hist[n]++;\r
+ if( !first && n == 2 )\r
+ {\r
+ if( corners[i]->row == ROW1 )\r
+ first = corners[i];\r
+ else if( !first2 && corners[i]->row == ROW2 )\r
+ first2 = corners[i];\r
+ }\r
+ }\r
+\r
+ // start with a corner that belongs to a quad with a signle neighbor.\r
+ // if we do not have such, start with a corner of a quad with two neighbors.\r
+ if( !first )\r
+ first = first2;\r
+\r
+ if( !first || hist[0] != 0 || hist[1] != 0 || hist[2] != 4 ||\r
+ hist[3] != (pattern_size.width + pattern_size.height)*2 - 8 )\r
+ EXIT;\r
+\r
+ cur = first;\r
+ right = below = 0;\r
+ out_corners[out_corner_count++] = cur;\r
+\r
+ for( k = 0; k < 4; k++ )\r
+ {\r
+ c = cur->neighbors[k];\r
+ if( c )\r
+ {\r
+ if( !right )\r
+ right = c;\r
+ else if( !below )\r
+ below = c;\r
+ }\r
+ }\r
+\r
+ if( !right || (right->count != 2 && right->count != 3) ||\r
+ !below || (below->count != 2 && below->count != 3) )\r
+ EXIT;\r
+\r
+ cur->row = 0;\r
+ //cvCircle( debug_img, cvPointFrom32f(cur->pt), 3, cvScalar(0,255,0), -1, 8, 0 );\r
+\r
+ first = below; // remember the first corner in the next row\r
+ // find and store the first row (or column)\r
+ for(j=1;;j++)\r
+ {\r
+ right->row = 0;\r
+ out_corners[out_corner_count++] = right;\r
+ //cvCircle( debug_img, cvPointFrom32f(right->pt), 3, cvScalar(0,255-j*10,0), -1, 8, 0 );\r
+ if( right->count == 2 )\r
+ break;\r
+ if( right->count != 3 || out_corner_count >= MAX(pattern_size.width,pattern_size.height) )\r
+ EXIT;\r
+ cur = right;\r
+ for( k = 0; k < 4; k++ )\r
+ {\r
+ c = cur->neighbors[k];\r
+ if( c && c->row > 0 )\r
+ {\r
+ for( kk = 0; kk < 4; kk++ )\r
+ {\r
+ if( c->neighbors[kk] == below )\r
+ break;\r
+ }\r
+ if( kk < 4 )\r
+ below = c;\r
+ else\r
+ right = c;\r
+ }\r
+ }\r
+ }\r
+\r
+ width = out_corner_count;\r
+ if( width == pattern_size.width )\r
+ height = pattern_size.height;\r
+ else if( width == pattern_size.height )\r
+ height = pattern_size.width;\r
+ else\r
+ EXIT;\r
+\r
+ // find and store all the other rows\r
+ for( i = 1; ; i++ )\r
+ {\r
+ if( !first )\r
+ break;\r
+ cur = first;\r
+ first = 0;\r
+ for( j = 0;; j++ )\r
+ {\r
+ cur->row = i;\r
+ out_corners[out_corner_count++] = cur;\r
+ //cvCircle( debug_img, cvPointFrom32f(cur->pt), 3, cvScalar(0,0,255-j*10), -1, 8, 0 );\r
+ if( cur->count == 2 + (i < height-1) && j > 0 )\r
+ break;\r
+\r
+ right = 0;\r
+\r
+ // find a neighbor that has not been processed yet\r
+ // and that has a neighbor from the previous row\r
+ for( k = 0; k < 4; k++ )\r
+ {\r
+ c = cur->neighbors[k];\r
+ if( c && c->row > i )\r
+ {\r
+ for( kk = 0; kk < 4; kk++ )\r
+ {\r
+ if( c->neighbors[kk] && c->neighbors[kk]->row == i-1 )\r
+ break;\r
+ }\r
+ if( kk < 4 )\r
+ {\r
+ right = c;\r
+ if( j > 0 )\r
+ break;\r
+ }\r
+ else if( j == 0 )\r
+ first = c;\r
+ }\r
+ }\r
+ if( !right )\r
+ EXIT;\r
+ cur = right;\r
+ }\r
+\r
+ if( j != width - 1 )\r
+ EXIT;\r
+ }\r
+\r
+ if( out_corner_count != corner_count )\r
+ EXIT;\r
+\r
+ // check if we need to transpose the board\r
+ if( width != pattern_size.width )\r
+ {\r
+ CV_SWAP( width, height, k );\r
+\r
+ memcpy( corners, out_corners, corner_count*sizeof(corners[0]) );\r
+ for( i = 0; i < height; i++ )\r
+ for( j = 0; j < width; j++ )\r
+ out_corners[i*width + j] = corners[j*height + i];\r
+ }\r
+\r
+ // check if we need to revert the order in each row\r
+ {\r
+ CvPoint2D32f p0 = out_corners[0]->pt, p1 = out_corners[pattern_size.width-1]->pt,\r
+ p2 = out_corners[pattern_size.width]->pt;\r
+ if( (p1.x - p0.x)*(p2.y - p1.y) - (p1.y - p0.y)*(p2.x - p1.x) < 0 )\r
+ {\r
+ if( width % 2 == 0 )\r
+ {\r
+ for( i = 0; i < height; i++ )\r
+ for( j = 0; j < width/2; j++ )\r
+ CV_SWAP( out_corners[i*width+j], out_corners[i*width+width-j-1], c );\r
+ }\r
+ else\r
+ {\r
+ for( j = 0; j < width; j++ )\r
+ for( i = 0; i < height/2; i++ )\r
+ CV_SWAP( out_corners[i*width+j], out_corners[(height - i - 1)*width+j], c );\r
+ }\r
+ }\r
+ }\r
+\r
+ result = corner_count;\r
+\r
+ __END__;\r
+\r
+ if( result <= 0 && corners )\r
+ {\r
+ corner_count = MIN( corner_count, pattern_size.width*pattern_size.height );\r
+ for( i = 0; i < corner_count; i++ )\r
+ out_corners[i] = corners[i];\r
+ result = -corner_count;\r
+\r
+ if (result == -pattern_size.width*pattern_size.height)\r
+ result = -result;\r
+ }\r
+\r
+ cvFree( &corners );\r
+\r
+ return result;\r
+}\r
+\r
+\r
+\r
+\r
+//=====================================================================================\r
+\r
+static void icvFindQuadNeighbors( CvCBQuad *quads, int quad_count )\r
+{\r
+ const float thresh_scale = 1.f;\r
+ int idx, i, k, j;\r
+ float dx, dy, dist;\r
+\r
+ // find quad neighbors\r
+ for( idx = 0; idx < quad_count; idx++ )\r
+ {\r
+ CvCBQuad* cur_quad = &quads[idx];\r
+\r
+ // choose the points of the current quadrangle that are close to\r
+ // some points of the other quadrangles\r
+ // (it can happen for split corners (due to dilation) of the\r
+ // checker board). Search only in other quadrangles!\r
+\r
+ // for each corner of this quadrangle\r
+ for( i = 0; i < 4; i++ )\r
+ {\r
+ CvPoint2D32f pt;\r
+ float min_dist = FLT_MAX;\r
+ int closest_corner_idx = -1;\r
+ CvCBQuad *closest_quad = 0;\r
+ CvCBCorner *closest_corner = 0;\r
+\r
+ if( cur_quad->neighbors[i] )\r
+ continue;\r
+\r
+ pt = cur_quad->corners[i]->pt;\r
+\r
+ // find the closest corner in all other quadrangles\r
+ for( k = 0; k < quad_count; k++ )\r
+ {\r
+ if( k == idx )\r
+ continue;\r
+\r
+ for( j = 0; j < 4; j++ )\r
+ {\r
+ if( quads[k].neighbors[j] )\r
+ continue;\r
+\r
+ dx = pt.x - quads[k].corners[j]->pt.x;\r
+ dy = pt.y - quads[k].corners[j]->pt.y;\r
+ dist = dx * dx + dy * dy;\r
+\r
+ if( dist < min_dist &&\r
+ dist <= cur_quad->edge_len*thresh_scale &&\r
+ dist <= quads[k].edge_len*thresh_scale )\r
+ {\r
+ // check edge lengths, make sure they're compatible\r
+ // edges that are different by more than 1:4 are rejected\r
+ float ediff = cur_quad->edge_len - quads[k].edge_len;\r
+ if (ediff > 32*cur_quad->edge_len ||\r
+ ediff > 32*quads[k].edge_len)\r
+ {\r
+ PRINTF("Incompatible edge lengths\n");\r
+ continue;\r
+ }\r
+ closest_corner_idx = j;\r
+ closest_quad = &quads[k];\r
+ min_dist = dist;\r
+ }\r
+ }\r
+ }\r
+\r
+ // we found a matching corner point?\r
+ if( closest_corner_idx >= 0 && min_dist < FLT_MAX )\r
+ {\r
+ // If another point from our current quad is closer to the found corner\r
+ // than the current one, then we don't count this one after all.\r
+ // This is necessary to support small squares where otherwise the wrong\r
+ // corner will get matched to closest_quad;\r
+ closest_corner = closest_quad->corners[closest_corner_idx];\r
+\r
+ for( j = 0; j < 4; j++ )\r
+ {\r
+ if( cur_quad->neighbors[j] == closest_quad )\r
+ break;\r
+\r
+ dx = closest_corner->pt.x - cur_quad->corners[j]->pt.x;\r
+ dy = closest_corner->pt.y - cur_quad->corners[j]->pt.y;\r
+\r
+ if( dx * dx + dy * dy < min_dist )\r
+ break;\r
+ }\r
+\r
+ if( j < 4 || cur_quad->count >= 4 || closest_quad->count >= 4 )\r
+ continue;\r
+\r
+ // Check that each corner is a neighbor of different quads\r
+ for( j = 0; j < closest_quad->count; j++ )\r
+ {\r
+ if( closest_quad->neighbors[j] == cur_quad )\r
+ break;\r
+ }\r
+ if( j < closest_quad->count )\r
+ continue;\r
+\r
+ // check whether the closest corner to closest_corner\r
+ // is different from cur_quad->corners[i]->pt\r
+ for( k = 0; k < quad_count; k++ )\r
+ {\r
+ CvCBQuad* q = &quads[k];\r
+ if( k == idx || q == closest_quad )\r
+ continue;\r
+\r
+ for( j = 0; j < 4; j++ )\r
+ if( !q->neighbors[j] )\r
+ {\r
+ dx = closest_corner->pt.x - q->corners[j]->pt.x;\r
+ dy = closest_corner->pt.y - q->corners[j]->pt.y;\r
+ dist = dx*dx + dy*dy;\r
+ if( dist < min_dist )\r
+ break;\r
+ }\r
+ if( j < 4 )\r
+ break;\r
+ }\r
+\r
+ if( k < quad_count )\r
+ continue;\r
+\r
+ closest_corner->pt.x = (pt.x + closest_corner->pt.x) * 0.5f;\r
+ closest_corner->pt.y = (pt.y + closest_corner->pt.y) * 0.5f;\r
+\r
+ // We've found one more corner - remember it\r
+ cur_quad->count++;\r
+ cur_quad->neighbors[i] = closest_quad;\r
+ cur_quad->corners[i] = closest_corner;\r
+\r
+ closest_quad->count++;\r
+ closest_quad->neighbors[closest_corner_idx] = cur_quad;\r
+ }\r
+ }\r
+ }\r
+}\r
+\r
+//=====================================================================================\r
+\r
+// returns corners in clockwise order\r
+// corners don't necessarily start at same position on quad (e.g.,\r
+// top left corner)\r
+\r
+static int\r
+icvGenerateQuads( CvCBQuad **out_quads, CvCBCorner **out_corners,\r
+ CvMemStorage *storage, CvMat *image, int flags )\r
+{\r
+ int quad_count = 0;\r
+ CvMemStorage *temp_storage = 0;\r
+\r
+ if( out_quads )\r
+ *out_quads = 0;\r
+\r
+ if( out_corners )\r
+ *out_corners = 0;\r
+\r
+ CV_FUNCNAME( "icvGenerateQuads" );\r
+\r
+ __BEGIN__;\r
+\r
+ CvSeq *src_contour = 0;\r
+ CvSeq *root;\r
+ CvContourEx* board = 0;\r
+ CvContourScanner scanner;\r
+ int i, idx, min_size;\r
+\r
+ CV_ASSERT( out_corners && out_quads );\r
+\r
+ // empiric bound for minimal allowed perimeter for squares\r
+ min_size = 25; //cvRound( image->cols * image->rows * .03 * 0.01 * 0.92 );\r
+\r
+ // create temporary storage for contours and the sequence of pointers to found quadrangles\r
+ CV_CALL( temp_storage = cvCreateChildMemStorage( storage ));\r
+ CV_CALL( root = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSeq*), temp_storage ));\r
+\r
+ // initialize contour retrieving routine\r
+ CV_CALL( scanner = cvStartFindContours( image, temp_storage, sizeof(CvContourEx),\r
+ CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE ));\r
+\r
+ // get all the contours one by one\r
+ while( (src_contour = cvFindNextContour( scanner )) != 0 )\r
+ {\r
+ CvSeq *dst_contour = 0;\r
+ CvRect rect = ((CvContour*)src_contour)->rect;\r
+\r
+ // reject contours with too small perimeter\r
+ if( CV_IS_SEQ_HOLE(src_contour) && rect.width*rect.height >= min_size )\r
+ {\r
+ const int min_approx_level = 2, max_approx_level = MAX_CONTOUR_APPROX;\r
+ int approx_level;\r
+ for( approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++ )\r
+ {\r
+ dst_contour = cvApproxPoly( src_contour, sizeof(CvContour), temp_storage,\r
+ CV_POLY_APPROX_DP, (float)approx_level );\r
+ // we call this again on its own output, because sometimes\r
+ // cvApproxPoly() does not simplify as much as it should.\r
+ dst_contour = cvApproxPoly( dst_contour, sizeof(CvContour), temp_storage,\r
+ CV_POLY_APPROX_DP, (float)approx_level );\r
+\r
+ if( dst_contour->total == 4 )\r
+ break;\r
+ }\r
+\r
+ // reject non-quadrangles\r
+ if( dst_contour->total == 4 && cvCheckContourConvexity(dst_contour) )\r
+ {\r
+ CvPoint pt[4];\r
+ double d1, d2, p = cvContourPerimeter(dst_contour);\r
+ double area = fabs(cvContourArea(dst_contour, CV_WHOLE_SEQ));\r
+ double dx, dy;\r
+\r
+ for( i = 0; i < 4; i++ )\r
+ pt[i] = *(CvPoint*)cvGetSeqElem(dst_contour, i);\r
+\r
+ dx = pt[0].x - pt[2].x;\r
+ dy = pt[0].y - pt[2].y;\r
+ d1 = sqrt(dx*dx + dy*dy);\r
+\r
+ dx = pt[1].x - pt[3].x;\r
+ dy = pt[1].y - pt[3].y;\r
+ d2 = sqrt(dx*dx + dy*dy);\r
+\r
+ // philipg. Only accept those quadrangles which are more square\r
+ // than rectangular and which are big enough\r
+ double d3, d4;\r
+ dx = pt[0].x - pt[1].x;\r
+ dy = pt[0].y - pt[1].y;\r
+ d3 = sqrt(dx*dx + dy*dy);\r
+ dx = pt[1].x - pt[2].x;\r
+ dy = pt[1].y - pt[2].y;\r
+ d4 = sqrt(dx*dx + dy*dy);\r
+ if( !(flags & CV_CALIB_CB_FILTER_QUADS) ||\r
+ (d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > min_size &&\r
+ d1 >= 0.15 * p && d2 >= 0.15 * p) )\r
+ {\r
+ CvContourEx* parent = (CvContourEx*)(src_contour->v_prev);\r
+ parent->counter++;\r
+ if( !board || board->counter < parent->counter )\r
+ board = parent;\r
+ dst_contour->v_prev = (CvSeq*)parent;\r
+ //for( i = 0; i < 4; i++ ) cvLine( debug_img, pt[i], pt[(i+1)&3], cvScalar(200,255,255), 1, CV_AA, 0 );\r
+ cvSeqPush( root, &dst_contour );\r
+ }\r
+ }\r
+ }\r
+ }\r
+\r
+ // finish contour retrieving\r
+ cvEndFindContours( &scanner );\r
+\r
+ // allocate quad & corner buffers\r
+ CV_CALL( *out_quads = (CvCBQuad*)cvAlloc((root->total+root->total / 2) * sizeof((*out_quads)[0])));\r
+ CV_CALL( *out_corners = (CvCBCorner*)cvAlloc((root->total+root->total / 2) * 4 * sizeof((*out_corners)[0])));\r
+\r
+ // Create array of quads structures\r
+ for( idx = 0; idx < root->total; idx++ )\r
+ {\r
+ CvCBQuad* q = &(*out_quads)[quad_count];\r
+ src_contour = *(CvSeq**)cvGetSeqElem( root, idx );\r
+ if( (flags & CV_CALIB_CB_FILTER_QUADS) && src_contour->v_prev != (CvSeq*)board )\r
+ continue;\r
+\r
+ // reset group ID\r
+ memset( q, 0, sizeof(*q) );\r
+ q->group_idx = -1;\r
+ assert( src_contour->total == 4 );\r
+ for( i = 0; i < 4; i++ )\r
+ {\r
+ CvPoint2D32f pt = cvPointTo32f(*(CvPoint*)cvGetSeqElem(src_contour, i));\r
+ CvCBCorner* corner = &(*out_corners)[quad_count*4 + i];\r
+\r
+ memset( corner, 0, sizeof(*corner) );\r
+ corner->pt = pt;\r
+ q->corners[i] = corner;\r
+ }\r
+ q->edge_len = FLT_MAX;\r
+ for( i = 0; i < 4; i++ )\r
+ {\r
+ float dx = q->corners[i]->pt.x - q->corners[(i+1)&3]->pt.x;\r
+ float dy = q->corners[i]->pt.y - q->corners[(i+1)&3]->pt.y;\r
+ float d = dx*dx + dy*dy;\r
+ if( q->edge_len > d )\r
+ q->edge_len = d;\r
+ }\r
+ quad_count++;\r
+ }\r
+\r
+ __END__;\r
+\r
+ if( cvGetErrStatus() < 0 )\r
+ {\r
+ if( out_quads )\r
+ cvFree( out_quads );\r
+ if( out_corners )\r
+ cvFree( out_corners );\r
+ quad_count = 0;\r
+ }\r
+\r
+ cvReleaseMemStorage( &temp_storage );\r
+ return quad_count;\r
+}\r
+\r
+\r
+//=====================================================================================\r
+\r
+#if 0\r
+static int is_equal_quad( const void* _a, const void* _b, void* )\r
+{\r
+ CvRect a = (*((CvContour**)_a))->rect;\r
+ CvRect b = (*((CvContour**)_b))->rect;\r
+\r
+ int dx = MIN( b.x + b.width - 1, a.x + a.width - 1) - MAX( b.x, a.x);\r
+ int dy = MIN( b.y + b.height - 1, a.y + a.height - 1) - MAX( b.y, a.y);\r
+ int w = (a.width + b.width)>>1;\r
+ int h = (a.height + b.height)>>1;\r
+\r
+ if( dx > w*0.75 && dy > h*0.75 && dx < w*1.25 && dy < h*1.25 ) return 1;\r
+\r
+ return 0;\r
+}\r
+\r
+static int\r
+icvGenerateQuadsEx( CvCBQuad **out_quads, CvCBCorner **out_corners,\r
+ CvMemStorage *storage, CvMat *image, CvMat *thresh_img, int dilations, int flags )\r
+{\r
+ int l;\r
+ int quad_count = 0;\r
+ CvMemStorage *temp_storage = 0;\r
+\r
+ if( out_quads )\r
+ *out_quads = 0;\r
+\r
+ if( out_corners )\r
+ *out_corners = 0;\r
+\r
+ CV_FUNCNAME( "icvGenerateQuads" );\r
+\r
+ __BEGIN__;\r
+\r
+ CvSeq *src_contour = 0;\r
+ CvSeq *root, *root_tmp;\r
+ CvContourEx* board = 0;\r
+ CvContourScanner scanner;\r
+ int i, idx, min_size;\r
+ int step_level = 25;\r
+\r
+ CV_ASSERT( out_corners && out_quads );\r
+\r
+ // empiric bound for minimal allowed perimeter for squares\r
+ min_size = cvRound( image->cols * image->rows * .03 * 0.01 * 0.92 );\r
+\r
+ // create temporary storage for contours and the sequence of pointers to found quadrangles\r
+ CV_CALL( temp_storage = cvCreateChildMemStorage( storage ));\r
+ CV_CALL( root_tmp = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSeq*), temp_storage ));\r
+ CV_CALL( root = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSeq*), temp_storage ));\r
+\r
+ //perform contours slicing\r
+ cvEqualizeHist(image,image);\r
+ for(l = step_level; l < 256-step_level; l+= step_level)\r
+ {\r
+ cvThreshold( image, thresh_img, l, 255, CV_THRESH_BINARY );\r
+ cvDilate( thresh_img, thresh_img, 0, dilations );\r
+\r
+ //draw frame to extract edge quads\r
+ cvRectangle( thresh_img, cvPoint(0,0), cvPoint(thresh_img->cols-1,\r
+ thresh_img->rows-1), CV_RGB(255,255,255), 3, 8);\r
+\r
+ // initialize contour retrieving routine\r
+ CV_CALL( scanner = cvStartFindContours( thresh_img, temp_storage, sizeof(CvContourEx),\r
+ CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE ));\r
+\r
+ // get all the contours one by one\r
+ while( (src_contour = cvFindNextContour( scanner )) != 0 )\r
+ {\r
+ CvSeq *dst_contour = 0;\r
+ CvRect rect = ((CvContour*)src_contour)->rect;\r
+\r
+ // reject contours with too small perimeter\r
+ if( CV_IS_SEQ_HOLE(src_contour) && rect.width*rect.height >= min_size )\r
+ {\r
+ const int min_approx_level = 2, max_approx_level = MAX_CONTOUR_APPROX;\r
+ int approx_level;\r
+ for( approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++ )\r
+ {\r
+ dst_contour = cvApproxPoly( src_contour, sizeof(CvContour), temp_storage,\r
+ CV_POLY_APPROX_DP, (float)approx_level );\r
+ // we call this again on its own output, because sometimes\r
+ // cvApproxPoly() does not simplify as much as it should.\r
+ dst_contour = cvApproxPoly( dst_contour, sizeof(CvContour), temp_storage,\r
+ CV_POLY_APPROX_DP, (float)approx_level );\r
+\r
+ if( dst_contour->total == 4 )\r
+ break;\r
+ }\r
+\r
+ // reject non-quadrangles\r
+ if( dst_contour->total == 4 && cvCheckContourConvexity(dst_contour) )\r
+ {\r
+ CvPoint pt[4];\r
+ double d1, d2, p = cvContourPerimeter(dst_contour);\r
+ double area = fabs(cvContourArea(dst_contour, CV_WHOLE_SEQ));\r
+ double dx, dy;\r
+\r
+ for( i = 0; i < 4; i++ )\r
+ pt[i] = *(CvPoint*)cvGetSeqElem(dst_contour, i);\r
+\r
+ //check border condition. if this is edge square we will add this as is\r
+ int edge_flag = 0, eps = 2;\r
+ for( i = 0; i < 4; i++ )\r
+ if( pt[i].x <= eps || pt[i].y <= eps ||\r
+ pt[i].x >= image->width - eps ||\r
+ pt[i].y >= image->height - eps ) edge_flag = 1;\r
+\r
+ dx = pt[0].x - pt[2].x;\r
+ dy = pt[0].y - pt[2].y;\r
+ d1 = sqrt(dx*dx + dy*dy);\r
+\r
+ dx = pt[1].x - pt[3].x;\r
+ dy = pt[1].y - pt[3].y;\r
+ d2 = sqrt(dx*dx + dy*dy);\r
+\r
+ // philipg. Only accept those quadrangles which are more square\r
+ // than rectangular and which are big enough\r
+ double d3, d4;\r
+ dx = pt[0].x - pt[1].x;\r
+ dy = pt[0].y - pt[1].y;\r
+ d3 = sqrt(dx*dx + dy*dy);\r
+ dx = pt[1].x - pt[2].x;\r
+ dy = pt[1].y - pt[2].y;\r
+ d4 = sqrt(dx*dx + dy*dy);\r
+ if( edge_flag ||\r
+ (!(flags & CV_CALIB_CB_FILTER_QUADS) ||\r
+ (d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > min_size &&\r
+ d1 >= 0.15 * p && d2 >= 0.15 * p)) )\r
+ {\r
+ CvContourEx* parent = (CvContourEx*)(src_contour->v_prev);\r
+ parent->counter++;\r
+ if( !board || board->counter < parent->counter )\r
+ board = parent;\r
+ dst_contour->v_prev = (CvSeq*)parent;\r
+ //for( i = 0; i < 4; i++ ) cvLine( debug_img, pt[i], pt[(i+1)&3], cvScalar(200,255,255), 1, CV_AA, 0 );\r
+ cvSeqPush( root_tmp, &dst_contour );\r
+ }\r
+ }\r
+ }\r
+ }\r
+ // finish contour retrieving\r
+ cvEndFindContours( &scanner );\r
+ }\r
+\r
+\r
+ // Perform clustering of extracted quads\r
+ // Same quad can be extracted from different binarization levels\r
+ if( root_tmp->total )\r
+ {\r
+ CvSeq* idx_seq = 0;\r
+ int n_quads = cvSeqPartition( root_tmp, temp_storage, &idx_seq, is_equal_quad, 0 );\r
+ for( i = 0; i < n_quads; i++ )\r
+ {\r
+ //extract biggest quad in group\r
+ int max_size = 0;\r
+ CvSeq* max_seq = 0;\r
+ for( int j = 0; j < root_tmp->total; j++ )\r
+ {\r
+ int index = *(int*)cvGetSeqElem(idx_seq, j);\r
+ if(index!=i) continue;\r
+ CvContour* cnt = *(CvContour**)cvGetSeqElem(root_tmp, j);\r
+ if(cnt->rect.width > max_size)\r
+ {\r
+ max_size = cnt->rect.width;\r
+ max_seq = (CvSeq*)cnt;\r
+ }\r
+ }\r
+ cvSeqPush( root, &max_seq);\r
+ }\r
+ }\r
+\r
+ // allocate quad & corner buffers\r
+ CV_CALL( *out_quads = (CvCBQuad*)cvAlloc((root->total+root->total / 2) * sizeof((*out_quads)[0])));\r
+ CV_CALL( *out_corners = (CvCBCorner*)cvAlloc((root->total+root->total / 2) * 4 * sizeof((*out_corners)[0])));\r
+\r
+ // Create array of quads structures\r
+ for( idx = 0; idx < root->total; idx++ )\r
+ {\r
+ CvCBQuad* q = &(*out_quads)[quad_count];\r
+ src_contour = *(CvSeq**)cvGetSeqElem( root, idx );\r
+ if( (flags & CV_CALIB_CB_FILTER_QUADS) && src_contour->v_prev != (CvSeq*)board )\r
+ continue;\r
+\r
+ // reset group ID\r
+ memset( q, 0, sizeof(*q) );\r
+ q->group_idx = -1;\r
+ assert( src_contour->total == 4 );\r
+ for( i = 0; i < 4; i++ )\r
+ {\r
+ CvPoint2D32f pt = cvPointTo32f(*(CvPoint*)cvGetSeqElem(src_contour, i));\r
+ CvCBCorner* corner = &(*out_corners)[quad_count*4 + i];\r
+\r
+ memset( corner, 0, sizeof(*corner) );\r
+ corner->pt = pt;\r
+ q->corners[i] = corner;\r
+ }\r
+ q->edge_len = FLT_MAX;\r
+ for( i = 0; i < 4; i++ )\r
+ {\r
+ float dx = q->corners[i]->pt.x - q->corners[(i+1)&3]->pt.x;\r
+ float dy = q->corners[i]->pt.y - q->corners[(i+1)&3]->pt.y;\r
+ float d = dx*dx + dy*dy;\r
+ if( q->edge_len > d )\r
+ q->edge_len = d;\r
+ }\r
+ quad_count++;\r
+ }\r
+\r
+ __END__;\r
+\r
+ if( cvGetErrStatus() < 0 )\r
+ {\r
+ if( out_quads )\r
+ cvFree( out_quads );\r
+ if( out_corners )\r
+ cvFree( out_corners );\r
+ quad_count = 0;\r
+ }\r
+\r
+ cvReleaseMemStorage( &temp_storage );\r
+ return quad_count;\r
+}\r
+#endif\r
+\r
+CV_IMPL void\r
+cvDrawChessboardCorners( CvArr* _image, CvSize pattern_size,\r
+ CvPoint2D32f* corners, int count, int found )\r
+{\r
+ CV_FUNCNAME( "cvDrawChessboardCorners" );\r
+\r
+ __BEGIN__;\r
+\r
+ const int shift = 0;\r
+ const int radius = 4;\r
+ const int r = radius*(1 << shift);\r
+ int i;\r
+ CvMat stub, *image;\r
+ double scale = 1;\r
+ int type, cn, line_type;\r
+\r
+ CV_CALL( image = cvGetMat( _image, &stub ));\r
+\r
+ type = CV_MAT_TYPE(image->type);\r
+ cn = CV_MAT_CN(type);\r
+ if( cn != 1 && cn != 3 && cn != 4 )\r
+ CV_ERROR( CV_StsUnsupportedFormat, "Number of channels must be 1, 3 or 4" );\r
+\r
+ switch( CV_MAT_DEPTH(image->type) )\r
+ {\r
+ case CV_8U:\r
+ scale = 1;\r
+ break;\r
+ case CV_16U:\r
+ scale = 256;\r
+ break;\r
+ case CV_32F:\r
+ scale = 1./255;\r
+ break;\r
+ default:\r
+ CV_ERROR( CV_StsUnsupportedFormat,\r
+ "Only 8-bit, 16-bit or floating-point 32-bit images are supported" );\r
+ }\r
+\r
+ line_type = type == CV_8UC1 || type == CV_8UC3 ? CV_AA : 8;\r
+\r
+ if( !found )\r
+ {\r
+ CvScalar color = {{0,0,255}};\r
+ if( cn == 1 )\r
+ color = cvScalarAll(200);\r
+ color.val[0] *= scale;\r
+ color.val[1] *= scale;\r
+ color.val[2] *= scale;\r
+ color.val[3] *= scale;\r
+\r
+ for( i = 0; i < count; i++ )\r
+ {\r
+ CvPoint pt;\r
+ pt.x = cvRound(corners[i].x*(1 << shift));\r
+ pt.y = cvRound(corners[i].y*(1 << shift));\r
+ cvLine( image, cvPoint( pt.x - r, pt.y - r ),\r
+ cvPoint( pt.x + r, pt.y + r ), color, 1, line_type, shift );\r
+ cvLine( image, cvPoint( pt.x - r, pt.y + r),\r
+ cvPoint( pt.x + r, pt.y - r), color, 1, line_type, shift );\r
+ cvCircle( image, pt, r+(1<<shift), color, 1, line_type, shift );\r
+ }\r
+ }\r
+ else\r
+ {\r
+ int x, y;\r
+ CvPoint prev_pt = {0, 0};\r
+ const int line_max = 7;\r
+ static const CvScalar line_colors[line_max] =\r
+ {\r
+ {{0,0,255}},\r
+ {{0,128,255}},\r
+ {{0,200,200}},\r
+ {{0,255,0}},\r
+ {{200,200,0}},\r
+ {{255,0,0}},\r
+ {{255,0,255}}\r
+ };\r
+\r
+ for( y = 0, i = 0; y < pattern_size.height; y++ )\r
+ {\r
+ CvScalar color = line_colors[y % line_max];\r
+ if( cn == 1 )\r
+ color = cvScalarAll(200);\r
+ color.val[0] *= scale;\r
+ color.val[1] *= scale;\r
+ color.val[2] *= scale;\r
+ color.val[3] *= scale;\r
+\r
+ for( x = 0; x < pattern_size.width; x++, i++ )\r
+ {\r
+ CvPoint pt;\r
+ pt.x = cvRound(corners[i].x*(1 << shift));\r
+ pt.y = cvRound(corners[i].y*(1 << shift));\r
+\r
+ if( i != 0 )\r
+ cvLine( image, prev_pt, pt, color, 1, line_type, shift );\r
+\r
+ cvLine( image, cvPoint(pt.x - r, pt.y - r),\r
+ cvPoint(pt.x + r, pt.y + r), color, 1, line_type, shift );\r
+ cvLine( image, cvPoint(pt.x - r, pt.y + r),\r
+ cvPoint(pt.x + r, pt.y - r), color, 1, line_type, shift );\r
+ cvCircle( image, pt, r+(1<<shift), color, 1, line_type, shift );\r
+ prev_pt = pt;\r
+ }\r
+ }\r
+ }\r
+\r
+ __END__;\r
+}\r
+\r
+namespace cv\r
+{\r
+\r
+bool findChessboardCorners( const Mat& image, Size patternSize,\r
+ vector<Point2f>& corners, int flags )\r
+{\r
+ int count = patternSize.area()*2;\r
+ corners.resize(count);\r
+ CvMat _image = image;\r
+ bool ok = cvFindChessboardCorners(&_image, patternSize,\r
+ (CvPoint2D32f*)&corners[0], &count, flags ) > 0;\r
+ corners.resize(count);\r
+ return ok;\r
+}\r
+\r
+void drawChessboardCorners( Mat& image, Size patternSize,\r
+ const Mat& corners,\r
+ bool patternWasFound )\r
+{\r
+ CvMat _image = image;\r
+ CV_Assert((corners.cols == 1 || corners.rows == 1) &&\r
+ corners.type() == CV_32FC2 && corners.isContinuous());\r
+ cvDrawChessboardCorners( &_image, patternSize, (CvPoint2D32f*)corners.data,\r
+ corners.cols + corners.rows - 1, patternWasFound );\r
+}\r
+\r
+}\r
+\r
+/* End of file. */\r
projects / opencv / blobdiff