Skip to content

Commit 0e7edca

Browse files
alalekalalek
committed
Merge pull request opencv#9340 from paroj:stereocalib_cpp
2 parents 9e5a8ba + 5ada462 commit 0e7edca

File tree

1 file changed

+31
-52
lines changed

1 file changed

+31
-52
lines changed

‎modules/calib3d/src/calibration.cpp

Lines changed: 31 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -1766,16 +1766,16 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
17661766
CvTermCriteria termCrit )
17671767
{
17681768
const int NINTRINSIC = 18;
1769-
Ptr<CvMat> npoints, err, J_LR, Je, Ji, imagePoints[2], objectPoints, RT0;
1769+
Ptr<CvMat> npoints, imagePoints[2], objectPoints, RT0;
17701770
double reprojErr = 0;
17711771

1772-
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];
1772+
double A[2][9], dk[2][14]={{0}}, rlr[9];
17731773
CvMat K[2], Dist[2], om_LR, T_LR;
17741774
CvMat R_LR = cvMat(3, 3, CV_64F, rlr);
17751775
int i, k, p, ni = 0, ofs, nimages, pointsTotal, maxPoints = 0;
17761776
int nparams;
17771777
bool recomputeIntrinsics = false;
1778-
double aspectRatio[2] = {0,0};
1778+
double aspectRatio[2] = {0};
17791779

17801780
CV_Assert( CV_IS_MAT(_imagePoints1) && CV_IS_MAT(_imagePoints2) &&
17811781
CV_IS_MAT(_objectPoints) && CV_IS_MAT(_npoints) &&
@@ -1855,11 +1855,10 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
18551855

18561856
recomputeIntrinsics = (flags & CALIB_FIX_INTRINSIC) == 0;
18571857

1858-
err.reset(cvCreateMat( maxPoints*2, 1, CV_64F ));
1859-
Je.reset(cvCreateMat( maxPoints*2, 6, CV_64F ));
1860-
J_LR.reset(cvCreateMat( maxPoints*2, 6, CV_64F ));
1861-
Ji.reset(cvCreateMat( maxPoints*2, NINTRINSIC, CV_64F ));
1862-
cvZero( Ji );
1858+
Mat err( maxPoints*2, 1, CV_64F );
1859+
Mat Je( maxPoints*2, 6, CV_64F );
1860+
Mat J_LR( maxPoints*2, 6, CV_64F );
1861+
Mat Ji( maxPoints*2, NINTRINSIC, CV_64F, Scalar(0) );
18631862

18641863
// we optimize for the inter-camera R(3),t(3), then, optionally,
18651864
// 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
19431942
R[k] = cvMat(3, 3, CV_64F, r[k]);
19441943
T[k] = cvMat(3, 1, CV_64F, t[k]);
19451944

1946-
// FIXME: here we ignore activePoints[k] because of
1947-
// the limited API of cvFindExtrnisicCameraParams2
19481945
cvFindExtrinsicCameraParams2( &objpt_i, &imgpt_i[k], &K[k], &Dist[k], &om[k], &T[k] );
19491946
cvRodrigues2( &om[k], &R[k] );
19501947
if( k == 0 )
@@ -2001,7 +1998,6 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
20011998
for(;;)
20021999
{
20032000
const CvMat* param = 0;
2004-
CvMat tmpimagePoints;
20052001
CvMat *JtJ = 0, *JtErr = 0;
20062002
double *_errNorm = 0;
20072003
double _omR[3], _tR[3];
@@ -2013,8 +2009,6 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
20132009
CvMat dt3dt1 = cvMat(3, 3, CV_64F, _dt3dt1);
20142010
CvMat dt3dt2 = cvMat(3, 3, CV_64F, _dt3dt2);
20152011
CvMat om[2], T[2], imgpt_i[2];
2016-
CvMat dpdrot_hdr, dpdt_hdr, dpdf_hdr, dpdc_hdr, dpdk_hdr;
2017-
CvMat *dpdrot = &dpdrot_hdr, *dpdt = &dpdt_hdr, *dpdf = 0, *dpdc = 0, *dpdk = 0;
20182012

20192013
if( !solver.updateAlt( param, JtJ, JtErr, _errNorm ))
20202014
break;
@@ -2028,9 +2022,7 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
20282022
{
20292023
double* iparam = solver.param->data.db + (nimages+1)*6;
20302024
double* ipparam = solver.prevParam->data.db + (nimages+1)*6;
2031-
dpdf = &dpdf_hdr;
2032-
dpdc = &dpdc_hdr;
2033-
dpdk = &dpdk_hdr;
2025+
20342026
if( flags & CALIB_SAME_FOCAL_LENGTH )
20352027
{
20362028
iparam[NINTRINSIC] = iparam[0];
@@ -2083,44 +2075,43 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
20832075
cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1] );
20842076

20852077
objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3);
2086-
err->rows = Je->rows = J_LR->rows = Ji->rows = ni*2;
2087-
cvReshape( err, &tmpimagePoints, 2, 1 );
2078+
err.resize(ni*2); Je.resize(ni*2); J_LR.resize(ni*2); Ji.resize(ni*2);
20882079

2089-
cvGetCols( Ji, &dpdf_hdr, 0, 2 );
2090-
cvGetCols( Ji, &dpdc_hdr, 2, 4 );
2091-
cvGetCols( Ji, &dpdk_hdr, 4, NINTRINSIC );
2092-
cvGetCols( Je, &dpdrot_hdr, 0, 3 );
2093-
cvGetCols( Je, &dpdt_hdr, 3, 6 );
2080+
CvMat tmpimagePoints(err.reshape(2, 1));
2081+
CvMat dpdf(Ji.colRange(0, 2));
2082+
CvMat dpdc(Ji.colRange(2, 4));
2083+
CvMat dpdk(Ji.colRange(4, NINTRINSIC));
2084+
CvMat dpdrot(Je.colRange(0, 3));
2085+
CvMat dpdt(Je.colRange(3, 6));
20942086

20952087
for( k = 0; k < 2; k++ )
20962088
{
2097-
double l2err;
20982089
imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2);
20992090

21002091
if( JtJ || JtErr )
21012092
cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k],
2102-
&tmpimagePoints, dpdrot, dpdt, dpdf, dpdc, dpdk,
2093+
&tmpimagePoints, &dpdrot, &dpdt, &dpdf, &dpdc, &dpdk,
21032094
(flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio[k] : 0);
21042095
else
21052096
cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k], &tmpimagePoints );
21062097
cvSub( &tmpimagePoints, &imgpt_i[k], &tmpimagePoints );
21072098

2108-
l2err = cvNorm( &tmpimagePoints, 0, CV_L2 );
2109-
2110-
if( JtJ || JtErr )
2099+
if( solver.state == CvLevMarq::CALC_J )
21112100
{
21122101
int iofs = (nimages+1)*6 + k*NINTRINSIC, eofs = (i+1)*6;
21132102
assert( JtJ && JtErr );
21142103

2104+
Mat _JtJ(cvarrToMat(JtJ)), _JtErr(cvarrToMat(JtErr));
2105+
21152106
if( k == 1 )
21162107
{
21172108
// d(err_{x|y}R) ~ de3
21182109
// convert de3/{dr3,dt3} => de3{dr1,dt1} & de3{dr2,dt2}
21192110
for( p = 0; p < ni*2; p++ )
21202111
{
2121-
CvMat de3dr3 = cvMat( 1, 3, CV_64F, Je->data.ptr + Je->step*p );
2112+
CvMat de3dr3 = cvMat( 1, 3, CV_64F, Je.ptr(p));
21222113
CvMat de3dt3 = cvMat( 1, 3, CV_64F, de3dr3.data.db + 3 );
2123-
CvMat de3dr2 = cvMat( 1, 3, CV_64F, J_LR->data.ptr + J_LR->step*p );
2114+
CvMat de3dr2 = cvMat( 1, 3, CV_64F, J_LR.ptr(p) );
21242115
CvMat de3dt2 = cvMat( 1, 3, CV_64F, de3dr2.data.db + 3 );
21252116
double _de3dr1[3], _de3dt1[3];
21262117
CvMat de3dr1 = cvMat( 1, 3, CV_64F, _de3dr1 );
@@ -2138,39 +2129,27 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
21382129
cvCopy( &de3dt1, &de3dt3 );
21392130
}
21402131

2141-
cvGetSubRect( JtJ, &_part, cvRect(0, 0, 6, 6) );
2142-
cvGEMM( J_LR, J_LR, 1, &_part, 1, &_part, CV_GEMM_A_T );
2143-
2144-
cvGetSubRect( JtJ, &_part, cvRect(eofs, 0, 6, 6) );
2145-
cvGEMM( J_LR, Je, 1, 0, 0, &_part, CV_GEMM_A_T );
2146-
2147-
cvGetRows( JtErr, &_part, 0, 6 );
2148-
cvGEMM( J_LR, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
2132+
_JtJ(Rect(0, 0, 6, 6)) += J_LR.t()*J_LR;
2133+
_JtJ(Rect(eofs, 0, 6, 6)) = J_LR.t()*Je;
2134+
_JtErr.rowRange(0, 6) += J_LR.t()*err;
21492135
}
21502136

2151-
cvGetSubRect( JtJ, &_part, cvRect(eofs, eofs, 6, 6) );
2152-
cvGEMM( Je, Je, 1, &_part, 1, &_part, CV_GEMM_A_T );
2153-
2154-
cvGetRows( JtErr, &_part, eofs, eofs + 6 );
2155-
cvGEMM( Je, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
2137+
_JtJ(Rect(eofs, eofs, 6, 6)) += Je.t()*Je;
2138+
_JtErr.rowRange(eofs, eofs + 6) += Je.t()*err;
21562139

21572140
if( recomputeIntrinsics )
21582141
{
2159-
cvGetSubRect( JtJ, &_part, cvRect(iofs, iofs, NINTRINSIC, NINTRINSIC) );
2160-
cvGEMM( Ji, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
2161-
cvGetSubRect( JtJ, &_part, cvRect(iofs, eofs, NINTRINSIC, 6) );
2162-
cvGEMM( Je, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
2142+
_JtJ(Rect(iofs, iofs, NINTRINSIC, NINTRINSIC)) += Ji.t()*Ji;
2143+
_JtJ(Rect(iofs, eofs, NINTRINSIC, 6)) += Je.t()*Ji;
21632144
if( k == 1 )
21642145
{
2165-
cvGetSubRect( JtJ, &_part, cvRect(iofs, 0, NINTRINSIC, 6) );
2166-
cvGEMM( J_LR, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
2146+
_JtJ(Rect(iofs, 0, NINTRINSIC, 6)) += J_LR.t()*Ji;
21672147
}
2168-
cvGetRows( JtErr, &_part, iofs, iofs + NINTRINSIC );
2169-
cvGEMM( Ji, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
2148+
_JtErr.rowRange(iofs, iofs + NINTRINSIC) += Ji.t()*err;
21702149
}
21712150
}
21722151

2173-
reprojErr += l2err*l2err;
2152+
reprojErr += norm(err, NORM_L2SQR);
21742153
}
21752154
}
21762155
if(_errNorm)

0 commit comments

Comments
 (0)