bool CalibrationFilter::calibrateCamera()
{
int numImages = captures.size();
// int numExpectedPoints = targetCorners.nx*targetCorners.ny;
// int numPoints = numImages*numExpectedPoints;
CvPoint2D32f* screenPoints = new CvPoint2D32f[numPoints];
for( int i=0; i<numPoints; ++i ) {
screenPoints[i].x = found_points[i].x;
screenPoints[i].y = found_points[i].y;
}
CvPoint3D32f* worldPoints = new CvPoint3D32f[numPoints];
for( int i=0; i<numPoints; ++i ) {
screenPoints[i].x = calibObject.x;
screenPoints[i].y = calibObject.y;
screenPoints[i].z = calibObject.y;
}
CvVect32f translation_vectors = NULL;
CvMatr32f rotation_matrices = NULL;
if (settings.calc_obj_pos)
{
translation_vectors = new float[3*numImages];
rotation_matrices = new float[9*numImages];
}
cvCalibrateCamera2(screenPoints, //Pointer 2D points in screen space.
worldPoints, //Pointer 3D points in real space
numPoints,
cvSize(videoSize.x, videoSize.y), //Size of the image.
camera.distCoeffs, //output: 4 distortion coefficients
camera.intrinsics, //output: intrinsic camera matrix
translation_vectors, //output: Array of translations vectors
rotation_matrices, //output: Array of rotation matrices
settings.guess_intrinsics); //intrisic guess needed
delete numFoundPoints;
delete screenPoints;
delete worldPoints;
if (settings.calc_obj_pos)
{
delete translation_vectors;
delete rotation_matrices;
}
if (settings.do_interpolate != camera.was_interpolated)
{
delete camera.undistMap;
int mult = settings.do_interpolate? 3 : 1;
camera.undistMap = cvCreateImage(cvSize(mult*videoSize.x, mult*videoSize.y), CV_32S, 1);
}
cvUnDistortInit(input.getCvImage(),
camera.undistMap, //undistortion map (calc once, use many times)
camera.intrinsics, //intrinsic camera matrix
camera.distCoeffs, //4 distortion coefficients
settings.do_interpolate); //
camera.was_interpolated = settings.do_interpolate;
}
//--------------------------------------------------- Test body --------------------------
static int fmaUnDistort( void )
{
int i, itest, io=0, num_test, err1=0, err2=0, err3=0, err4=0,
err10=0, err20=0, err30=0, err40=0, err=0, err5, pass=1;
int n, n3, step, step3;
int* data;
IplImage* undistMap = 0;
uchar *srcImg, *dstImg, *tstImg, *srcImg3, *dstImg3, *tstImg3;
IplImage *src, *dst, *tst, *src3, *dst3, *tst3;
float *a, *k, p = 0.f;
AtsRandState state;
CvSize size;
double norm, norm1;
/* Reading test parameters */
trsiRead( &img_width, "320", "width of image" );
trsiRead( &img_height, "240", "height of image" );
trsiRead( &roi_step, "0", "ROI step" );
n = img_width * img_height;
size.height = img_height; size.width = img_width;
a = (float*)cvAlloc( sizeof(float) * 9 );
k = (float*)cvAlloc( sizeof(float) * 4 );
//data = (int*) icvAlloc( 3*n*sizeof(int) );
src = cvCreateImage( size, IPL_DEPTH_8U, 1 );
cvSetImageROI( src, cvRect(0, 0, src->width, src->height) );
dst = cvCreateImage( size, IPL_DEPTH_8U, 1 );
cvSetImageROI( dst, cvRect(0, 0, dst->width, dst->height) );
tst = cvCreateImage( size, IPL_DEPTH_8U, 1 );
cvSetImageROI( tst, cvRect(0, 0, tst->width, tst->height) );
src3 = cvCreateImage( size, IPL_DEPTH_8U, 3 );
cvSetImageROI( src3, cvRect(0, 0, src3->width, src3->height) );
dst3 = cvCreateImage( size, IPL_DEPTH_8U, 3 );
cvSetImageROI( dst3, cvRect(0, 0, dst3->width, dst3->height) );
tst3 = cvCreateImage( size, IPL_DEPTH_8U, 3 );
cvSetImageROI( tst3, cvRect(0, 0, tst3->width, tst3->height) );
undistMap = cvCreateImage( size, IPL_DEPTH_32S, 3 );
srcImg = (uchar*)src->imageData;
dstImg = (uchar*)dst->imageData;
tstImg = (uchar*)tst->imageData;
srcImg3 = (uchar*)src3->imageData;
dstImg3 = (uchar*)dst3->imageData;
tstImg3 = (uchar*)tst3->imageData;
data = (int*)undistMap->imageData;
step = src->widthStep; step3 = src3->widthStep;
n = step*img_height; n3 = step3*img_height;
atsRandInit( &state, 0, 255, 13 );
atsbRand8u ( &state, srcImg, n );
atsbRand8u ( &state, srcImg3, n3 );
a[0] = img_width/3.f;
a[4] = img_height/2.f;
a[2] = img_width/2.f;
a[5] = img_height/2.f;
k[0] = -0.04f;
k[1] = 0.004f;
k[2] = 0.f;
k[3] = 0.f;
if(roi_step)
{
num_test = (img_width/2 - 3)/roi_step;
if( num_test > (img_height/2)/roi_step ) num_test = (img_height/2)/roi_step;
}
else num_test = 1;
if( num_test < 1 ) num_test = 1;
begin:
trsWrite(TW_RUN|TW_CON, "\n %d pass of 4 :\n", pass);
for(itest=0; itest<num_test; itest++)
{
int ii = (itest*10)/num_test;
int roi_offset = roi_step*itest;
int img_offset = roi_offset*(step + 1);
int img_offset3 = roi_offset*(step3 + 3);
size.width = img_width - 2*roi_offset; size.height = img_height - 2*roi_offset;
src->roi->xOffset = src->roi->yOffset = roi_offset;
src->roi->height = size.height; src->roi->width = size.width;
dst->roi->xOffset = dst->roi->yOffset = roi_offset;
dst->roi->height = size.height; dst->roi->width = size.width;
tst->roi->xOffset = tst->roi->yOffset = roi_offset;
tst->roi->height = size.height; tst->roi->width = size.width;
src3->roi->xOffset = src3->roi->yOffset = roi_offset;
src3->roi->height = size.height; src3->roi->width = size.width;
dst3->roi->xOffset = dst3->roi->yOffset = roi_offset;
dst3->roi->height = size.height; dst3->roi->width = size.width;
tst3->roi->xOffset = tst3->roi->yOffset = roi_offset;
tst3->roi->height = size.height; tst3->roi->width = size.width;
/* 8uC1 flavor test without interpolation */
for(i=0; i<n; i++) dstImg[i] = tstImg[i] = 0;
cvUnDistortInit ( src, undistMap, a, k, 0 );
cvUnDistort ( src, dst, undistMap, 0 );
UnDistortTest_C1( srcImg + img_offset, tstImg + img_offset, step, size, a, k, 0 );
if( !img_offset )
{
norm = norm1 = 0.0;
for(i=0; i<n; i++)
{
norm += fabs( dstImg[i] - tstImg[i] );
norm1 += fabs( tstImg[i] );
}
norm /= norm1;
printf( " 8u C1 without interpolation: %g\n", norm );
}
for(i=0; i<n; i++)
{
int d = dstImg[i] - tstImg[i];
if( d > MAXDIFF || d < -MAXDIFF ) err1++;
}
for(i=0; i<n; i++) dstImg[i] = 0;
cvUnDistortOnce ( src, dst, a, k, 0 );
//for(i=0; i<n; i++)printf(" %d", dstImg[i]); getchar();
if( !img_offset )
{
norm = norm1 = 0.0;
for(i=0; i<n; i++)
{
norm += fabs( dstImg[i] - tstImg[i] );
norm1 += fabs( tstImg[i] );
}
norm /= norm1;
printf( " %g\n", norm );
}
for(i=0; i<n; i++)
{
int d = dstImg[i] - tstImg[i];
if( d > MAXDIFF || d < -MAXDIFF ) err10++;
}
/* 8uC1 flavor test with interpolation */
for(i=0; i<n; i++) dstImg[i] = tstImg[i] = 0;
cvUnDistortInit ( src, undistMap, a, k, 1 );
cvUnDistort ( src, dst, undistMap, 1 );
UnDistortTest_C1( srcImg + img_offset, tstImg + img_offset, step, size, a, k, 1 );
if( !img_offset )
{
norm = norm1 = 0.0;
for(i=0; i<n; i++)
{
norm += fabs( dstImg[i] - tstImg[i] );
norm1 += fabs( tstImg[i] );
}
norm /= norm1;
printf( " 8u C1 with interpolation: %g\n", norm );
}
for(i=0; i<n; i++)
{
int d = dstImg[i] - tstImg[i];
if( d > MAXDIFF || d < -MAXDIFF ) err2++;
}
for(i=0; i<n; i++) dstImg[i] = 0;
cvUnDistortOnce ( src, dst, a, k, 1 );
if( !img_offset )
{
norm = norm1 = 0.0;
for(i=0; i<n; i++)
{
norm += fabs( dstImg[i] - tstImg[i] );
norm1 += fabs( tstImg[i] );
}
norm /= norm1;
printf( " %g\n", norm );
}
for(i=0; i<n; i++)
{
int d = dstImg[i] - tstImg[i];
if( d > MAXDIFF || d < -MAXDIFF ) err20++;
}
/* 8uC3 flavor test without interpolation */
for(i=0; i<n3; i++) dstImg3[i] = tstImg3[i] = 0;
cvUnDistortInit ( src3, undistMap, a, k, 0 );
cvUnDistort ( src3, dst3, undistMap, 0 );
UnDistortTest_C3( srcImg3+img_offset3, tstImg3+img_offset3, step3, size, a, k, 0 );
for(i=0; i<n3; i++)
{
int d = dstImg3[i] - tstImg3[i];
if( d > MAXDIFF || d < -MAXDIFF ) err3++;
}
if( !img_offset )
{
norm = norm1 = 0.0;
for(i=0; i<n3; i++)
{
norm += fabs( dstImg3[i] - tstImg3[i] );
norm1 += fabs( tstImg3[i] );
}
norm /= norm1;
printf( " 8u C3 without interpolation: %g\n", norm );
}
for(i=0; i<n3; i++) dstImg3[i] = 0;
cvUnDistortOnce ( src3, dst3, a, k, 0 );
if( !img_offset )
{
norm = norm1 = 0.0;
for(i=0; i<n3; i++)
{
norm += fabs( dstImg3[i] - tstImg3[i] );
norm1 += fabs( tstImg3[i] );
}
norm /= norm1;
printf( " %g\n", norm );
}
for(i=0; i<n3; i++)
{
int d = dstImg3[i] - tstImg3[i];
if( d > MAXDIFF || d < -MAXDIFF ) err30++;
}
/* 8uC3 flavor test with interpolation */
for(i=0; i<n3; i++) dstImg3[i] = tstImg3[i] = 0;
cvUnDistortInit ( src3, undistMap, a, k, 1 );
cvUnDistort ( src3, dst3, undistMap, 1 );
UnDistortTest_C3( srcImg3+img_offset3, tstImg3+img_offset3, step3, size, a, k, 1 );
for(i=0; i<n3; i++)
{
int d = dstImg3[i] - tstImg3[i];
if( d > MAXDIFF || d < -MAXDIFF ) err4++;
}
if( !img_offset )
{
norm = norm1 = 0.0;
for(i=0; i<n3; i++)
{
norm += fabs( dstImg3[i] - tstImg3[i] );
norm1 += fabs( tstImg3[i] );
}
norm /= norm1;
printf( " 8u C3 with interpolation: %g\n", norm );
}
for(i=0; i<n3; i++) dstImg3[i] = 0;
cvUnDistortOnce ( src3, dst3, a, k, 1 );
if( !img_offset )
{
norm = norm1 = 0.0;
for(i=0; i<n3; i++)
{
norm += fabs( dstImg3[i] - tstImg3[i] );
norm1 += fabs( tstImg3[i] );
}
norm /= norm1;
printf( " %g\n", norm );
}
for(i=0; i<n3; i++)
{
int d = dstImg3[i] - tstImg3[i];
if( d > MAXDIFF || d < -MAXDIFF ) err40++;
}
if(ii>io) { trsWrite(TW_RUN|TW_CON, " %d%% ", 10*ii); io=ii; }
}
err5 = err1 + err2 + err3 + err4 + err10 + err20 + err30 + err40;
err += err5;
if( p < err1*100.f / (float)(n*num_test) ) p = err1*100.f / (float)(n*num_test);
if( p < err2*100.f / (float)(n*num_test) ) p = err2*100.f / (float)(n*num_test);
if( p < err3*100.f / (float)(3*n*num_test) ) p = err3*100.f / (float)(3*n*num_test);
if( p < err4*100.f / (float)(3*n*num_test) ) p = err4*100.f / (float)(3*n*num_test);
if( p < err10*100.f / (float)(n*num_test) ) p = err10*100.f / (float)(n*num_test);
if( p < err20*100.f / (float)(n*num_test) ) p = err20*100.f / (float)(n*num_test);
if( p < err30*100.f / (float)(3*n*num_test) ) p = err30*100.f / (float)(3*n*num_test);
if( p < err40*100.f / (float)(3*n*num_test) ) p = err40*100.f / (float)(3*n*num_test);
//printf("\n %d %d %d %d\n %d %d %d %d %7.3f%% errors\n",
// err1, err2, err3, err4, err10, err20, err30, err40, p);
switch( pass )
{
case 1:
k[0] = -k[0];
io = 0;
err1 = err2 = err3 = err4 = err10 = err20 = err30 = err40 = 0;
pass++;
goto begin;
break;
case 2:
k[0] = -k[0];
k[2] = k[3] = 0.02f;
io = 0;
err1 = err2 = err3 = err4 = err10 = err20 = err30 = err40 = 0;
pass++;
goto begin;
break;
case 3:
k[0] = -k[0];
io = 0;
err1 = err2 = err3 = err4 = err10 = err20 = err30 = err40 = 0;
pass++;
goto begin;
break;
}
if( p < MAXPERCENT ) err = 0;
cvReleaseImage( &src );
cvReleaseImage( &dst );
cvReleaseImage( &tst );
cvReleaseImage( &src3 );
cvReleaseImage( &dst3 );
cvReleaseImage( &tst3 );
cvReleaseImage( &undistMap );
cvFree( (void**)&a );
cvFree( (void**)&k );
if( err == 0 ) return trsResult( TRS_OK, "No errors fixed by this test" );
else return trsResult( TRS_FAIL, "Total fixed %d errors", err );
} /*fma*/
