From 742fb559f7242b5a0afe31c7c08a32e37f155d54 Mon Sep 17 00:00:00 2001 From: Pavel Rojtberg Date: Tue, 3 Nov 2015 12:30:29 +0100 Subject: [PATCH 1/2] use cpp functions in cvCalibrateCamera2 to make it more readable --- modules/calib3d/src/calibration.cpp | 146 +++++++++++++--------------- 1 file changed, 69 insertions(+), 77 deletions(-) diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index bc789fa99..fc0a267be 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -1232,12 +1232,12 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, CvMat* rvecs, CvMat* tvecs, int flags, CvTermCriteria termCrit ) { const int NINTRINSIC = 16; - Ptr 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; + Matx33d A; + double k[12] = {0}; + CvMat matA = cvMat(3, 3, CV_64F, A.val), _k; int i, nimages, maxPoints = 0, ni = 0, pos, total = 0, nparams, npstep, cn; double aspectRatio = 0.; @@ -1302,25 +1302,31 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, 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 ); + CV_Error_( CV_StsOutOfRange, ("The number of points in the view #%d is < 4", i)); } maxPoints = MAX( maxPoints, ni ); total += ni; } - matM.reset(cvCreateMat( 1, total, CV_64FC3 )); - _m.reset(cvCreateMat( 1, total, CV_64FC2 )); + Mat matM( 1, total, CV_64FC3 ); + Mat _m( 1, total, CV_64FC2 ); - cvConvertPointsHomogeneous( objectPoints, matM ); - cvConvertPointsHomogeneous( imagePoints, _m ); + if(CV_MAT_CN(objectPoints->type) == 3) { + cvarrToMat(objectPoints).convertTo(matM, CV_64F); + } else { + convertPointsHomogeneous(cvarrToMat(objectPoints), matM); + } + + if(CV_MAT_CN(imagePoints->type) == 2) { + cvarrToMat(imagePoints).convertTo(_m, CV_64F); + } else { + convertPointsHomogeneous(cvarrToMat(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 ); + Mat _Ji( maxPoints*2, NINTRINSIC, CV_64FC1, Scalar(0)); + Mat _Je( maxPoints*2, 6, CV_64FC1 ); + Mat _err( maxPoints*2, 1, CV_64FC1 ); _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 ) @@ -1336,23 +1342,23 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, if( flags & CV_CALIB_USE_INTRINSIC_GUESS ) { cvConvert( cameraMatrix, &matA ); - if( A[0] <= 0 || A[4] <= 0 ) + if( A(0, 0) <= 0 || A(1, 1) <= 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 ) + if( A(0, 2) < 0 || A(0, 2) >= imageSize.width || + A(1, 2) < 0 || A(1, 2) >= imageSize.height ) CV_Error( CV_StsOutOfRange, "Principal point must be within the image" ); - if( fabs(A[1]) > 1e-5 ) + if( fabs(A(0, 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 ) + if( fabs(A(1, 0)) > 1e-5 || fabs(A(2, 0)) > 1e-5 || + fabs(A(2, 1)) > 1e-5 || fabs(A(2,2)-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.; + A(0, 1) = A(1, 0) = A(2, 0) = A(2, 1) = 0.; + A(2, 2) = 1.; if( flags & CV_CALIB_FIX_ASPECT_RATIO ) { - aspectRatio = A[0]/A[4]; + aspectRatio = A(0, 0)/A(1, 1); if( aspectRatio < minValidAspectRatio || aspectRatio > maxValidAspectRatio ) CV_Error( CV_StsOutOfRange, @@ -1362,13 +1368,13 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, } else { - CvScalar mean, sdv; - cvAvgSdv( matM, &mean, &sdv ); - if( fabs(mean.val[2]) > 1e-5 || fabs(sdv.val[2]) > 1e-5 ) + Scalar mean, sdv; + meanStdDev(matM, mean, sdv); + if( fabs(mean[2]) > 1e-5 || fabs(sdv[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.; + matM.at(i).z = 0.; if( flags & CV_CALIB_FIX_ASPECT_RATIO ) { @@ -1378,7 +1384,8 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, CV_Error( CV_StsOutOfRange, "The specified aspect ratio (= cameraMatrix[0][0] / cameraMatrix[1][1]) is incorrect" ); } - cvInitIntrinsicParams2D( matM, _m, npoints, imageSize, &matA, aspectRatio ); + CvMat _matM(matM), m(_m); + cvInitIntrinsicParams2D( &_matM, &m, npoints, imageSize, &matA, aspectRatio ); } solver.init( nparams, 0, termCrit ); @@ -1387,10 +1394,8 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, 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]; + param[0] = A(0, 0); param[1] = A(1, 1); param[2] = A(0, 2); param[3] = A(1, 2); + std::copy(k, k + 12, param + 4); if( flags & CV_CALIB_FIX_FOCAL_LENGTH ) mask[0] = mask[1] = 0; @@ -1405,18 +1410,13 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, 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; + + mask[ 4] = !(flags & CALIB_FIX_K1); + mask[ 5] = !(flags & CALIB_FIX_K2); + mask[ 8] = !(flags & CALIB_FIX_K3); + mask[ 9] = !(flags & CALIB_FIX_K4); + mask[10] = !(flags & CALIB_FIX_K5); + mask[11] = !(flags & CALIB_FIX_K6); if(flags & CALIB_FIX_S1_S2_S3_S4) { @@ -1430,14 +1430,14 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, // 2. initialize extrinsic parameters for( i = 0, pos = 0; i < nimages; i++, pos += ni ) { - CvMat _Mi, _mi, _ri, _ti; + CvMat _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 ); + CvMat _Mi(matM.colRange(pos, pos + ni)); + CvMat _mi(_m.colRange(pos, pos + ni)); cvFindExtrinsicCameraParams2( &_Mi, &_mi, &matA, &_k, &_ri, &_ti ); } @@ -1457,10 +1457,8 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, 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]; + A(0, 0) = param[0]; A(1, 1) = param[1]; A(0, 2) = param[2]; A(1, 2) = param[3]; + std::copy(param + 4, param + 4 + 12, k); if( !proceed ) break; @@ -1469,24 +1467,24 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, for( i = 0, pos = 0; i < nimages; i++, pos += ni ) { - CvMat _Mi, _mi, _ri, _ti, _dpdr, _dpdt, _dpdf, _dpdc, _dpdk, _mp, _part; + CvMat _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 ); + CvMat _Mi(matM.colRange(pos, pos + ni)); + CvMat _mi(_m.colRange(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 ); + _Je.resize(ni*2); _Ji.resize(ni*2); _err.resize(ni*2); + CvMat _dpdr(_Je.colRange(0, 3)); + CvMat _dpdt(_Je.colRange(3, 6)); + CvMat _dpdf(_Ji.colRange(0, 2)); + CvMat _dpdc(_Ji.colRange(2, 4)); + CvMat _dpdk(_Ji.colRange(4, NINTRINSIC)); + CvMat _mp(_err.reshape(2, 1)); - if( _JtJ || _JtErr ) + if( solver.state == CvLevMarq::CALC_J ) { cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, &_k, &_mp, &_dpdr, &_dpdt, (flags & CV_CALIB_FIX_FOCAL_LENGTH) ? 0 : &_dpdf, @@ -1498,26 +1496,20 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, cvSub( &_mp, &_mi, &_mp ); - if( _JtJ || _JtErr ) + if( solver.state == CvLevMarq::CALC_J ) { - cvGetSubRect( _JtJ, &_part, cvRect(0,0,NINTRINSIC,NINTRINSIC) ); - cvGEMM( _Ji, _Ji, 1, &_part, 1, &_part, CV_GEMM_A_T ); + Mat JtJ(cvarrToMat(_JtJ)), JtErr(cvarrToMat(_JtErr)); - cvGetSubRect( _JtJ, &_part, cvRect(NINTRINSIC+i*6,NINTRINSIC+i*6,6,6) ); - cvGEMM( _Je, _Je, 1, 0, 0, &_part, CV_GEMM_A_T ); + // see HZ: (A6.14) for details on the structure of the Jacobian + JtJ(Rect(0, 0, NINTRINSIC, NINTRINSIC)) += _Ji.t() * _Ji; + JtJ(Rect(NINTRINSIC + i * 6, NINTRINSIC + i * 6, 6, 6)) = _Je.t() * _Je; + JtJ(Rect(NINTRINSIC + i * 6, 0, 6, NINTRINSIC)) = _Ji.t() * _Je; - 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 ); + JtErr.rowRange(0, NINTRINSIC) += _Ji.t() * _err; + JtErr.rowRange(NINTRINSIC + i * 6, NINTRINSIC + (i + 1) * 6) = _Je.t() * _err; } - double errNorm = cvNorm( &_mp, 0, CV_L2 ); - reprojErr += errNorm*errNorm; + reprojErr += norm(_err, NORM_L2SQR); } if( _errNorm ) *_errNorm = reprojErr; From 9233472bdd7228acd91d390c04783b7bcae0fd6d Mon Sep 17 00:00:00 2001 From: Pavel Rojtberg Date: Fri, 6 Nov 2015 12:02:31 +0100 Subject: [PATCH 2/2] use cpp functions in CvLevMarq::step for better readability --- modules/calib3d/src/compat_ptsetreg.cpp | 42 +++++++++++++------------ 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/modules/calib3d/src/compat_ptsetreg.cpp b/modules/calib3d/src/compat_ptsetreg.cpp index a56eafb9d..d2ab252d3 100644 --- a/modules/calib3d/src/compat_ptsetreg.cpp +++ b/modules/calib3d/src/compat_ptsetreg.cpp @@ -120,8 +120,6 @@ void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _co bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err ) { - double change; - matJ = _err = 0; assert( !err.empty() ); @@ -174,7 +172,7 @@ bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err ) lambdaLg10 = MAX(lambdaLg10-1, -16); if( ++iters >= criteria.max_iter || - (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon ) + cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon ) { _param = param; state = DONE; @@ -193,8 +191,6 @@ bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err ) bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm ) { - double change; - CV_Assert( !err ); if( state == DONE ) { @@ -243,7 +239,7 @@ bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, d lambdaLg10 = MAX(lambdaLg10-1, -16); if( ++iters >= criteria.max_iter || - (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon ) + cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon ) { _param = param; state = DONE; @@ -262,32 +258,38 @@ bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, d void CvLevMarq::step() { + using namespace cv; const double LOG10 = log(10.); double lambda = exp(lambdaLg10*LOG10); - int i, j, nparams = param->rows; + int nparams = param->rows; - for( i = 0; i < nparams; i++ ) + Mat _JtJ = cvarrToMat(JtJ); + Mat _JtJN = cvarrToMat(JtJN); + Mat _JtJW = cvarrToMat(JtJW); + Mat _JtJV = cvarrToMat(JtJV); + + for( int i = 0; i < nparams; i++ ) if( mask->data.ptr[i] == 0 ) { - double *row = JtJ->data.db + i*nparams, *col = JtJ->data.db + i; - for( j = 0; j < nparams; j++ ) - row[j] = col[j*nparams] = 0; + _JtJ.row(i) = 0; + _JtJ.col(i) = 0; JtErr->data.db[i] = 0; } if( !err ) - cvCompleteSymm( JtJ, completeSymmFlag ); + completeSymm( _JtJ, completeSymmFlag ); + + _JtJ.copyTo(_JtJN); #if 1 - cvCopy( JtJ, JtJN ); - for( i = 0; i < nparams; i++ ) - JtJN->data.db[(nparams+1)*i] *= 1. + lambda; + _JtJN.diag() *= 1. + lambda; #else - cvSetIdentity(JtJN, cvRealScalar(lambda)); - cvAdd( JtJ, JtJN, JtJN ); + _JtJN.diag() += lambda; #endif - cvSVD( JtJN, JtJW, 0, JtJV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T ); - cvSVBkSb( JtJW, JtJV, JtJV, JtErr, param, CV_SVD_U_T + CV_SVD_V_T ); - for( i = 0; i < nparams; i++ ) + // solve(JtJN, JtErr, param, DECOMP_SVD); + SVD::compute(_JtJN, _JtJW, noArray(), _JtJV, SVD::MODIFY_A); + SVD::backSubst(_JtJW, _JtJV.t(), _JtJV, cvarrToMat(JtErr), cvarrToMat(param)); + + for( int i = 0; i < nparams; i++ ) param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? param->data.db[i] : 0); }