bool CvCalibFilter::FindEtalon( CvMat** mats )
{
bool result = true;
if( !mats || etalonPointCount == 0 )
{
assert(0);
result = false;
}
if( result )
{
int i, tempPointCount0 = etalonPointCount*2;
for( i = 0; i < cameraCount; i++ )
{
if( !latestPoints[i] )
latestPoints[i] = (CvPoint2D32f*)
cvAlloc( tempPointCount0*2*sizeof(latestPoints[0]));
}
for( i = 0; i < cameraCount; i++ )
{
CvSize size;
int tempPointCount = tempPointCount0;
bool found = false;
if( !CV_IS_MAT(mats[i]) && !CV_IS_IMAGE(mats[i]))
{
assert(0);
break;
}
size = cvGetSize(mats[i]);
if( size.width != imgSize.width || size.height != imgSize.height )
{
imgSize = size;
}
if( !grayImg || grayImg->width != imgSize.width ||
grayImg->height != imgSize.height )
{
cvReleaseMat( &grayImg );
cvReleaseMat( &tempImg );
grayImg = cvCreateMat( imgSize.height, imgSize.width, CV_8UC1 );
tempImg = cvCreateMat( imgSize.height, imgSize.width, CV_8UC1 );
}
if( !storage )
storage = cvCreateMemStorage();
switch( etalonType )
{
case CV_CALIB_ETALON_CHESSBOARD:
if( CV_MAT_CN(cvGetElemType(mats[i])) == 1 )
cvCopy( mats[i], grayImg );
else
cvCvtColor( mats[i], grayImg, CV_BGR2GRAY );
found = cvFindChessBoardCornerGuesses( grayImg, tempImg, storage,
cvSize( cvRound(etalonParams[0]),
cvRound(etalonParams[1])),
latestPoints[i], &tempPointCount ) != 0;
if( found )
cvFindCornerSubPix( grayImg, latestPoints[i], tempPointCount,
cvSize(5,5), cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,10,0.1));
break;
default:
assert(0);
result = false;
break;
}
latestCounts[i] = found ? tempPointCount : -tempPointCount;
result = result && found;
}
}
if( storage )
cvClearMemStorage( storage );
return result;
}
//
// Function: FindCorners
// Purpose : This function finds the corner points in the calibration images
// using opencv functions cvFindChessBoardCornerGuesses and
// cvFindCornerSubPix. After image pixel coordinates are found, their
// respective world coordinates are assigned. (z coordinate is always
// zero). When the function cannot find the specified number of
// coordinates in the image, the image is discarded because of the
// added complexity in assigning their world coordinates.
// Output : uveff : coordinates of the chessborder in image plane.
// XYZeff: coordinates of uveff w.r.t World Coordinate System
// m_effective_image_no : number of images processed.
//----------------------------------------------------------------------------
void camcal::FindCorners()
{
IplImage* img = 0;
IplImage* img0 = 0;
IplImage* img1 = 0;
IplImage* greyimg = 0;
CvFont dfont;
cvInitFont (&dfont, CV_FONT_VECTOR0, 0.3, 0.3, 0.0f, 1);
CvPoint onecorner;
int numcorners = m_corner_no;
CvPoint2D64d* uv = new CvPoint2D64d[m_image_number * m_corner_no];
CvPoint3D64d* XYZ = new CvPoint3D64d[m_image_number * m_corner_no];
CvPoint2D32f* corners = new CvPoint2D32f[m_corner_no];
CvMemStorage* storage = 0;
m_effective_image_no=-1;
for( int imgnum=0; imgnum<m_image_number; imgnum++ )
{
numcorners = m_corner_no;
img = m_input_images[imgnum];
imgsize.width = img->width;
imgsize.height= img->height;
img0 = cvCloneImage(img);
img1 = cvCloneImage(img);
greyimg = cvCreateImageHeader(imgsize,IPL_DEPTH_8U,1);
cvCreateImageData(greyimg);
cvCvtColor(img, greyimg, CV_RGB2GRAY);
img0 = cvCloneImage(greyimg);
cvFindChessBoardCornerGuesses(greyimg,
img0,
storage,
cvSize(m_x_height,m_x_width),
corners,
&numcorners);
if( numcorners != m_corner_no ) {
cvReleaseImage( &img0 );
cvReleaseImage( &img1 );
cvReleaseImage( &greyimg );
continue;
}
else
m_effective_image_no++;
// draw a circle at each corner found
for( int t = 0; t < numcorners; t++ )
{
onecorner.x = (int)corners[t].x;
onecorner.y = (int)corners[t].y;
cvCircle(img1, onecorner, 8, CV_RGB(0,255,0),2);
// image, center, radius, color, thickness
}
// Find sub-corners
cvFindCornerSubPix(greyimg,
corners,
numcorners,
cvSize (m_x_height,m_x_width),
cvSize(-1, -1),
cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 10, 0.1));
// correct order
if( m_apply_ordering ){
CvPoint3D32f init = FindRectangleCorner( corners, numcorners );
if( init.z < m_x_height*m_x_width )
SortPoints(corners, numcorners, &init);
else
{
cout<<"Sort Error";
cvReleaseImage( &img0 );
cvReleaseImage( &img1 );
cvReleaseImage( &greyimg );
m_effective_image_no--;
continue;
}
}
//draw a circle and put the corner number at each subcorner found
for( int t = 0; t < numcorners; t++ )
{
onecorner.x = (int)corners[t].x;
onecorner.y = (int)corners[t].y;
cvCircle(img1, onecorner, 3, CV_RGB(255,0,0),1);
char buf[10];
sprintf( buf, "%d", t );
// cvPutText(img1,numbers[t], cvPoint(onecorner.x, onecorner.y + 20), &dfont, CV_RGB(255,0,0));
cvPutText(img1, buf, cvPoint(onecorner.x, onecorner.y + 20), &dfont, CV_RGB(255,0,0));
}
// CAMERA CALIBRATION PART
for( int currPoint=0; currPoint < numcorners; currPoint++ )
{
uv[ m_effective_image_no*numcorners + currPoint].x = corners[currPoint].x;
uv[ m_effective_image_no*numcorners + currPoint].y = corners[currPoint].y;
}
int index;
for( int i = 0; i < m_x_width; i++ )
{
for( int j = 0; j < m_x_height; j++ )
{
index = m_effective_image_no*numcorners + i*m_x_height+j;
XYZ[ index ].x = m_grid_width *(m_x_width -i);
XYZ[ index ].y = m_grid_height*(m_x_height-j);
XYZ[ index ].z = 0;
}
}
if( m_display_corners )
{
cvvNamedWindow( "image", 1 );
cvvShowImage("image",img1);
cvvWaitKey(0);
cvDestroyWindow( "image" );
}
// cvReleaseImage( &img );
cvReleaseImage( &img0 );
cvReleaseImage( &img1 );
cvReleaseImage( &greyimg );
} //loop to next image
free (corners);
m_effective_image_no++;
delete []uveff ; uveff =NULL;
delete []XYZeff; XYZeff = NULL;
if( m_image_number == m_effective_image_no ){
uveff = uv; uv = NULL;
XYZeff = XYZ; XYZ = NULL;
}
else
{
int size = m_effective_image_no * m_corner_no;
uveff = new CvPoint2D64d[size];
XYZeff = new CvPoint3D64d[size];
for(int ph=0; ph<size; ph++)
{
uveff [ph] = uv [ph];
XYZeff[ph] = XYZ[ph];
}
delete []uv; uv = NULL;
delete []XYZ; XYZ = NULL;
}
}
/////////////////////////////////
// cv3dTrackerCalibrateCameras //
/////////////////////////////////
CV_IMPL CvBool cv3dTrackerCalibrateCameras(int num_cameras,
const Cv3dTrackerCameraIntrinsics camera_intrinsics[], // size is num_cameras
CvSize etalon_size,
float square_size,
IplImage *samples[], // size is num_cameras
Cv3dTrackerCameraInfo camera_info[]) // size is num_cameras
{
CV_FUNCNAME("cv3dTrackerCalibrateCameras");
const int num_points = etalon_size.width * etalon_size.height;
int cameras_done = 0; // the number of cameras whose positions have been determined
CvPoint3D32f *object_points = NULL; // real-world coordinates of checkerboard points
CvPoint2D32f *points = NULL; // 2d coordinates of checkerboard points as seen by a camera
IplImage *gray_img = NULL; // temporary image for color conversion
IplImage *tmp_img = NULL; // temporary image used by FindChessboardCornerGuesses
int c, i, j;
if (etalon_size.width < 3 || etalon_size.height < 3)
CV_ERROR(CV_StsBadArg, "Chess board size is invalid");
for (c = 0; c < num_cameras; c++)
{
// CV_CHECK_IMAGE is not available in the cvaux library
// so perform the checks inline.
//CV_CALL(CV_CHECK_IMAGE(samples[c]));
if( samples[c] == NULL )
CV_ERROR( CV_HeaderIsNull, "Null image" );
if( samples[c]->dataOrder != IPL_DATA_ORDER_PIXEL && samples[c]->nChannels > 1 )
CV_ERROR( CV_BadOrder, "Unsupported image format" );
if( samples[c]->maskROI != 0 || samples[c]->tileInfo != 0 )
CV_ERROR( CV_StsBadArg, "Unsupported image format" );
if( samples[c]->imageData == 0 )
CV_ERROR( CV_BadDataPtr, "Null image data" );
if( samples[c]->roi &&
((samples[c]->roi->xOffset | samples[c]->roi->yOffset
| samples[c]->roi->width | samples[c]->roi->height) < 0 ||
samples[c]->roi->xOffset + samples[c]->roi->width > samples[c]->width ||
samples[c]->roi->yOffset + samples[c]->roi->height > samples[c]->height ||
(unsigned) (samples[c]->roi->coi) > (unsigned) (samples[c]->nChannels)))
CV_ERROR( CV_BadROISize, "Invalid ROI" );
// End of CV_CHECK_IMAGE inline expansion
if (samples[c]->depth != IPL_DEPTH_8U)
CV_ERROR(CV_BadDepth, "Channel depth of source image must be 8");
if (samples[c]->nChannels != 3 && samples[c]->nChannels != 1)
CV_ERROR(CV_BadNumChannels, "Source image must have 1 or 3 channels");
}
CV_CALL(object_points = (CvPoint3D32f *)cvAlloc(num_points * sizeof(CvPoint3D32f)));
CV_CALL(points = (CvPoint2D32f *)cvAlloc(num_points * sizeof(CvPoint2D32f)));
// fill in the real-world coordinates of the checkerboard points
FillObjectPoints(object_points, etalon_size, square_size);
for (c = 0; c < num_cameras; c++)
{
CvSize image_size = cvSize(samples[c]->width, samples[c]->height);
IplImage *img;
// The input samples are not required to all have the same size or color
// format. If they have different sizes, the temporary images are
// reallocated as necessary.
if (samples[c]->nChannels == 3)
{
// convert to gray
if (gray_img == NULL || gray_img->width != samples[c]->width ||
gray_img->height != samples[c]->height )
{
if (gray_img != NULL)
cvReleaseImage(&gray_img);
CV_CALL(gray_img = cvCreateImage(image_size, IPL_DEPTH_8U, 1));
}
CV_CALL(cvCvtColor(samples[c], gray_img, CV_BGR2GRAY));
img = gray_img;
}
else
{
// no color conversion required
img = samples[c];
}
if (tmp_img == NULL || tmp_img->width != samples[c]->width ||
tmp_img->height != samples[c]->height )
{
if (tmp_img != NULL)
cvReleaseImage(&tmp_img);
CV_CALL(tmp_img = cvCreateImage(image_size, IPL_DEPTH_8U, 1));
}
int count = num_points;
bool found = cvFindChessBoardCornerGuesses(img, tmp_img, 0,
etalon_size, points, &count) != 0;
if (count == 0)
continue;
// If found is true, it means all the points were found (count = num_points).
// If found is false but count is non-zero, it means that not all points were found.
cvFindCornerSubPix(img, points, count, cvSize(5,5), cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 10, 0.01f));
// If the image origin is BL (bottom-left), fix the y coordinates
// so they are relative to the true top of the image.
if (samples[c]->origin == IPL_ORIGIN_BL)
{
for (i = 0; i < count; i++)
points[i].y = samples[c]->height - 1 - points[i].y;
}
if (found)
{
// Make sure x coordinates are increasing and y coordinates are decreasing.
// (The y coordinate of point (0,0) should be the greatest, because the point
// on the checkerboard that is the origin is nearest the bottom of the image.)
// This is done after adjusting the y coordinates according to the image origin.
if (points[0].x > points[1].x)
{
// reverse points in each row
for (j = 0; j < etalon_size.height; j++)
{
CvPoint2D32f *row = &points[j*etalon_size.width];
for (i = 0; i < etalon_size.width/2; i++)
std::swap(row[i], row[etalon_size.width-i-1]);
}
}
if (points[0].y < points[etalon_size.width].y)
{
// reverse points in each column
for (i = 0; i < etalon_size.width; i++)
{
for (j = 0; j < etalon_size.height/2; j++)
std::swap(points[i+j*etalon_size.width],
points[i+(etalon_size.height-j-1)*etalon_size.width]);
}
}
}
DrawEtalon(samples[c], points, count, etalon_size, found);
if (!found)
continue;
float rotVect[3];
float rotMatr[9];
float transVect[3];
cvFindExtrinsicCameraParams(count,
image_size,
points,
object_points,
const_cast<float *>(camera_intrinsics[c].focal_length),
camera_intrinsics[c].principal_point,
const_cast<float *>(camera_intrinsics[c].distortion),
rotVect,
transVect);
// Check result against an arbitrary limit to eliminate impossible values.
// (If the chess board were truly that far away, the camera wouldn't be able to
// see the squares.)
if (transVect[0] > 1000*square_size
|| transVect[1] > 1000*square_size
|| transVect[2] > 1000*square_size)
{
// ignore impossible results
continue;
}
CvMat rotMatrDescr = cvMat(3, 3, CV_32FC1, rotMatr);
CvMat rotVectDescr = cvMat(3, 1, CV_32FC1, rotVect);
/* Calc rotation matrix by Rodrigues Transform */
cvRodrigues2( &rotVectDescr, &rotMatrDescr );
//combine the two transformations into one matrix
//order is important! rotations are not commutative
float tmat[4][4] = { { 1.f, 0.f, 0.f, 0.f },
{ 0.f, 1.f, 0.f, 0.f },
{ 0.f, 0.f, 1.f, 0.f },
{ transVect[0], transVect[1], transVect[2], 1.f } };
float rmat[4][4] = { { rotMatr[0], rotMatr[1], rotMatr[2], 0.f },
{ rotMatr[3], rotMatr[4], rotMatr[5], 0.f },
{ rotMatr[6], rotMatr[7], rotMatr[8], 0.f },
{ 0.f, 0.f, 0.f, 1.f } };
MultMatrix(camera_info[c].mat, tmat, rmat);
// change the transformation of the cameras to put them in the world coordinate
// system we want to work with.
// Start with an identity matrix; then fill in the values to accomplish
// the desired transformation.
float smat[4][4] = { { 1.f, 0.f, 0.f, 0.f },
{ 0.f, 1.f, 0.f, 0.f },
{ 0.f, 0.f, 1.f, 0.f },
{ 0.f, 0.f, 0.f, 1.f } };
// First, reflect through the origin by inverting all three axes.
smat[0][0] = -1.f;
smat[1][1] = -1.f;
smat[2][2] = -1.f;
MultMatrix(tmat, camera_info[c].mat, smat);
// Scale x and y coordinates by the focal length (allowing for non-square pixels
// and/or non-symmetrical lenses).
smat[0][0] = 1.0f / camera_intrinsics[c].focal_length[0];
smat[1][1] = 1.0f / camera_intrinsics[c].focal_length[1];
smat[2][2] = 1.0f;
MultMatrix(camera_info[c].mat, smat, tmat);
camera_info[c].principal_point = camera_intrinsics[c].principal_point;
camera_info[c].valid = true;
cameras_done++;
}
exit:
cvReleaseImage(&gray_img);
cvReleaseImage(&tmp_img);
cvFree(&object_points);
cvFree(&points);
return cameras_done == num_cameras;
}
/* ///////////////////// chess_corner_test ///////////////////////// */
void CV_ChessboardDetectorTest::run( int start_from )
{
int code = CvTS::OK;
#ifndef WRITE_POINTS
const double rough_success_error_level = 2.5;
const double precise_success_error_level = 0.2;
double err = 0, max_rough_error = 0, max_precise_error = 0;
#endif
/* test parameters */
char filepath[1000];
char filename[1000];
CvMat* _u = 0;
CvMat* _v = 0;
CvPoint2D32f* u;
CvPoint2D32f* v;
IplImage* img = 0;
IplImage* gray = 0;
IplImage* thresh = 0;
int idx, max_idx;
int progress = 0;
sprintf( filepath, "%scameracalibration/", ts->get_data_path() );
sprintf( filename, "%schessboard_list.dat", filepath );
CvFileStorage* fs = cvOpenFileStorage( filename, 0, CV_STORAGE_READ );
CvFileNode* board_list = fs ? cvGetFileNodeByName( fs, 0, "boards" ) : 0;
if( !fs || !board_list || !CV_NODE_IS_SEQ(board_list->tag) ||
board_list->data.seq->total % 2 != 0 )
{
ts->printf( CvTS::LOG, "chessboard_list.dat can not be readed or is not valid" );
code = CvTS::FAIL_MISSING_TEST_DATA;
goto _exit_;
}
max_idx = board_list->data.seq->total/2;
for( idx = start_from; idx < max_idx; idx++ )
{
int etalon_count = -1;
int count = 0;
CvSize etalon_size = { -1, -1 };
int j, result;
ts->update_context( this, idx-1, true );
/* read the image */
sprintf( filename, "%s%s", filepath,
cvReadString((CvFileNode*)cvGetSeqElem(board_list->data.seq,idx*2),"dummy.txt"));
img = cvLoadImage( filename );
if( !img )
{
ts->printf( CvTS::LOG, "one of chessboard images can't be read: %s", filename );
if( max_idx == 1 )
{
code = CvTS::FAIL_MISSING_TEST_DATA;
goto _exit_;
}
continue;
}
gray = cvCreateImage( cvSize( img->width, img->height ), IPL_DEPTH_8U, 1 );
thresh = cvCreateImage( cvSize( img->width, img->height ), IPL_DEPTH_8U, 1 );
cvCvtColor( img, gray, CV_BGR2GRAY );
sprintf( filename, "%s%s", filepath,
cvReadString((CvFileNode*)cvGetSeqElem(board_list->data.seq,idx*2+1),"dummy.txt"));
_u = (CvMat*)cvLoad( filename );
if( _u == 0 )
{
if( idx == 0 )
ts->printf( CvTS::LOG, "one of chessboard corner files can't be read: %s", filename );
if( max_idx == 1 )
{
code = CvTS::FAIL_MISSING_TEST_DATA;
goto _exit_;
}
continue;
}
etalon_size.width = _u->cols;
etalon_size.height = _u->rows;
etalon_count = etalon_size.width*etalon_size.height;
/* allocate additional buffers */
_v = cvCloneMat( _u );
count = etalon_count;
u = (CvPoint2D32f*)_u->data.fl;
v = (CvPoint2D32f*)_v->data.fl;
OPENCV_CALL( result = cvFindChessBoardCornerGuesses(
gray, thresh, 0, etalon_size, v, &count ));
//show_points( gray, 0, etalon_count, v, count, etalon_size, result );
if( !result || count != etalon_count )
{
ts->printf( CvTS::LOG, "chess board is not found" );
code = CvTS::FAIL_INVALID_OUTPUT;
goto _exit_;
}
#ifndef WRITE_POINTS
err = 0;
for( j = 0; j < etalon_count; j++ )
{
double dx = fabs( v[j].x - u[j].x );
double dy = fabs( v[j].y - u[j].y );
dx = MAX( dx, dy );
if( dx > err )
{
err = dx;
if( err > rough_success_error_level )
{
ts->printf( CvTS::LOG, "bad accuracy of corner guesses" );
code = CvTS::FAIL_BAD_ACCURACY;
goto _exit_;
}
}
}
max_rough_error = MAX( max_rough_error, err );
#endif
OPENCV_CALL( cvFindCornerSubPix( gray, v, count, cvSize( 5, 5 ), cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_EPS|CV_TERMCRIT_ITER,30,0.1)));
//show_points( gray, u + 1, etalon_count, v, count );
#ifndef WRITE_POINTS
err = 0;
for( j = 0; j < etalon_count; j++ )
{
double dx = fabs( v[j].x - u[j].x );
double dy = fabs( v[j].y - u[j].y );
dx = MAX( dx, dy );
if( dx > err )
{
err = dx;
if( err > precise_success_error_level )
{
ts->printf( CvTS::LOG, "bad accuracy of adjusted corners" );
code = CvTS::FAIL_BAD_ACCURACY;
goto _exit_;
}
}
}
max_precise_error = MAX( max_precise_error, err );
#else
cvSave( filename, _v );
#endif
cvReleaseMat( &_u );
cvReleaseMat( &_v );
cvReleaseImage( &img );
cvReleaseImage( &gray );
cvReleaseImage( &thresh );
progress = update_progress( progress, idx-1, max_idx, 0 );
}
_exit_:
/* release occupied memory */
cvReleaseMat( &_u );
cvReleaseMat( &_v );
cvReleaseFileStorage( &fs );
cvReleaseImage( &img );
cvReleaseImage( &gray );
cvReleaseImage( &thresh );
if( code < 0 )
ts->set_failed_test_info( code );
}
