Skip to content

Commit 4142e73

Browse files
vpisarevvpisarev
committed
Merge pull request opencv#6453 from sovrasov:extend_calibrateCamera
2 parents 7fadf57 + 46fb46c commit 4142e73

File tree

5 files changed

+267
-28
lines changed

5 files changed

+267
-28
lines changed

‎modules/calib3d/include/opencv2/calib3d.hpp

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -767,6 +767,14 @@ k-th translation vector (see the next output parameter description) brings the c
767767
from the model coordinate space (in which object points are specified) to the world coordinate
768768
space, that is, a real position of the calibration pattern in the k-th pattern view (k=0.. *M* -1).
769769
@param tvecs Output vector of translation vectors estimated for each pattern view.
770+
@param stdDeviationsIntrinsics Output vector of standard deviations estimated for intrinsic parameters.
771+
Order of deviations values:
772+
\f$(f_x, f_y, c_x, c_y, k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6 , s_1, s_2, s_3,
773+
s_4, \tau_x, \tau_y)\f$ If one of parameters is not estimated, it's deviation is equals to zero.
774+
@param stdDeviationsExtrinsics Output vector of standard deviations estimated for extrinsic parameters.
775+
Order of deviations values: \f$(R_1, T_1, \dotsc , R_M, T_M)\f$ where M is number of pattern views,
776+
\f$R_i, T_i\f$ are concatenated 1x3 vectors.
777+
@param perViewErrors Output vector of average re-projection errors estimated for each pattern view.
770778
@param flags Different flags that may be zero or a combination of the following values:
771779
- **CV_CALIB_USE_INTRINSIC_GUESS** cameraMatrix contains valid initial values of
772780
fx, fy, cx, cy that are optimized further. Otherwise, (cx, cy) is initially set to the image
@@ -841,6 +849,24 @@ The function returns the final re-projection error.
841849
@sa
842850
findChessboardCorners, solvePnP, initCameraMatrix2D, stereoCalibrate, undistort
843851
*/
852+
CV_EXPORTS_AS(calibrateCameraExtended) double calibrateCamera( InputArrayOfArrays objectPoints,
853+
InputArrayOfArrays imagePoints, Size imageSize,
854+
InputOutputArray cameraMatrix, InputOutputArray distCoeffs,
855+
OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
856+
OutputArray stdDeviationsIntrinsics,
857+
OutputArray stdDeviationsExtrinsics,
858+
OutputArray perViewErrors,
859+
int flags = 0, TermCriteria criteria = TermCriteria(
860+
TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
861+
862+
/** @overload double calibrateCamera( InputArrayOfArrays objectPoints,
863+
InputArrayOfArrays imagePoints, Size imageSize,
864+
InputOutputArray cameraMatrix, InputOutputArray distCoeffs,
865+
OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
866+
OutputArray stdDeviations, OutputArray perViewErrors,
867+
int flags = 0, TermCriteria criteria = TermCriteria(
868+
TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) )
869+
*/
844870
CV_EXPORTS_W double calibrateCamera( InputArrayOfArrays objectPoints,
845871
InputArrayOfArrays imagePoints, Size imageSize,
846872
InputOutputArray cameraMatrix, InputOutputArray distCoeffs,

‎modules/calib3d/include/opencv2/calib3d/calib3d_c.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -246,6 +246,7 @@ CVAPI(void) cvDrawChessboardCorners( CvArr* image, CvSize pattern_size,
246246
#define CV_CALIB_TILTED_MODEL 262144
247247
#define CV_CALIB_FIX_TAUX_TAUY 524288
248248

249+
#define CV_CALIB_NINTRINSIC 18
249250

250251
/* Finds intrinsic and extrinsic camera parameters
251252
from a few views of known calibration pattern */

‎modules/calib3d/src/calibration.cpp

Lines changed: 155 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -1181,7 +1181,6 @@ CV_IMPL void cvFindExtrinsicCameraParams2( const CvMat* objectPoints,
11811181
cvConvert( &_t, tvec );
11821182
}
11831183

1184-
11851184
CV_IMPL void cvInitIntrinsicParams2D( const CvMat* objectPoints,
11861185
const CvMat* imagePoints, const CvMat* npoints,
11871186
CvSize imageSize, CvMat* cameraMatrix,
@@ -1270,15 +1269,37 @@ CV_IMPL void cvInitIntrinsicParams2D( const CvMat* objectPoints,
12701269
cvConvert( &_a, cameraMatrix );
12711270
}
12721271

1272+
static void subMatrix(const cv::Mat& src, cv::Mat& dst, const std::vector<uchar>& cols,
1273+
const std::vector<uchar>& rows) {
1274+
int nonzeros_cols = cv::countNonZero(cols);
1275+
cv::Mat tmp(src.rows, nonzeros_cols, CV_64FC1);
12731276

1274-
/* finds intrinsic and extrinsic camera parameters
1275-
from a few views of known calibration pattern */
1276-
CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
1277+
for (int i = 0, j = 0; i < (int)cols.size(); i++)
1278+
{
1279+
if (cols[i])
1280+
{
1281+
src.col(i).copyTo(tmp.col(j++));
1282+
}
1283+
}
1284+
1285+
int nonzeros_rows = cv::countNonZero(rows);
1286+
dst.create(nonzeros_rows, nonzeros_cols, CV_64FC1);
1287+
for (int i = 0, j = 0; i < (int)rows.size(); i++)
1288+
{
1289+
if (rows[i])
1290+
{
1291+
tmp.row(i).copyTo(dst.row(j++));
1292+
}
1293+
}
1294+
}
1295+
1296+
static double cvCalibrateCamera2Internal( const CvMat* objectPoints,
12771297
const CvMat* imagePoints, const CvMat* npoints,
12781298
CvSize imageSize, CvMat* cameraMatrix, CvMat* distCoeffs,
1279-
CvMat* rvecs, CvMat* tvecs, int flags, CvTermCriteria termCrit )
1299+
CvMat* rvecs, CvMat* tvecs, CvMat* stdDevs,
1300+
CvMat* perViewErrors, int flags, CvTermCriteria termCrit )
12801301
{
1281-
const int NINTRINSIC = 18;
1302+
const int NINTRINSIC = CV_CALIB_NINTRINSIC;
12821303
double reprojErr = 0;
12831304

12841305
Matx33d A;
@@ -1338,6 +1359,20 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
13381359
"1xn or nx1 array or 1-channel nx3 array, where n is the number of views" );
13391360
}
13401361

1362+
if( stdDevs )
1363+
{
1364+
cn = CV_MAT_CN(stdDevs->type);
1365+
if( !CV_IS_MAT(stdDevs) ||
1366+
(CV_MAT_DEPTH(stdDevs->type) != CV_32F && CV_MAT_DEPTH(stdDevs->type) != CV_64F) ||
1367+
((stdDevs->rows != (nimages*6 + NINTRINSIC) || stdDevs->cols*cn != 1) &&
1368+
(stdDevs->rows != 1 || stdDevs->cols != (nimages*6 + NINTRINSIC) || cn != 1)) )
1369+
#define STR__(x) #x
1370+
#define STR_(x) STR__(x)
1371+
CV_Error( CV_StsBadArg, "the output array of standard deviations vectors must be 1-channel "
1372+
"1x(n*6 + NINTRINSIC) or (n*6 + NINTRINSIC)x1 array, where n is the number of views,"
1373+
" NINTRINSIC = " STR_(CV_CALIB_NINTRINSIC));
1374+
}
1375+
13411376
if( (CV_MAT_TYPE(cameraMatrix->type) != CV_32FC1 &&
13421377
CV_MAT_TYPE(cameraMatrix->type) != CV_64FC1) ||
13431378
cameraMatrix->rows != 3 || cameraMatrix->cols != 3 )
@@ -1367,6 +1402,7 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
13671402

13681403
Mat matM( 1, total, CV_64FC3 );
13691404
Mat _m( 1, total, CV_64FC2 );
1405+
Mat allErrors(1, total, CV_64FC2);
13701406

13711407
if(CV_MAT_CN(objectPoints->type) == 3) {
13721408
cvarrToMat(objectPoints).convertTo(matM, CV_64F);
@@ -1518,6 +1554,7 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
15181554
double* _errNorm = 0;
15191555
bool proceed = solver.updateAlt( _param, _JtJ, _JtErr, _errNorm );
15201556
double *param = solver.param->data.db, *pparam = solver.prevParam->data.db;
1557+
bool calcJ = solver.state == CvLevMarq::CALC_J || (!proceed && stdDevs);
15211558

15221559
if( flags & CALIB_FIX_ASPECT_RATIO )
15231560
{
@@ -1528,8 +1565,10 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
15281565
A(0, 0) = param[0]; A(1, 1) = param[1]; A(0, 2) = param[2]; A(1, 2) = param[3];
15291566
std::copy(param + 4, param + 4 + 14, k);
15301567

1531-
if( !proceed )
1568+
if ( !proceed && !stdDevs && !perViewErrors )
15321569
break;
1570+
else if ( !proceed && stdDevs )
1571+
cvZero(_JtJ);
15331572

15341573
reprojErr = 0;
15351574

@@ -1543,6 +1582,7 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
15431582

15441583
CvMat _Mi(matM.colRange(pos, pos + ni));
15451584
CvMat _mi(_m.colRange(pos, pos + ni));
1585+
CvMat _me(allErrors.colRange(pos, pos + ni));
15461586

15471587
_Je.resize(ni*2); _Ji.resize(ni*2); _err.resize(ni*2);
15481588
CvMat _dpdr(_Je.colRange(0, 3));
@@ -1552,7 +1592,7 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
15521592
CvMat _dpdk(_Ji.colRange(4, NINTRINSIC));
15531593
CvMat _mp(_err.reshape(2, 1));
15541594

1555-
if( solver.state == CvLevMarq::CALC_J )
1595+
if( calcJ )
15561596
{
15571597
cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, &_k, &_mp, &_dpdr, &_dpdt,
15581598
(flags & CALIB_FIX_FOCAL_LENGTH) ? 0 : &_dpdf,
@@ -1563,8 +1603,10 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
15631603
cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, &_k, &_mp );
15641604

15651605
cvSub( &_mp, &_mi, &_mp );
1606+
if (perViewErrors || stdDevs)
1607+
cvCopy(&_mp, &_me);
15661608

1567-
if( solver.state == CvLevMarq::CALC_J )
1609+
if( calcJ )
15681610
{
15691611
Mat JtJ(cvarrToMat(_JtJ)), JtErr(cvarrToMat(_JtErr));
15701612

@@ -1581,15 +1623,52 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
15811623
}
15821624
if( _errNorm )
15831625
*_errNorm = reprojErr;
1626+
1627+
if( !proceed )
1628+
{
1629+
if( stdDevs )
1630+
{
1631+
Mat mask = cvarrToMat(solver.mask);
1632+
int nparams_nz = countNonZero(mask);
1633+
Mat JtJinv, JtJN;
1634+
JtJN.create(nparams_nz, nparams_nz, CV_64F);
1635+
subMatrix(cvarrToMat(_JtJ), JtJN, mask, mask);
1636+
completeSymm(JtJN, false);
1637+
cv::invert(JtJN, JtJinv, DECOMP_SVD);
1638+
//sigma2 is deviation of the noise
1639+
//see any papers about variance of the least squares estimator for
1640+
//detailed description of the variance estimation methods
1641+
double sigma2 = norm(allErrors, NORM_L2SQR) / (total - nparams_nz);
1642+
Mat stdDevsM = cvarrToMat(stdDevs);
1643+
int j = 0;
1644+
for ( int s = 0; s < nparams; s++ )
1645+
if( mask.data[s] )
1646+
{
1647+
stdDevsM.at<double>(s) = std::sqrt(JtJinv.at<double>(j,j) * sigma2);
1648+
j++;
1649+
}
1650+
else
1651+
stdDevsM.at<double>(s) = 0.;
1652+
}
1653+
break;
1654+
}
15841655
}
15851656

15861657
// 4. store the results
15871658
cvConvert( &matA, cameraMatrix );
15881659
cvConvert( &_k, distCoeffs );
15891660

1590-
for( i = 0; i < nimages; i++ )
1661+
for( i = 0, pos = 0; i < nimages; i++ )
15911662
{
15921663
CvMat src, dst;
1664+
if( perViewErrors )
1665+
{
1666+
ni = npoints->data.i[i*npstep];
1667+
perViewErrors->data.db[i] = std::sqrt(cv::norm(allErrors.colRange(pos, pos + ni),
1668+
NORM_L2SQR) / ni);
1669+
pos+=ni;
1670+
}
1671+
15931672
if( rvecs )
15941673
{
15951674
src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 );
@@ -1622,6 +1701,17 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
16221701
}
16231702

16241703

1704+
/* finds intrinsic and extrinsic camera parameters
1705+
from a few views of known calibration pattern */
1706+
CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
1707+
const CvMat* imagePoints, const CvMat* npoints,
1708+
CvSize imageSize, CvMat* cameraMatrix, CvMat* distCoeffs,
1709+
CvMat* rvecs, CvMat* tvecs, int flags, CvTermCriteria termCrit )
1710+
{
1711+
return cvCalibrateCamera2Internal(objectPoints, imagePoints, npoints, imageSize, cameraMatrix,
1712+
distCoeffs, rvecs, tvecs, NULL, NULL, flags, termCrit);
1713+
}
1714+
16251715
void cvCalibrationMatrixValues( const CvMat *calibMatr, CvSize imgSize,
16261716
double apertureWidth, double apertureHeight, double *fovx, double *fovy,
16271717
double *focalLength, CvPoint2D64f *principalPoint, double *pasp )
@@ -1772,7 +1862,7 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
17721862
if( !(flags & (CV_CALIB_FIX_INTRINSIC|CV_CALIB_USE_INTRINSIC_GUESS)))
17731863
{
17741864
cvCalibrateCamera2( objectPoints, imagePoints[k],
1775-
npoints, imageSize, &K[k], &Dist[k], 0, 0, flags );
1865+
npoints, imageSize, &K[k], &Dist[k], NULL, NULL, flags );
17761866
}
17771867
}
17781868

@@ -3091,7 +3181,6 @@ static void collectCalibrationData( InputArrayOfArrays objectPoints,
30913181
}
30923182
}
30933183

3094-
30953184
static Mat prepareCameraMatrix(Mat& cameraMatrix0, int rtype)
30963185
{
30973186
Mat cameraMatrix = Mat::eye(3, 3, rtype);
@@ -3287,10 +3376,23 @@ cv::Mat cv::initCameraMatrix2D( InputArrayOfArrays objectPoints,
32873376
}
32883377

32893378

3379+
32903380
double cv::calibrateCamera( InputArrayOfArrays _objectPoints,
32913381
InputArrayOfArrays _imagePoints,
32923382
Size imageSize, InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs,
32933383
OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, int flags, TermCriteria criteria )
3384+
{
3385+
return calibrateCamera(_objectPoints, _imagePoints, imageSize, _cameraMatrix, _distCoeffs,
3386+
_rvecs, _tvecs, noArray(), noArray(), noArray(), flags, criteria);
3387+
}
3388+
3389+
double cv::calibrateCamera(InputArrayOfArrays _objectPoints,
3390+
InputArrayOfArrays _imagePoints,
3391+
Size imageSize, InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs,
3392+
OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs,
3393+
OutputArray stdDeviationsIntrinsics,
3394+
OutputArray stdDeviationsExtrinsics,
3395+
OutputArray _perViewErrors, int flags, TermCriteria criteria )
32943396
{
32953397
int rtype = CV_64F;
32963398
Mat cameraMatrix = _cameraMatrix.getMat();
@@ -3304,14 +3406,17 @@ double cv::calibrateCamera( InputArrayOfArrays _objectPoints,
33043406

33053407
int nimages = int(_objectPoints.total());
33063408
CV_Assert( nimages > 0 );
3307-
Mat objPt, imgPt, npoints, rvecM, tvecM;
3409+
Mat objPt, imgPt, npoints, rvecM, tvecM, stdDeviationsM, errorsM;
33083410

3309-
bool rvecs_needed = _rvecs.needed(), tvecs_needed = _tvecs.needed();
3411+
bool rvecs_needed = _rvecs.needed(), tvecs_needed = _tvecs.needed(),
3412+
stddev_needed = stdDeviationsIntrinsics.needed(), errors_needed = _perViewErrors.needed(),
3413+
stddev_ext_needed = stdDeviationsExtrinsics.needed();
33103414

33113415
bool rvecs_mat_vec = _rvecs.isMatVector();
33123416
bool tvecs_mat_vec = _tvecs.isMatVector();
33133417

3314-
if( rvecs_needed ) {
3418+
if( rvecs_needed )
3419+
{
33153420
_rvecs.create(nimages, 1, CV_64FC3);
33163421

33173422
if(rvecs_mat_vec)
@@ -3320,7 +3425,8 @@ double cv::calibrateCamera( InputArrayOfArrays _objectPoints,
33203425
rvecM = _rvecs.getMat();
33213426
}
33223427

3323-
if( tvecs_needed ) {
3428+
if( tvecs_needed )
3429+
{
33243430
_tvecs.create(nimages, 1, CV_64FC3);
33253431

33263432
if(tvecs_mat_vec)
@@ -3329,16 +3435,46 @@ double cv::calibrateCamera( InputArrayOfArrays _objectPoints,
33293435
tvecM = _tvecs.getMat();
33303436
}
33313437

3438+
if( stddev_needed || stddev_ext_needed )
3439+
{
3440+
stdDeviationsM.create(nimages*6 + CV_CALIB_NINTRINSIC, 1, CV_64F);
3441+
}
3442+
3443+
if( errors_needed )
3444+
{
3445+
_perViewErrors.create(nimages, 1, CV_64F);
3446+
errorsM = _perViewErrors.getMat();
3447+
}
3448+
33323449
collectCalibrationData( _objectPoints, _imagePoints, noArray(),
33333450
objPt, imgPt, 0, npoints );
33343451
CvMat c_objPt = objPt, c_imgPt = imgPt, c_npoints = npoints;
33353452
CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs;
3336-
CvMat c_rvecM = rvecM, c_tvecM = tvecM;
3453+
CvMat c_rvecM = rvecM, c_tvecM = tvecM, c_stdDev = stdDeviationsM, c_errors = errorsM;
33373454

3338-
double reprojErr = cvCalibrateCamera2(&c_objPt, &c_imgPt, &c_npoints, imageSize,
3455+
double reprojErr = cvCalibrateCamera2Internal(&c_objPt, &c_imgPt, &c_npoints, imageSize,
33393456
&c_cameraMatrix, &c_distCoeffs,
33403457
rvecs_needed ? &c_rvecM : NULL,
3341-
tvecs_needed ? &c_tvecM : NULL, flags, criteria );
3458+
tvecs_needed ? &c_tvecM : NULL,
3459+
stddev_needed ? &c_stdDev : NULL,
3460+
errors_needed ? &c_errors : NULL, flags, criteria );
3461+
3462+
if( stddev_needed )
3463+
{
3464+
stdDeviationsIntrinsics.create(CV_CALIB_NINTRINSIC, 1, CV_64F);
3465+
Mat stdDeviationsIntrinsicsMat = stdDeviationsIntrinsics.getMat();
3466+
std::memcpy(stdDeviationsIntrinsicsMat.ptr(), stdDeviationsM.ptr(),
3467+
CV_CALIB_NINTRINSIC*sizeof(double));
3468+
}
3469+
3470+
if ( stddev_ext_needed )
3471+
{
3472+
stdDeviationsExtrinsics.create(nimages*6, 1, CV_64F);
3473+
Mat stdDeviationsExtrinsicsMat = stdDeviationsExtrinsics.getMat();
3474+
std::memcpy(stdDeviationsExtrinsicsMat.ptr(),
3475+
stdDeviationsM.ptr() + CV_CALIB_NINTRINSIC*sizeof(double),
3476+
nimages*6*sizeof(double));
3477+
}
33423478

33433479
// overly complicated and inefficient rvec/ tvec handling to support vector<Mat>
33443480
for(int i = 0; i < nimages; i++ )

‎modules/calib3d/src/compat_ptsetreg.cpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -241,6 +241,8 @@ bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, d
241241
cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon )
242242
{
243243
_param = param;
244+
_JtJ = JtJ;
245+
_JtErr = JtErr;
244246
state = DONE;
245247
return false;
246248
}

0 commit comments

Comments
 (0)