diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index 31569282f9..0e2b9042db 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -1766,16 +1766,16 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1 CvTermCriteria termCrit ) { const int NINTRINSIC = 18; - Ptr npoints, err, J_LR, Je, Ji, imagePoints[2], objectPoints, RT0; + Ptr 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)