--- calibinit.cpp 2010-08-20 17:11:32.000000000 +0200 +++ calibinit.cpp 2010-08-26 17:09:10.000000000 +0200 @@ -61,11 +61,13 @@ #include "precomp.hpp" #include +#include //#define ENABLE_TRIM_COL_ROW //#pragma comment(lib, "highgui200d.lib") //#define DEBUG_CHESSBOARD +//#define DEBUG_CHESSBOARD_EVERY_CORNER #ifdef DEBUG_CHESSBOARD static int PRINTF( const char* fmt, ... ) { @@ -151,7 +153,7 @@ icvGenerateQuadsEx( CvCBQuad **out_quads, CvCBCorner **out_corners, CvMemStorage *storage, CvMat *image, CvMat *thresh_img, int dilation, int flags );*/ -static void icvFindQuadNeighbors( CvCBQuad *quads, int quad_count ); +static void icvFindQuadNeighbors( CvCBQuad *quads, int quad_count, cv::Ptr ); static int icvFindConnectedQuads( CvCBQuad *quads, int quad_count, CvCBQuad **quad_group, int group_idx, @@ -234,12 +236,13 @@ cv::Ptr dbg_img; cv::Ptr dbg1_img; cv::Ptr dbg2_img; + cv::Ptr dbg3_img; #endif cv::Ptr storage; CvMat stub, *img = (CvMat*)arr; - int expected_corners_num = (pattern_size.width/2+1)*(pattern_size.height/2+1); + int expected_quad_num = ((pattern_size.width+1)*(pattern_size.height+1)+1)/2; int prev_sqr_size = 0; @@ -269,6 +272,7 @@ dbg_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3 ); dbg1_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3 ); dbg2_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3 ); + dbg3_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3 ); #endif if( CV_MAT_CN(img->type) != 1 || (flags & CV_CALIB_CB_NORMALIZE_IMAGE) ) @@ -320,7 +324,7 @@ // Run multi-level quads extraction // In case one-level binarization did not give enough number of quads CV_CALL( quad_count = icvGenerateQuadsEx( &quads, &corners, storage, img, thresh_img, dilations, flags )); - PRINTF("EX quad count: %d/%d\n", quad_count, expected_corners_num); + PRINTF("EX quad count: %d/%d\n", quad_count, expected_quad_num); } else*/ { @@ -363,7 +367,7 @@ quad_count = icvGenerateQuads( &quads, &corners, storage, thresh_img, flags ); - PRINTF("Quad count: %d/%d\n", quad_count, expected_corners_num); + PRINTF("Quad count: %d/%d\n", quad_count, expected_quad_num); } @@ -396,7 +400,33 @@ continue; // Find quad's neighbors - icvFindQuadNeighbors( quads, quad_count ); +#ifdef DEBUG_CHESSBOARD + icvFindQuadNeighbors( quads, quad_count, dbg_img ); +#else + icvFindQuadNeighbors( quads, quad_count, NULL ); +#endif + +#ifdef DEBUG_CHESSBOARD + cvCopy(dbg_img, dbg3_img); + cvNamedWindow("quads_neighbors", 1); + for (i = 0; i < quad_count; i++) { + for (int k=0; k<4; k++) + { + CvPoint2D32f pt1, pt2; + CvScalar color = CV_RGB(30,255,30); + pt1 = quads[i].corners[k]->pt; + pt2 = quads[i].corners[(k+1)%4]->pt; + if (k>0) + color = CV_RGB(200,200,0); + cvLine( dbg3_img, cvPointFrom32f(pt1), cvPointFrom32f(pt2), color, 2, 8); + if (quads[i].neighbors[k] != NULL) { + cvCircle(dbg3_img, cvPointFrom32f(pt1), 4, CV_RGB(255,0,0), 1, 8, 0); + } + } + } + cvShowImage("quads_neighbors", (IplImage*)dbg3_img); + cvWaitKey(); +#endif // allocate extra for adding in icvOrderFoundQuads cvFree(&quad_group); @@ -1498,141 +1542,321 @@ //===================================================================================== -static void icvFindQuadNeighbors( CvCBQuad *quads, int quad_count ) -{ - const float thresh_scale = 1.f; - int idx, i, k, j; - float dx, dy, dist; - - // find quad neighbors - for( idx = 0; idx < quad_count; idx++ ) - { - CvCBQuad* cur_quad = &quads[idx]; - - // choose the points of the current quadrangle that are close to - // some points of the other quadrangles - // (it can happen for split corners (due to dilation) of the - // checker board). Search only in other quadrangles! - - // for each corner of this quadrangle - for( i = 0; i < 4; i++ ) - { - CvPoint2D32f pt; - float min_dist = FLT_MAX; - int closest_corner_idx = -1; - CvCBQuad *closest_quad = 0; - CvCBCorner *closest_corner = 0; +typedef struct quadList { + CvCBQuad *quad; + struct quadList *next; + int length; +} quadList; + +static quadList *newQuadList(CvCBQuad *quad) { + quadList *list = (quadList *)cvAlloc(sizeof(quadList)); + list->quad = quad; + list->next = NULL; + list->length = 1; + return list; +} - if( cur_quad->neighbors[i] ) - continue; +static void deleteQuadList(quadList *list) { + quadList *next; + while (list != NULL) { + next = list->next; + cvFree(&list); + list = next; + } +} - pt = cur_quad->corners[i]->pt; +static void addToQuadList(quadList **list, CvCBQuad *quad) { + quadList *newList = newQuadList(quad); + if (*list != NULL) { + newList->next = *list; + newList->length += (*list)->length; + } + *list = newList; +} - // find the closest corner in all other quadrangles - for( k = 0; k < quad_count; k++ ) - { - if( k == idx ) - continue; +static bool inQuadList(quadList *list, CvCBQuad *quad) { + while (list != NULL) { + if (list->quad == quad) { + return true; + } + list = list->next; + } + return false; +} - for( j = 0; j < 4; j++ ) - { - if( quads[k].neighbors[j] ) - continue; +static double itMean(double oldValue, double newValue, int it) { + // it is zero-based + if (it == 0) { + return newValue; + } + else { + return oldValue + (newValue - oldValue) / (it + 1); + } +} - dx = pt.x - quads[k].corners[j]->pt.x; - dy = pt.y - quads[k].corners[j]->pt.y; - dist = dx * dx + dy * dy; +static double edgeMeanRec(double mean, CvCBQuad *quad, quadList **list) { + if (inQuadList(*list, quad)) { + return mean; + } - if( dist < min_dist && - dist <= cur_quad->edge_len*thresh_scale && - dist <= quads[k].edge_len*thresh_scale ) - { - // check edge lengths, make sure they're compatible - // edges that are different by more than 1:4 are rejected - float ediff = cur_quad->edge_len - quads[k].edge_len; - if (ediff > 32*cur_quad->edge_len || - ediff > 32*quads[k].edge_len) - { - PRINTF("Incompatible edge lengths\n"); - continue; - } - closest_corner_idx = j; - closest_quad = &quads[k]; - min_dist = dist; - } - } - } + int i; + double dx, dy; - // we found a matching corner point? - if( closest_corner_idx >= 0 && min_dist < FLT_MAX ) - { - // If another point from our current quad is closer to the found corner - // than the current one, then we don't count this one after all. - // This is necessary to support small squares where otherwise the wrong - // corner will get matched to closest_quad; - closest_corner = closest_quad->corners[closest_corner_idx]; + int length = *list == NULL ? 0 : (*list)->length; + for (i = 0; i < 4; i++) { + dx = quad->corners[i]->pt.x - quad->corners[(i+1)%4]->pt.x; + dy = quad->corners[i]->pt.y - quad->corners[(i+1)%4]->pt.y; + mean = itMean(mean, dx * dx + dy * dy, 4 * length + i); + } + addToQuadList(list, quad); + + for (i = 0; i < 4; i++) { + if (quad->neighbors[i] != NULL) { + mean = edgeMeanRec(mean, quad->neighbors[i], list); + } + } + + return mean; +} - for( j = 0; j < 4; j++ ) - { - if( cur_quad->neighbors[j] == closest_quad ) - break; +static double edgeMean(CvCBQuad *cur_quad, CvCBQuad *other_quad) { + double mean = 0.0; + quadList *list = NULL; + mean = edgeMeanRec(mean, cur_quad, &list); + mean = edgeMeanRec(mean, other_quad, &list); + deleteQuadList(list); + return mean; +} - dx = closest_corner->pt.x - cur_quad->corners[j]->pt.x; - dy = closest_corner->pt.y - cur_quad->corners[j]->pt.y; +static int minNumerOfEdgesBetweenCorners(CvCBQuad *cur_quad, int cur_corner, CvCBCorner *target_corner, int max_limit) { + if (cur_quad->corners[cur_corner] == target_corner) { + return 0; + } + + if (max_limit == 0) { + return INT_MAX - 1; // Infinity, not reachable in zero edges, -1 to not overflow later on + } + + int cur_edges; + int min_edges = max_limit; + + cur_edges = minNumerOfEdgesBetweenCorners(cur_quad, (cur_corner+1)%4, target_corner, max_limit-1) + 1; + if (cur_edges < min_edges) + min_edges = cur_edges; + + cur_edges = minNumerOfEdgesBetweenCorners(cur_quad, (cur_corner-1+4)%4, target_corner, max_limit-1) + 1; + if (cur_edges < min_edges) + min_edges = cur_edges; + + CvCBQuad *neighbor_quad = cur_quad->neighbors[cur_corner]; + if (neighbor_quad != NULL) { + int i; + for (i = 0; i < 4; i++) { + if (neighbor_quad->neighbors[i] == cur_quad) + break; + } + CV_Assert( i < 4 ); + + cur_edges = minNumerOfEdgesBetweenCorners(neighbor_quad, (i+1)%4, target_corner, max_limit-1) + 1; + if (cur_edges < min_edges) + min_edges = cur_edges; + + cur_edges = minNumerOfEdgesBetweenCorners(neighbor_quad, (i-1+4)%4, target_corner, max_limit-1) + 1; + if (cur_edges < min_edges) + min_edges = cur_edges; + } + + return min_edges; +} - if( dx * dx + dy * dy < min_dist ) - break; - } +static void icvFindQuadNeighbors( CvCBQuad *quads, int quad_count, cv::Ptr img ) +{ + const double max_thresh_scale = 2.0; // Ratio of max diagonal in a x 2a quad, squared (max for all quads is 4.0, 2.0^2) + const double thresh_scale_step = 0.1; + double thresh_scale; + int idx, i, k, j; + float dx, dy, dist; + bool quad_added; + int check_other_closest; - if( j < 4 || cur_quad->count >= 4 || closest_quad->count >= 4 ) - continue; +#ifdef DEBUG_CHESSBOARD + cv::Ptr dbg_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3 ); +#endif - // Check that each corner is a neighbor of different quads - for( j = 0; j < closest_quad->count; j++ ) + check_other_closest = 1; + do { + for (thresh_scale = thresh_scale_step; thresh_scale <= max_thresh_scale; thresh_scale += thresh_scale_step) { + do { + quad_added = false; + + // find quad neighbors + for( idx = 0; idx < quad_count; idx++ ) { - if( closest_quad->neighbors[j] == cur_quad ) - break; - } - if( j < closest_quad->count ) - continue; - - // check whether the closest corner to closest_corner - // is different from cur_quad->corners[i]->pt - for( k = 0; k < quad_count; k++ ) - { - CvCBQuad* q = &quads[k]; - if( k == idx || q == closest_quad ) - continue; - - for( j = 0; j < 4; j++ ) - if( !q->neighbors[j] ) + CvCBQuad* cur_quad = &quads[idx]; + + // choose the points of the current quadrangle that are close to + // some points of the other quadrangles + // (it can happen for split corners (due to dilation) of the + // checker board). Search only in other quadrangles! + + // for each corner of this quadrangle + for( i = 0; i < 4; i++ ) + { + CvPoint2D32f pt; + float min_dist = FLT_MAX; + int closest_corner_idx = -1; + CvCBQuad *closest_quad = 0; + CvCBCorner *closest_corner = 0; + + if( cur_quad->neighbors[i] ) + continue; + + pt = cur_quad->corners[i]->pt; + + // find the closest corner in all other quadrangles + for( k = 0; k < quad_count; k++ ) { - dx = closest_corner->pt.x - q->corners[j]->pt.x; - dy = closest_corner->pt.y - q->corners[j]->pt.y; - dist = dx*dx + dy*dy; - if( dist < min_dist ) - break; + if( k == idx ) + continue; + + for( j = 0; j < 4; j++ ) + { + if( quads[k].neighbors[j] ) + continue; + + dx = pt.x - quads[k].corners[j]->pt.x; + dy = pt.y - quads[k].corners[j]->pt.y; + dist = dx * dx + dy * dy; + + if( dist < min_dist && + dist <= edgeMean(cur_quad, &(quads[k]))*thresh_scale ) + { + // check edge lengths, make sure they're compatible + // edges that are different by more than 1:4 are rejected + float ediff = cur_quad->edge_len - quads[k].edge_len; + if (ediff > 32*cur_quad->edge_len || + ediff > 32*quads[k].edge_len) + { + PRINTF("Incompatible edge lengths\n"); + continue; + } + closest_corner_idx = j; + closest_quad = &quads[k]; + min_dist = dist; + } + } } - if( j < 4 ) - break; + + // we found a matching corner point? + if( closest_corner_idx >= 0 && min_dist < FLT_MAX ) + { + // If another point from our current quad is closer to the found corner + // than the current one, then we don't count this one after all. + // This is necessary to support small squares where otherwise the wrong + // corner will get matched to closest_quad; + closest_corner = closest_quad->corners[closest_corner_idx]; + + #ifdef DEBUG_CHESSBOARD + #ifdef DEBUG_CHESSBOARD_EVERY_CORNER + cvCopy(img,dbg_img); + cvNamedWindow("closest_corner", 1); + cvCircle(dbg_img, cvPointFrom32f(cur_quad->corners[i]->pt), 4, CV_RGB(0,255,0), 1, 8, 0); + cvCircle(dbg_img, cvPointFrom32f(closest_corner->pt), 4, CV_RGB(255,0,0), 1, 8, 0); + cvShowImage("closest_corner", (IplImage*)dbg_img); + cvWaitKey(); + #endif + #endif + + for( j = 0; j < 4; j++ ) + { + if( cur_quad->neighbors[j] == closest_quad ) + break; + + dx = closest_corner->pt.x - cur_quad->corners[j]->pt.x; + dy = closest_corner->pt.y - cur_quad->corners[j]->pt.y; + + if( dx * dx + dy * dy < min_dist ) + break; + } + + if( j < 4 || cur_quad->count >= 4 || closest_quad->count >= 4 ) { + PRINTF("Another corner of the current quad is closer.\n"); + continue; + } + + // Check that each corner is a neighbor of different quads + for( j = 0; j < closest_quad->count; j++ ) + { + if( closest_quad->neighbors[j] == cur_quad ) + break; + } + if( j < closest_quad->count ) { + PRINTF("Closest quad has already for neighbor current quad.\n"); + continue; + } + + // Check if merging corners will make a triangle + // This happens if two edges from opposite quads are merged together, then space (white quad) between + // them becomes a triangle + // As we increase thresh_scale incrementally we can be sure that better (closer) corners were already + // merged if this was possible, so this one is probably not the right one to merge + if (minNumerOfEdgesBetweenCorners(cur_quad, i, closest_corner, 4) < 4) { + PRINTF("Merging corners would make a triangle.\n"); + continue; + } + + // First time we check if there is some other quad which is closer to selected closest corner + // Second time all useful corners in other quads are already used so we can use selected closest corner + // even if there is some possible (but obviously not really useful) other quad closer + if (check_other_closest > 0) { + // check whether the closest corner to closest_corner + // is different from cur_quad->corners[i]->pt + for( k = 0; k < quad_count; k++ ) + { + CvCBQuad* q = &quads[k]; + if( k == idx || q == closest_quad ) + continue; + + for( j = 0; j < 4; j++ ) + if( !q->neighbors[j] ) + { + dx = closest_corner->pt.x - q->corners[j]->pt.x; + dy = closest_corner->pt.y - q->corners[j]->pt.y; + dist = dx*dx + dy*dy; + if( dist < min_dist ) + break; + } + if( j < 4 ) + break; + } + + if( k < quad_count ) { + PRINTF("Another corner is closer than our closest one.\n"); + continue; + } + } + + PRINTF("Found closest corner.\n"); + + closest_corner->pt.x = (pt.x + closest_corner->pt.x) * 0.5f; + closest_corner->pt.y = (pt.y + closest_corner->pt.y) * 0.5f; + + // We've found one more corner - remember it + cur_quad->count++; + cur_quad->neighbors[i] = closest_quad; + cur_quad->corners[i] = closest_corner; + + closest_quad->count++; + closest_quad->neighbors[closest_corner_idx] = cur_quad; + + quad_added = true; + } + } } - - if( k < quad_count ) - continue; - - closest_corner->pt.x = (pt.x + closest_corner->pt.x) * 0.5f; - closest_corner->pt.y = (pt.y + closest_corner->pt.y) * 0.5f; - - // We've found one more corner - remember it - cur_quad->count++; - cur_quad->neighbors[i] = closest_quad; - cur_quad->corners[i] = closest_corner; - - closest_quad->count++; - closest_quad->neighbors[closest_corner_idx] = cur_quad; - } + } while (quad_added); } - } + check_other_closest--; + } while (check_other_closest >= 0); } //=====================================================================================