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