Skip to content

O
opencv
Commit 5ada4627 authored by Pavel Rojtberg's avatar Pavel Rojtberg
Browse files
Options

calib3d: use cpp functions in cvStereoCalibrate to make it more readable

parent 7b8e6307
Show whitespace changes
Inline Side-by-side
Showing with 31 additions and 52 deletions
modules/calib3d/src/calibration.cpp
View file @ 5ada4627
......@@ -1766,16 +1766,16 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
CvTermCriteria termCrit )
{
const int NINTRINSIC = 18;
Ptr<CvMat> npoints, err, J_LR, Je, Ji, imagePoints[2], objectPoints, RT0;
Ptr<CvMat> npoints, imagePoints[2], objectPoints, RT0;
double reprojErr = 0;
double A[2][9], dk[2][14]={{0,0,0,0,0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0,0,0,0,0}}, rlr[9];
double A[2][9], dk[2][14]={{0}}, rlr[9];
CvMat K[2], Dist[2], om_LR, T_LR;
CvMat R_LR = cvMat(3, 3, CV_64F, rlr);
int i, k, p, ni = 0, ofs, nimages, pointsTotal, maxPoints = 0;
int nparams;
bool recomputeIntrinsics = false;
double aspectRatio[2] = {0,0};
double aspectRatio[2] = {0};
CV_Assert( CV_IS_MAT(_imagePoints1) && CV_IS_MAT(_imagePoints2) &&
CV_IS_MAT(_objectPoints) && CV_IS_MAT(_npoints) &&
......@@ -1855,11 +1855,10 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
recomputeIntrinsics = (flags & CALIB_FIX_INTRINSIC) == 0;
err.reset(cvCreateMat( maxPoints*2, 1, CV_64F ));
Je.reset(cvCreateMat( maxPoints*2, 6, CV_64F ));
J_LR.reset(cvCreateMat( maxPoints*2, 6, CV_64F ));
Ji.reset(cvCreateMat( maxPoints*2, NINTRINSIC, CV_64F ));
cvZero( Ji );
Mat err( maxPoints*2, 1, CV_64F );
Mat Je( maxPoints*2, 6, CV_64F );
Mat J_LR( maxPoints*2, 6, CV_64F );
Mat Ji( maxPoints*2, NINTRINSIC, CV_64F, Scalar(0) );
// we optimize for the inter-camera R(3),t(3), then, optionally,
// for intrinisic parameters of each camera ((fx,fy,cx,cy,k1,k2,p1,p2) ~ 8 parameters).
......@@ -1943,8 +1942,6 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
R[k] = cvMat(3, 3, CV_64F, r[k]);
T[k] = cvMat(3, 1, CV_64F, t[k]);
// FIXME: here we ignore activePoints[k] because of
// the limited API of cvFindExtrnisicCameraParams2
cvFindExtrinsicCameraParams2( &objpt_i, &imgpt_i[k], &K[k], &Dist[k], &om[k], &T[k] );
cvRodrigues2( &om[k], &R[k] );
if( k == 0 )
......@@ -2001,7 +1998,6 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
for(;;)
{
const CvMat* param = 0;
CvMat tmpimagePoints;
CvMat *JtJ = 0, *JtErr = 0;
double *_errNorm = 0;
double _omR[3], _tR[3];
......@@ -2013,8 +2009,6 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
CvMat dt3dt1 = cvMat(3, 3, CV_64F, _dt3dt1);
CvMat dt3dt2 = cvMat(3, 3, CV_64F, _dt3dt2);
CvMat om[2], T[2], imgpt_i[2];
CvMat dpdrot_hdr, dpdt_hdr, dpdf_hdr, dpdc_hdr, dpdk_hdr;
CvMat *dpdrot = &dpdrot_hdr, *dpdt = &dpdt_hdr, *dpdf = 0, *dpdc = 0, *dpdk = 0;
if( !solver.updateAlt( param, JtJ, JtErr, _errNorm ))
break;
......@@ -2028,9 +2022,7 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
{
double* iparam = solver.param->data.db + (nimages+1)*6;
double* ipparam = solver.prevParam->data.db + (nimages+1)*6;
dpdf = &dpdf_hdr;
dpdc = &dpdc_hdr;
dpdk = &dpdk_hdr;
if( flags & CALIB_SAME_FOCAL_LENGTH )
{
iparam[NINTRINSIC] = iparam[0];
......@@ -2083,44 +2075,43 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1] );
objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3);
err->rows = Je->rows = J_LR->rows = Ji->rows = ni*2;
cvReshape( err, &tmpimagePoints, 2, 1 );
err.resize(ni*2); Je.resize(ni*2); J_LR.resize(ni*2); Ji.resize(ni*2);
cvGetCols( Ji, &dpdf_hdr, 0, 2 );
cvGetCols( Ji, &dpdc_hdr, 2, 4 );
cvGetCols( Ji, &dpdk_hdr, 4, NINTRINSIC );
cvGetCols( Je, &dpdrot_hdr, 0, 3 );
cvGetCols( Je, &dpdt_hdr, 3, 6 );
CvMat tmpimagePoints(err.reshape(2, 1));
CvMat dpdf(Ji.colRange(0, 2));
CvMat dpdc(Ji.colRange(2, 4));
CvMat dpdk(Ji.colRange(4, NINTRINSIC));
CvMat dpdrot(Je.colRange(0, 3));
CvMat dpdt(Je.colRange(3, 6));
for( k = 0; k < 2; k++ )
{
double l2err;
imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2);
if( JtJ || JtErr )
cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k],
&tmpimagePoints, dpdrot, dpdt, dpdf, dpdc, dpdk,
&tmpimagePoints, &dpdrot, &dpdt, &dpdf, &dpdc, &dpdk,
(flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio[k] : 0);
else
cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k], &tmpimagePoints );
cvSub( &tmpimagePoints, &imgpt_i[k], &tmpimagePoints );
l2err = cvNorm( &tmpimagePoints, 0, CV_L2 );
if( JtJ || JtErr )
if( solver.state == CvLevMarq::CALC_J )
{
int iofs = (nimages+1)*6 + k*NINTRINSIC, eofs = (i+1)*6;
assert( JtJ && JtErr );
Mat _JtJ(cvarrToMat(JtJ)), _JtErr(cvarrToMat(JtErr));
if( k == 1 )
{
// d(err_{x|y}R) ~ de3
// convert de3/{dr3,dt3} => de3{dr1,dt1} & de3{dr2,dt2}
for( p = 0; p < ni*2; p++ )
{
CvMat de3dr3 = cvMat( 1, 3, CV_64F, Je->data.ptr + Je->step*p );
CvMat de3dr3 = cvMat( 1, 3, CV_64F, Je.ptr(p));
CvMat de3dt3 = cvMat( 1, 3, CV_64F, de3dr3.data.db + 3 );
CvMat de3dr2 = cvMat( 1, 3, CV_64F, J_LR->data.ptr + J_LR->step*p );
CvMat de3dr2 = cvMat( 1, 3, CV_64F, J_LR.ptr(p) );
CvMat de3dt2 = cvMat( 1, 3, CV_64F, de3dr2.data.db + 3 );
double _de3dr1[3], _de3dt1[3];
CvMat de3dr1 = cvMat( 1, 3, CV_64F, _de3dr1 );
......@@ -2138,39 +2129,27 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
cvCopy( &de3dt1, &de3dt3 );
}
cvGetSubRect( JtJ, &_part, cvRect(0, 0, 6, 6) );
cvGEMM( J_LR, J_LR, 1, &_part, 1, &_part, CV_GEMM_A_T );
cvGetSubRect( JtJ, &_part, cvRect(eofs, 0, 6, 6) );
cvGEMM( J_LR, Je, 1, 0, 0, &_part, CV_GEMM_A_T );
cvGetRows( JtErr, &_part, 0, 6 );
cvGEMM( J_LR, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
_JtJ(Rect(0, 0, 6, 6)) += J_LR.t()*J_LR;
_JtJ(Rect(eofs, 0, 6, 6)) = J_LR.t()*Je;
_JtErr.rowRange(0, 6) += J_LR.t()*err;
}
cvGetSubRect( JtJ, &_part, cvRect(eofs, eofs, 6, 6) );
cvGEMM( Je, Je, 1, &_part, 1, &_part, CV_GEMM_A_T );
cvGetRows( JtErr, &_part, eofs, eofs + 6 );
cvGEMM( Je, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
_JtJ(Rect(eofs, eofs, 6, 6)) += Je.t()*Je;
_JtErr.rowRange(eofs, eofs + 6) += Je.t()*err;
if( recomputeIntrinsics )
{
cvGetSubRect( JtJ, &_part, cvRect(iofs, iofs, NINTRINSIC, NINTRINSIC) );
cvGEMM( Ji, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
cvGetSubRect( JtJ, &_part, cvRect(iofs, eofs, NINTRINSIC, 6) );
cvGEMM( Je, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
_JtJ(Rect(iofs, iofs, NINTRINSIC, NINTRINSIC)) += Ji.t()*Ji;
_JtJ(Rect(iofs, eofs, NINTRINSIC, 6)) += Je.t()*Ji;
if( k == 1 )
{
cvGetSubRect( JtJ, &_part, cvRect(iofs, 0, NINTRINSIC, 6) );
cvGEMM( J_LR, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
_JtJ(Rect(iofs, 0, NINTRINSIC, 6)) += J_LR.t()*Ji;
}
cvGetRows( JtErr, &_part, iofs, iofs + NINTRINSIC );
cvGEMM( Ji, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
_JtErr.rowRange(iofs, iofs + NINTRINSIC) += Ji.t()*err;
}
}
reprojErr += l2err*l2err;
reprojErr += norm(err, NORM_L2SQR);
}
}
if(_errNorm)
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment