in modules/calib3d/src/calibration.cpp [1229:1562]
CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
const CvMat* imagePoints, const CvMat* npoints,
CvSize imageSize, CvMat* cameraMatrix, CvMat* distCoeffs,
CvMat* rvecs, CvMat* tvecs, int flags, CvTermCriteria termCrit )
{
const int NINTRINSIC = 16;
Ptr<CvMat> matM, _m, _Ji, _Je, _err;
CvLevMarq solver;
double reprojErr = 0;
double A[9], k[12] = {0,0,0,0,0,0,0,0,0,0,0,0};
CvMat matA = cvMat(3, 3, CV_64F, A), _k;
int i, nimages, maxPoints = 0, ni = 0, pos, total = 0, nparams, npstep, cn;
double aspectRatio = 0.;
// 0. check the parameters & allocate buffers
if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(imagePoints) ||
!CV_IS_MAT(npoints) || !CV_IS_MAT(cameraMatrix) || !CV_IS_MAT(distCoeffs) )
CV_Error( CV_StsBadArg, "One of required vector arguments is not a valid matrix" );
if( imageSize.width <= 0 || imageSize.height <= 0 )
CV_Error( CV_StsOutOfRange, "image width and height must be positive" );
if( CV_MAT_TYPE(npoints->type) != CV_32SC1 ||
(npoints->rows != 1 && npoints->cols != 1) )
CV_Error( CV_StsUnsupportedFormat,
"the array of point counters must be 1-dimensional integer vector" );
//when the thin prism model is used the distortion coefficients matrix must have 12 parameters
if((flags & CV_CALIB_THIN_PRISM_MODEL) && (distCoeffs->cols*distCoeffs->rows != 12))
CV_Error( CV_StsBadArg, "Thin prism model must have 12 parameters in the distortion matrix" );
nimages = npoints->rows*npoints->cols;
npstep = npoints->rows == 1 ? 1 : npoints->step/CV_ELEM_SIZE(npoints->type);
if( rvecs )
{
cn = CV_MAT_CN(rvecs->type);
if( !CV_IS_MAT(rvecs) ||
(CV_MAT_DEPTH(rvecs->type) != CV_32F && CV_MAT_DEPTH(rvecs->type) != CV_64F) ||
((rvecs->rows != nimages || (rvecs->cols*cn != 3 && rvecs->cols*cn != 9)) &&
(rvecs->rows != 1 || rvecs->cols != nimages || cn != 3)) )
CV_Error( CV_StsBadArg, "the output array of rotation vectors must be 3-channel "
"1xn or nx1 array or 1-channel nx3 or nx9 array, where n is the number of views" );
}
if( tvecs )
{
cn = CV_MAT_CN(tvecs->type);
if( !CV_IS_MAT(tvecs) ||
(CV_MAT_DEPTH(tvecs->type) != CV_32F && CV_MAT_DEPTH(tvecs->type) != CV_64F) ||
((tvecs->rows != nimages || tvecs->cols*cn != 3) &&
(tvecs->rows != 1 || tvecs->cols != nimages || cn != 3)) )
CV_Error( CV_StsBadArg, "the output array of translation vectors must be 3-channel "
"1xn or nx1 array or 1-channel nx3 array, where n is the number of views" );
}
if( (CV_MAT_TYPE(cameraMatrix->type) != CV_32FC1 &&
CV_MAT_TYPE(cameraMatrix->type) != CV_64FC1) ||
cameraMatrix->rows != 3 || cameraMatrix->cols != 3 )
CV_Error( CV_StsBadArg,
"Intrinsic parameters must be 3x3 floating-point matrix" );
if( (CV_MAT_TYPE(distCoeffs->type) != CV_32FC1 &&
CV_MAT_TYPE(distCoeffs->type) != CV_64FC1) ||
(distCoeffs->cols != 1 && distCoeffs->rows != 1) ||
(distCoeffs->cols*distCoeffs->rows != 4 &&
distCoeffs->cols*distCoeffs->rows != 5 &&
distCoeffs->cols*distCoeffs->rows != 8 &&
distCoeffs->cols*distCoeffs->rows != 12) )
CV_Error( CV_StsBadArg, cvDistCoeffErr );
for( i = 0; i < nimages; i++ )
{
ni = npoints->data.i[i*npstep];
if( ni < 4 )
{
char buf[100];
sprintf( buf, "The number of points in the view #%d is < 4", i );
CV_Error( CV_StsOutOfRange, buf );
}
maxPoints = MAX( maxPoints, ni );
total += ni;
}
matM.reset(cvCreateMat( 1, total, CV_64FC3 ));
_m.reset(cvCreateMat( 1, total, CV_64FC2 ));
cvConvertPointsHomogeneous( objectPoints, matM );
cvConvertPointsHomogeneous( imagePoints, _m );
nparams = NINTRINSIC + nimages*6;
_Ji.reset(cvCreateMat( maxPoints*2, NINTRINSIC, CV_64FC1 ));
_Je.reset(cvCreateMat( maxPoints*2, 6, CV_64FC1 ));
_err.reset(cvCreateMat( maxPoints*2, 1, CV_64FC1 ));
cvZero( _Ji );
_k = cvMat( distCoeffs->rows, distCoeffs->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k);
if( distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) < 8 )
{
if( distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) < 5 )
flags |= CV_CALIB_FIX_K3;
flags |= CV_CALIB_FIX_K4 | CV_CALIB_FIX_K5 | CV_CALIB_FIX_K6;
}
const double minValidAspectRatio = 0.01;
const double maxValidAspectRatio = 100.0;
// 1. initialize intrinsic parameters & LM solver
if( flags & CV_CALIB_USE_INTRINSIC_GUESS )
{
cvConvert( cameraMatrix, &matA );
if( A[0] <= 0 || A[4] <= 0 )
CV_Error( CV_StsOutOfRange, "Focal length (fx and fy) must be positive" );
if( A[2] < 0 || A[2] >= imageSize.width ||
A[5] < 0 || A[5] >= imageSize.height )
CV_Error( CV_StsOutOfRange, "Principal point must be within the image" );
if( fabs(A[1]) > 1e-5 )
CV_Error( CV_StsOutOfRange, "Non-zero skew is not supported by the function" );
if( fabs(A[3]) > 1e-5 || fabs(A[6]) > 1e-5 ||
fabs(A[7]) > 1e-5 || fabs(A[8]-1) > 1e-5 )
CV_Error( CV_StsOutOfRange,
"The intrinsic matrix must have [fx 0 cx; 0 fy cy; 0 0 1] shape" );
A[1] = A[3] = A[6] = A[7] = 0.;
A[8] = 1.;
if( flags & CV_CALIB_FIX_ASPECT_RATIO )
{
aspectRatio = A[0]/A[4];
if( aspectRatio < minValidAspectRatio || aspectRatio > maxValidAspectRatio )
CV_Error( CV_StsOutOfRange,
"The specified aspect ratio (= cameraMatrix[0][0] / cameraMatrix[1][1]) is incorrect" );
}
cvConvert( distCoeffs, &_k );
}
else
{
CvScalar mean, sdv;
cvAvgSdv( matM, &mean, &sdv );
if( fabs(mean.val[2]) > 1e-5 || fabs(sdv.val[2]) > 1e-5 )
CV_Error( CV_StsBadArg,
"For non-planar calibration rigs the initial intrinsic matrix must be specified" );
for( i = 0; i < total; i++ )
((CvPoint3D64f*)matM->data.db)[i].z = 0.;
if( flags & CV_CALIB_FIX_ASPECT_RATIO )
{
aspectRatio = cvmGet(cameraMatrix,0,0);
aspectRatio /= cvmGet(cameraMatrix,1,1);
if( aspectRatio < minValidAspectRatio || aspectRatio > maxValidAspectRatio )
CV_Error( CV_StsOutOfRange,
"The specified aspect ratio (= cameraMatrix[0][0] / cameraMatrix[1][1]) is incorrect" );
}
cvInitIntrinsicParams2D( matM, _m, npoints, imageSize, &matA, aspectRatio );
}
solver.init( nparams, 0, termCrit );
{
double* param = solver.param->data.db;
uchar* mask = solver.mask->data.ptr;
param[0] = A[0]; param[1] = A[4]; param[2] = A[2]; param[3] = A[5];
param[4] = k[0]; param[5] = k[1]; param[6] = k[2]; param[7] = k[3];
param[8] = k[4]; param[9] = k[5]; param[10] = k[6]; param[11] = k[7];
param[12] = k[8]; param[13] = k[9]; param[14] = k[10]; param[15] = k[11];
if( flags & CV_CALIB_FIX_FOCAL_LENGTH )
mask[0] = mask[1] = 0;
if( flags & CV_CALIB_FIX_PRINCIPAL_POINT )
mask[2] = mask[3] = 0;
if( flags & CV_CALIB_ZERO_TANGENT_DIST )
{
param[6] = param[7] = 0;
mask[6] = mask[7] = 0;
}
if( !(flags & CV_CALIB_RATIONAL_MODEL) )
flags |= CV_CALIB_FIX_K4 + CV_CALIB_FIX_K5 + CV_CALIB_FIX_K6;
if( !(flags & CV_CALIB_THIN_PRISM_MODEL))
flags |= CALIB_FIX_S1_S2_S3_S4;
if( flags & CV_CALIB_FIX_K1 )
mask[4] = 0;
if( flags & CV_CALIB_FIX_K2 )
mask[5] = 0;
if( flags & CV_CALIB_FIX_K3 )
mask[8] = 0;
if( flags & CV_CALIB_FIX_K4 )
mask[9] = 0;
if( flags & CV_CALIB_FIX_K5 )
mask[10] = 0;
if( flags & CV_CALIB_FIX_K6 )
mask[11] = 0;
if(flags & CALIB_FIX_S1_S2_S3_S4)
{
mask[12] = 0;
mask[13] = 0;
mask[14] = 0;
mask[15] = 0;
}
}
// 2. initialize extrinsic parameters
for( i = 0, pos = 0; i < nimages; i++, pos += ni )
{
CvMat _Mi, _mi, _ri, _ti;
ni = npoints->data.i[i*npstep];
cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 );
cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 );
cvGetCols( matM, &_Mi, pos, pos + ni );
cvGetCols( _m, &_mi, pos, pos + ni );
cvFindExtrinsicCameraParams2( &_Mi, &_mi, &matA, &_k, &_ri, &_ti );
}
// 3. run the optimization
for(;;)
{
const CvMat* _param = 0;
CvMat *_JtJ = 0, *_JtErr = 0;
double* _errNorm = 0;
bool proceed = solver.updateAlt( _param, _JtJ, _JtErr, _errNorm );
double *param = solver.param->data.db, *pparam = solver.prevParam->data.db;
if( flags & CV_CALIB_FIX_ASPECT_RATIO )
{
param[0] = param[1]*aspectRatio;
pparam[0] = pparam[1]*aspectRatio;
}
A[0] = param[0]; A[4] = param[1]; A[2] = param[2]; A[5] = param[3];
k[0] = param[4]; k[1] = param[5]; k[2] = param[6]; k[3] = param[7];
k[4] = param[8]; k[5] = param[9]; k[6] = param[10]; k[7] = param[11];
k[8] = param[12];k[9] = param[13];k[10] = param[14];k[11] = param[15];
if( !proceed )
break;
reprojErr = 0;
for( i = 0, pos = 0; i < nimages; i++, pos += ni )
{
CvMat _Mi, _mi, _ri, _ti, _dpdr, _dpdt, _dpdf, _dpdc, _dpdk, _mp, _part;
ni = npoints->data.i[i*npstep];
cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 );
cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 );
cvGetCols( matM, &_Mi, pos, pos + ni );
cvGetCols( _m, &_mi, pos, pos + ni );
_Je->rows = _Ji->rows = _err->rows = ni*2;
cvGetCols( _Je, &_dpdr, 0, 3 );
cvGetCols( _Je, &_dpdt, 3, 6 );
cvGetCols( _Ji, &_dpdf, 0, 2 );
cvGetCols( _Ji, &_dpdc, 2, 4 );
cvGetCols( _Ji, &_dpdk, 4, NINTRINSIC );
cvReshape( _err, &_mp, 2, 1 );
if( _JtJ || _JtErr )
{
cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, &_k, &_mp, &_dpdr, &_dpdt,
(flags & CV_CALIB_FIX_FOCAL_LENGTH) ? 0 : &_dpdf,
(flags & CV_CALIB_FIX_PRINCIPAL_POINT) ? 0 : &_dpdc, &_dpdk,
(flags & CV_CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0);
}
else
cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, &_k, &_mp );
cvSub( &_mp, &_mi, &_mp );
if( _JtJ || _JtErr )
{
cvGetSubRect( _JtJ, &_part, cvRect(0,0,NINTRINSIC,NINTRINSIC) );
cvGEMM( _Ji, _Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
cvGetSubRect( _JtJ, &_part, cvRect(NINTRINSIC+i*6,NINTRINSIC+i*6,6,6) );
cvGEMM( _Je, _Je, 1, 0, 0, &_part, CV_GEMM_A_T );
cvGetSubRect( _JtJ, &_part, cvRect(NINTRINSIC+i*6,0,6,NINTRINSIC) );
cvGEMM( _Ji, _Je, 1, 0, 0, &_part, CV_GEMM_A_T );
cvGetRows( _JtErr, &_part, 0, NINTRINSIC );
cvGEMM( _Ji, _err, 1, &_part, 1, &_part, CV_GEMM_A_T );
cvGetRows( _JtErr, &_part, NINTRINSIC + i*6, NINTRINSIC + (i+1)*6 );
cvGEMM( _Je, _err, 1, 0, 0, &_part, CV_GEMM_A_T );
}
double errNorm = cvNorm( &_mp, 0, CV_L2 );
reprojErr += errNorm*errNorm;
}
if( _errNorm )
*_errNorm = reprojErr;
}
// 4. store the results
cvConvert( &matA, cameraMatrix );
cvConvert( &_k, distCoeffs );
for( i = 0; i < nimages; i++ )
{
CvMat src, dst;
if( rvecs )
{
src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 );
if( rvecs->rows == nimages && rvecs->cols*CV_MAT_CN(rvecs->type) == 9 )
{
dst = cvMat( 3, 3, CV_MAT_DEPTH(rvecs->type),
rvecs->data.ptr + rvecs->step*i );
cvRodrigues2( &src, &matA );
cvConvert( &matA, &dst );
}
else
{
dst = cvMat( 3, 1, CV_MAT_DEPTH(rvecs->type), rvecs->rows == 1 ?
rvecs->data.ptr + i*CV_ELEM_SIZE(rvecs->type) :
rvecs->data.ptr + rvecs->step*i );
cvConvert( &src, &dst );
}
}
if( tvecs )
{
src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 + 3 );
dst = cvMat( 3, 1, CV_MAT_DEPTH(tvecs->type), tvecs->rows == 1 ?
tvecs->data.ptr + i*CV_ELEM_SIZE(tvecs->type) :
tvecs->data.ptr + tvecs->step*i );
cvConvert( &src, &dst );
}
}
return std::sqrt(reprojErr/total);
}