From eff6d323372079e0d30338f47f11f162d723603f Mon Sep 17 00:00:00 2001 From: Vadim Pisarevsky Date: Mon, 7 Jun 2021 20:55:25 +0800 Subject: [PATCH] * refactored the remaining old-style functions in 3d and calib modules to use the new C++ API. * extended C++ version of Levenberg-Marquardt (LM) solver to accommodate all features of the C counterpart. * removed C version of LM solver * made a few other little changes to make the code compile and run smoothly --- modules/3d/include/opencv2/3d.hpp | 64 +- modules/3d/src/calibration_base.cpp | 1253 ++++----- modules/3d/src/compat_ptsetreg.cpp | 322 --- modules/3d/src/epnp.cpp | 181 +- modules/3d/src/epnp.h | 17 +- modules/3d/src/levmarq.cpp | 305 ++- modules/3d/src/triangulate.cpp | 481 ++-- modules/3d/test/test_solvepnp_ransac.cpp | 21 +- modules/calib/include/opencv2/calib.hpp | 12 +- modules/calib/src/calibration.cpp | 2401 ++++------------- modules/calib/test/test_cameracalibration.cpp | 4 +- .../test/test_cameracalibration_badarg.cpp | 2 - modules/core/include/opencv2/core/mat.hpp | 5 + modules/core/include/opencv2/core/matx.hpp | 8 +- .../core/include/opencv2/core/operations.hpp | 10 + modules/core/include/opencv2/core/types.hpp | 21 + modules/core/src/copy.cpp | 19 + modules/core/src/matrix_wrap.cpp | 16 + modules/features2d/test/test_keypoints.cpp | 1 - modules/stitching/src/motion_estimators.cpp | 48 +- 20 files changed, 1583 insertions(+), 3608 deletions(-) delete mode 100644 modules/3d/src/compat_ptsetreg.cpp diff --git a/modules/3d/include/opencv2/3d.hpp b/modules/3d/include/opencv2/3d.hpp index 6984b705a2..59298686d2 100644 --- a/modules/3d/include/opencv2/3d.hpp +++ b/modules/3d/include/opencv2/3d.hpp @@ -522,6 +522,14 @@ public: */ static Ptr create(const Ptr& cb, int maxIters); static Ptr create(const Ptr& cb, int maxIters, double eps); + + static int run(InputOutputArray param, InputArray mask, + int nerrs, const TermCriteria& termcrit, int solveMethod, + std::function callb); + static int runAlt(InputOutputArray param, InputArray mask, + const TermCriteria& termcrit, int solveMethod, bool LtoR, + std::function callb); }; /** @example samples/cpp/tutorial_code/features2D/Homography/pose_from_homography.cpp @@ -745,7 +753,17 @@ CV_EXPORTS_W void projectPoints( InputArray objectPoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray imagePoints, OutputArray jacobian = noArray(), - double aspectRatio = 0 ); + double aspectRatio = 0); + +/** @overload */ +CV_EXPORTS_AS(projectPointsSepJ) void projectPoints( + InputArray objectPoints, + InputArray rvec, InputArray tvec, + InputArray cameraMatrix, InputArray distCoeffs, + OutputArray imagePoints, OutputArray dpdr, + OutputArray dpdt, OutputArray dpdf=noArray(), + OutputArray dpdc=noArray(), OutputArray dpdk=noArray(), + OutputArray dpdo=noArray(), double aspectRatio=0.); /** @example samples/cpp/tutorial_code/features2D/Homography/homography_from_camera_displacement.cpp An example program about homography from the camera displacement @@ -1961,13 +1979,13 @@ correctly only when there are more than 50% of inliers. @sa estimateAffinePartial2D, getAffineTransform */ -CV_EXPORTS_W cv::Mat estimateAffine2D(InputArray from, InputArray to, OutputArray inliers = noArray(), +CV_EXPORTS_W Mat estimateAffine2D(InputArray from, InputArray to, OutputArray inliers = noArray(), int method = RANSAC, double ransacReprojThreshold = 3, size_t maxIters = 2000, double confidence = 0.99, size_t refineIters = 10); -CV_EXPORTS_W cv::Mat estimateAffine2D(InputArray pts1, InputArray pts2, OutputArray inliers, +CV_EXPORTS_W Mat estimateAffine2D(InputArray pts1, InputArray pts2, OutputArray inliers, const UsacParams ¶ms); /** @brief Computes an optimal limited affine transformation with 4 degrees of freedom between @@ -2329,46 +2347,6 @@ void undistortPoints(InputArray src, OutputArray dst, InputArray R = noArray(), InputArray P = noArray(), TermCriteria criteria=TermCriteria(TermCriteria::MAX_ITER, 5, 0.01)); -////////////////////////////////////////////////////////////////////////////////////////// - -// the old-style Levenberg-Marquardt solver; to be removed soon -class CV_EXPORTS CvLevMarq -{ -public: - CvLevMarq(); - CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria= - cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON), - bool completeSymmFlag=false ); - ~CvLevMarq(); - void init( int nparams, int nerrs, CvTermCriteria criteria= - cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON), - bool completeSymmFlag=false ); - bool update( const CvMat*& param, CvMat*& J, CvMat*& err ); - bool updateAlt( const CvMat*& param, CvMat*& JtJ, CvMat*& JtErr, double*& errNorm ); - - void clear(); - void step(); - enum { DONE=0, STARTED=1, CALC_J=2, CHECK_ERR=3 }; - - cv::Ptr mask; - cv::Ptr prevParam; - cv::Ptr param; - cv::Ptr J; - cv::Ptr err; - cv::Ptr JtJ; - cv::Ptr JtJN; - cv::Ptr JtErr; - cv::Ptr JtJV; - cv::Ptr JtJW; - double prevErrNorm, errNorm; - int lambdaLg10; - CvTermCriteria criteria; - int state; - int iters; - bool completeSymmFlag; - int solveMethod; -}; - //! @} _3d } //end namespace cv diff --git a/modules/3d/src/calibration_base.cpp b/modules/3d/src/calibration_base.cpp index 77a2f3cba1..860ecd7510 100644 --- a/modules/3d/src/calibration_base.cpp +++ b/modules/3d/src/calibration_base.cpp @@ -57,158 +57,122 @@ using namespace cv; */ // reimplementation of dAB.m -static void cvCalcMatMulDeriv( const CvMat* A, const CvMat* B, CvMat* dABdA, CvMat* dABdB ) +void cv::matMulDeriv( InputArray A_, InputArray B_, OutputArray dABdA_, OutputArray dABdB_ ) { - int i, j, M, N, L; - int bstep; + CV_INSTRUMENT_REGION(); - CV_Assert( CV_IS_MAT(A) && CV_IS_MAT(B) ); - CV_Assert( CV_ARE_TYPES_EQ(A, B) && - (CV_MAT_TYPE(A->type) == CV_32F || CV_MAT_TYPE(A->type) == CV_64F) ); - CV_Assert( A->cols == B->rows ); + Mat A = A_.getMat(), B = B_.getMat(); + int type = A.type(); + CV_Assert(type == B.type()); + CV_Assert(type == CV_32F || type == CV_64F); + CV_Assert(A.cols == B.rows); - M = A->rows; - L = A->cols; - N = B->cols; - bstep = B->step/CV_ELEM_SIZE(B->type); + dABdA_.create(A.rows*B.cols, A.rows*A.cols, type); + dABdB_.create(A.rows*B.cols, B.rows*B.cols, type); + Mat dABdA = dABdA_.getMat(), dABdB = dABdB_.getMat(); - if( dABdA ) + int M = A.rows, L = A.cols, N = B.cols; + int bstep = (int)(B.step/B.elemSize()); + + if( type == CV_32F ) { - CV_Assert( CV_ARE_TYPES_EQ(A, dABdA) && - dABdA->rows == A->rows*B->cols && dABdA->cols == A->rows*A->cols ); - } - - if( dABdB ) - { - CV_Assert( CV_ARE_TYPES_EQ(A, dABdB) && - dABdB->rows == A->rows*B->cols && dABdB->cols == B->rows*B->cols ); - } - - if( CV_MAT_TYPE(A->type) == CV_32F ) - { - for( i = 0; i < M*N; i++ ) + for( int i = 0; i < M*N; i++ ) { - int i1 = i / N, i2 = i % N; + int j, i1 = i / N, i2 = i % N; - if( dABdA ) + const float* a = A.ptr(i1); + const float* b = B.ptr() + i2; + float* dcda = dABdA.ptr(i); + float* dcdb = dABdB.ptr(i); + + memset(dcda, 0, M*L*sizeof(dcda[0])); + memset(dcdb, 0, L*N*sizeof(dcdb[0])); + + for( j = 0; j < L; j++ ) { - float* dcda = (float*)(dABdA->data.ptr + dABdA->step*i); - const float* b = (const float*)B->data.ptr + i2; - - for( j = 0; j < M*L; j++ ) - dcda[j] = 0; - for( j = 0; j < L; j++ ) - dcda[i1*L + j] = b[j*bstep]; - } - - if( dABdB ) - { - float* dcdb = (float*)(dABdB->data.ptr + dABdB->step*i); - const float* a = (const float*)(A->data.ptr + A->step*i1); - - for( j = 0; j < L*N; j++ ) - dcdb[j] = 0; - for( j = 0; j < L; j++ ) - dcdb[j*N + i2] = a[j]; + dcda[i1*L + j] = b[j*bstep]; + dcdb[j*N + i2] = a[j]; } } } else { - for( i = 0; i < M*N; i++ ) + for( int i = 0; i < M*N; i++ ) { - int i1 = i / N, i2 = i % N; + int j, i1 = i / N, i2 = i % N; - if( dABdA ) + const double* a = A.ptr(i1); + const double* b = B.ptr() + i2; + double* dcda = dABdA.ptr(i); + double* dcdb = dABdB.ptr(i); + + memset(dcda, 0, M*L*sizeof(dcda[0])); + memset(dcdb, 0, L*N*sizeof(dcdb[0])); + + for( j = 0; j < L; j++ ) { - double* dcda = (double*)(dABdA->data.ptr + dABdA->step*i); - const double* b = (const double*)B->data.ptr + i2; - - for( j = 0; j < M*L; j++ ) - dcda[j] = 0; - for( j = 0; j < L; j++ ) - dcda[i1*L + j] = b[j*bstep]; - } - - if( dABdB ) - { - double* dcdb = (double*)(dABdB->data.ptr + dABdB->step*i); - const double* a = (const double*)(A->data.ptr + A->step*i1); - - for( j = 0; j < L*N; j++ ) - dcdb[j] = 0; - for( j = 0; j < L; j++ ) - dcdb[j*N + i2] = a[j]; + dcda[i1*L + j] = b[j*bstep]; + dcdb[j*N + i2] = a[j]; } } } } -static int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian=0 ) +void cv::Rodrigues(InputArray _src, OutputArray _dst, OutputArray _jacobian) { + CV_INSTRUMENT_REGION(); + + Mat src = _src.getMat(); + const Size srcSz = src.size(); + int srccn = src.channels(); + int depth = src.depth(); + CV_Check(srcSz, ((srcSz == Size(3, 1) || srcSz == Size(1, 3)) && srccn == 1) || + (srcSz == Size(1, 1) && srccn == 3) || + (srcSz == Size(3, 3) && srccn == 1), + "Input matrix must be 1x3 or 3x1 for a rotation vector, or 3x3 for a rotation matrix"); + + bool v2m = src.cols == 1 || src.rows == 1; + _dst.create(3, v2m ? 3 : 1, depth); + Mat dst = _dst.getMat(), jacobian; + if( _jacobian.needed() ) + { + _jacobian.create(v2m ? Size(9, 3) : Size(3, 9), src.depth()); + jacobian = _jacobian.getMat(); + } + double J[27] = {0}; - CvMat matJ = cvMat( 3, 9, CV_64F, J ); + Mat matJ( 3, 9, CV_64F, J); - if( !CV_IS_MAT(src) ) - CV_Error( !src ? CV_StsNullPtr : CV_StsBadArg, "Input argument is not a valid matrix" ); - - if( !CV_IS_MAT(dst) ) - CV_Error( !dst ? CV_StsNullPtr : CV_StsBadArg, - "The first output argument is not a valid matrix" ); - - int depth = CV_MAT_DEPTH(src->type); - int elem_size = CV_ELEM_SIZE(depth); + dst.setZero(); if( depth != CV_32F && depth != CV_64F ) CV_Error( CV_StsUnsupportedFormat, "The matrices must have 32f or 64f data type" ); - if( !CV_ARE_DEPTHS_EQ(src, dst) ) - CV_Error( CV_StsUnmatchedFormats, "All the matrices must have the same data type" ); - - if( jacobian ) + if( v2m ) { - if( !CV_IS_MAT(jacobian) ) - CV_Error( CV_StsBadArg, "Jacobian is not a valid matrix" ); - - if( !CV_ARE_DEPTHS_EQ(src, jacobian) || CV_MAT_CN(jacobian->type) != 1 ) - CV_Error( CV_StsUnmatchedFormats, "Jacobian must have 32fC1 or 64fC1 datatype" ); - - if( (jacobian->rows != 9 || jacobian->cols != 3) && - (jacobian->rows != 3 || jacobian->cols != 9)) - CV_Error( CV_StsBadSize, "Jacobian must be 3x9 or 9x3" ); - } - - if( src->cols == 1 || src->rows == 1 ) - { - int step = src->rows > 1 ? src->step / elem_size : 1; - - if( src->rows + src->cols*CV_MAT_CN(src->type) - 1 != 3 ) - CV_Error( CV_StsBadSize, "Input matrix must be 1x3, 3x1 or 3x3" ); - - if( dst->rows != 3 || dst->cols != 3 || CV_MAT_CN(dst->type) != 1 ) - CV_Error( CV_StsBadSize, "Output matrix must be 3x3, single-channel floating point matrix" ); + int sstep = src.rows > 1 ? (int)src.step1() : 1; Point3d r; if( depth == CV_32F ) { - r.x = src->data.fl[0]; - r.y = src->data.fl[step]; - r.z = src->data.fl[step*2]; + const float* sptr = src.ptr(); + r.x = sptr[0]; + r.y = sptr[sstep]; + r.z = sptr[sstep*2]; } else { - r.x = src->data.db[0]; - r.y = src->data.db[step]; - r.z = src->data.db[step*2]; + const double* sptr = src.ptr(); + r.x = sptr[0]; + r.y = sptr[sstep]; + r.z = sptr[sstep*2]; } double theta = norm(r); - if( theta < DBL_EPSILON ) { - cvSetIdentity( dst ); - - if( jacobian ) + dst = Mat::eye(3, 3, depth); + if( jacobian.data ) { memset( J, 0, sizeof(J) ); J[5] = J[15] = J[19] = -1; @@ -231,10 +195,9 @@ static int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian=0 ) // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] Matx33d R = c*Matx33d::eye() + c1*rrt + s*r_x; + R.convertTo(dst, depth); - Mat(R).convertTo(cvarrToMat(dst), dst->type); - - if( jacobian ) + if( jacobian.data ) { const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; double drrt[] = { r.x+r.x, r.y, r.z, r.y, 0, 0, r.z, 0, 0, @@ -255,25 +218,22 @@ static int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian=0 ) } } } - else if( src->cols == 3 && src->rows == 3 ) + else { Matx33d U, Vt; Vec3d W; double theta, s, c; - int step = dst->rows > 1 ? dst->step / elem_size : 1; + int dstep = dst.rows > 1 ? (int)dst.step1() : 1; - if( (dst->rows != 1 || dst->cols*CV_MAT_CN(dst->type) != 3) && - (dst->rows != 3 || dst->cols != 1 || CV_MAT_CN(dst->type) != 1)) - CV_Error( CV_StsBadSize, "Output matrix must be 1x3 or 3x1" ); - - Matx33d R = cvarrToMat(src); + Matx33d R; + src.convertTo(R, CV_64F); if( !checkRange(R, true, NULL, -100, 100) ) { - cvZero(dst); - if( jacobian ) - cvZero(jacobian); - return 0; + dst.setZero(); + if (jacobian.data) + jacobian.setZero(); + return; } SVD::compute(R, W, U, Vt); @@ -306,7 +266,7 @@ static int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian=0 ) r *= theta; } - if( jacobian ) + if( jacobian.data ) { memset( J, 0, sizeof(J) ); if( c > 0 ) @@ -320,7 +280,7 @@ static int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian=0 ) { double vth = 1/(2*s); - if( jacobian ) + if( jacobian.data ) { double t, dtheta_dtr = -1./s; // var1 = [vth;theta] @@ -352,14 +312,15 @@ static int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian=0 ) 0, 0, theta, r.z*vth }; - CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR ); - CvMat _dvar2dvar = cvMat( 4, 5, CV_64FC1, dvar2dvar ); - CvMat _domegadvar2 = cvMat( 3, 4, CV_64FC1, domegadvar2 ); + Mat _dvardR( 5, 9, CV_64FC1, dvardR ); + Mat _dvar2dvar( 4, 5, CV_64FC1, dvar2dvar ); + Mat _domegadvar2( 3, 4, CV_64FC1, domegadvar2 ); double t0[3*5]; - CvMat _t0 = cvMat( 3, 5, CV_64FC1, t0 ); + Mat _t0( 3, 5, CV_64FC1, t0 ); - cvMatMul( &_domegadvar2, &_dvar2dvar, &_t0 ); - cvMatMul( &_t0, &_dvardR, &matJ ); + gemm(_domegadvar2, _dvar2dvar, 1, noArray(), 0, _t0); + gemm(_t0, _dvardR, 1, noArray(), 0, matJ); + CV_Assert(matJ.ptr() == J); // transpose every row of matJ (treat the rows as 3x3 matrices) CV_SWAP(J[1], J[3], t); CV_SWAP(J[2], J[6], t); CV_SWAP(J[5], J[7], t); @@ -373,401 +334,328 @@ static int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian=0 ) if( depth == CV_32F ) { - dst->data.fl[0] = (float)r.x; - dst->data.fl[step] = (float)r.y; - dst->data.fl[step*2] = (float)r.z; + float* dptr = dst.ptr(); + dptr[0] = (float)r.x; + dptr[dstep] = (float)r.y; + dptr[dstep*2] = (float)r.z; } else { - dst->data.db[0] = r.x; - dst->data.db[step] = r.y; - dst->data.db[step*2] = r.z; + double* dptr = dst.ptr(); + dptr[0] = r.x; + dptr[dstep] = r.y; + dptr[dstep*2] = r.z; } } - else - { - CV_Error(CV_StsBadSize, "Input matrix must be 1x3 or 3x1 for a rotation vector, or 3x3 for a rotation matrix"); - } - if( jacobian ) + if( jacobian.data ) { if( depth == CV_32F ) { - if( jacobian->rows == matJ.rows ) - cvConvert( &matJ, jacobian ); + if( jacobian.rows == matJ.rows ) + matJ.convertTo(jacobian, CV_32F); else { float Jf[3*9]; - CvMat _Jf = cvMat( matJ.rows, matJ.cols, CV_32FC1, Jf ); - cvConvert( &matJ, &_Jf ); - cvTranspose( &_Jf, jacobian ); + Mat _Jf( matJ.rows, matJ.cols, CV_32FC1, Jf ); + matJ.convertTo(_Jf, CV_32F); + transpose(_Jf, jacobian); } } - else if( jacobian->rows == matJ.rows ) - cvCopy( &matJ, jacobian ); + else if( jacobian.rows == matJ.rows ) + matJ.copyTo(jacobian); else - cvTranspose( &matJ, jacobian ); + transpose(matJ, jacobian); } - - return 1; } // reimplementation of compose_motion.m -static void cvComposeRT( const CvMat* _rvec1, const CvMat* _tvec1, - const CvMat* _rvec2, const CvMat* _tvec2, - CvMat* _rvec3, CvMat* _tvec3, - CvMat* dr3dr1, CvMat* dr3dt1, - CvMat* dr3dr2, CvMat* dr3dt2, - CvMat* dt3dr1, CvMat* dt3dt1, - CvMat* dt3dr2, CvMat* dt3dt2 ) +void cv::composeRT( InputArray _rvec1, InputArray _tvec1, + InputArray _rvec2, InputArray _tvec2, + OutputArray _rvec3, OutputArray _tvec3, + OutputArray _dr3dr1, OutputArray _dr3dt1, + OutputArray _dr3dr2, OutputArray _dr3dt2, + OutputArray _dt3dr1, OutputArray _dt3dt1, + OutputArray _dt3dr2, OutputArray _dt3dt2 ) { + Mat rvec1 = _rvec1.getMat(), tvec1 = _tvec1.getMat(); + Mat rvec2 = _rvec2.getMat(), tvec2 = _tvec2.getMat(); + int rtype = rvec1.type(); + + CV_Assert(rtype == CV_32F || rtype == CV_64F); + Size rsz = rvec1.size(); + CV_Assert(rsz == Size(3, 1) || rsz == Size(1, 3)); + CV_Assert(rsz == rvec2.size() && rsz == tvec1.size() && rsz == tvec2.size()); + + Mat dr3dr1, dr3dt1, dr3dr2, dr3dt2; + Mat dt3dr1, dt3dt1, dt3dr2, dt3dt2; + if(_dr3dr1.needed()) { + _dr3dr1.create(3, 3, rtype); + dr3dr1 = _dr3dr1.getMat(); + } + if(_dr3dt1.needed()) { + _dr3dt1.create(3, 3, rtype); + dr3dt1 = _dr3dt1.getMat(); + } + if(_dr3dr2.needed()) { + _dr3dr2.create(3, 3, rtype); + dr3dr2 = _dr3dr2.getMat(); + } + if(_dr3dt2.needed()) { + _dr3dt2.create(3, 3, rtype); + dr3dt2 = _dr3dt2.getMat(); + } + if(_dt3dr1.needed()) { + _dt3dr1.create(3, 3, rtype); + dt3dr1 = _dt3dr1.getMat(); + } + if(_dt3dt1.needed()) { + _dt3dt1.create(3, 3, rtype); + dt3dt1 = _dt3dt1.getMat(); + } + if(_dt3dr2.needed()) { + _dt3dr2.create(3, 3, rtype); + dt3dr2 = _dt3dr2.getMat(); + } + if(_dt3dt2.needed()) { + _dt3dt2.create(3, 3, rtype); + dt3dt2 = _dt3dt2.getMat(); + } + double _r1[3], _r2[3]; double _R1[9], _d1[9*3], _R2[9], _d2[9*3]; - CvMat r1 = cvMat(3,1,CV_64F,_r1), r2 = cvMat(3,1,CV_64F,_r2); - CvMat R1 = cvMat(3,3,CV_64F,_R1), R2 = cvMat(3,3,CV_64F,_R2); - CvMat dR1dr1 = cvMat(9,3,CV_64F,_d1), dR2dr2 = cvMat(9,3,CV_64F,_d2); + Mat r1(rsz,CV_64F,_r1), r2(rsz,CV_64F,_r2); + Mat R1(3,3,CV_64F,_R1), R2(3,3,CV_64F,_R2); + Mat dR1dr1(3,9,CV_64F,_d1), dR2dr2(3,9,CV_64F,_d2); - CV_Assert( CV_IS_MAT(_rvec1) && CV_IS_MAT(_rvec2) ); + rvec1.convertTo(r1, CV_64F); + rvec2.convertTo(r2, CV_64F); - CV_Assert( CV_MAT_TYPE(_rvec1->type) == CV_32F || - CV_MAT_TYPE(_rvec1->type) == CV_64F ); + Rodrigues(r1, R1, dR1dr1); + Rodrigues(r2, R2, dR2dr2); + CV_Assert(dR1dr1.ptr() == _d1); + CV_Assert(dR2dr2.ptr() == _d2); - CV_Assert( _rvec1->rows == 3 && _rvec1->cols == 1 && CV_ARE_SIZES_EQ(_rvec1, _rvec2) ); + double _r3[3], _R3[9], _dR3dR1[9*9], _dR3dR2[9*9], _dr3dR3[9*3]; + double _W1[9*3], _W2[3*3]; + Mat r3(3,1,CV_64F,_r3), R3(3,3,CV_64F,_R3); + Mat dR3dR1(9,9,CV_64F,_dR3dR1), dR3dR2(9,9,CV_64F,_dR3dR2); + Mat dr3dR3(9,3,CV_64F,_dr3dR3); + Mat W1(3,9,CV_64F,_W1), W2(3,3,CV_64F,_W2); - cvConvert( _rvec1, &r1 ); - cvConvert( _rvec2, &r2 ); + R3 = R2*R1; + matMulDeriv(R2, R1, dR3dR2, dR3dR1); + Rodrigues(R3, r3, dr3dR3); + CV_Assert(dr3dR3.ptr() == _dr3dR3); - cvRodrigues2( &r1, &R1, &dR1dr1 ); - cvRodrigues2( &r2, &R2, &dR2dr2 ); + r3.convertTo(_rvec3, rtype); - if( _rvec3 || dr3dr1 || dr3dr2 ) + if( dr3dr1.data ) { - double _r3[3], _R3[9], _dR3dR1[9*9], _dR3dR2[9*9], _dr3dR3[9*3]; - double _W1[9*3], _W2[3*3]; - CvMat r3 = cvMat(3,1,CV_64F,_r3), R3 = cvMat(3,3,CV_64F,_R3); - CvMat dR3dR1 = cvMat(9,9,CV_64F,_dR3dR1), dR3dR2 = cvMat(9,9,CV_64F,_dR3dR2); - CvMat dr3dR3 = cvMat(3,9,CV_64F,_dr3dR3); - CvMat W1 = cvMat(3,9,CV_64F,_W1), W2 = cvMat(3,3,CV_64F,_W2); - - cvMatMul( &R2, &R1, &R3 ); - cvCalcMatMulDeriv( &R2, &R1, &dR3dR2, &dR3dR1 ); - - cvRodrigues2( &R3, &r3, &dr3dR3 ); - - if( _rvec3 ) - cvConvert( &r3, _rvec3 ); - - if( dr3dr1 ) - { - cvMatMul( &dr3dR3, &dR3dR1, &W1 ); - cvMatMul( &W1, &dR1dr1, &W2 ); - cvConvert( &W2, dr3dr1 ); - } - - if( dr3dr2 ) - { - cvMatMul( &dr3dR3, &dR3dR2, &W1 ); - cvMatMul( &W1, &dR2dr2, &W2 ); - cvConvert( &W2, dr3dr2 ); - } + gemm(dr3dR3, dR3dR1, 1, noArray(), 0, W1, GEMM_1_T); + gemm(W1, dR1dr1, 1, noArray(), 0, W2, GEMM_2_T); + W2.convertTo(dr3dr1, rtype); } - if( dr3dt1 ) - cvZero( dr3dt1 ); - if( dr3dt2 ) - cvZero( dr3dt2 ); - - if( _tvec3 || dt3dr2 || dt3dt1 ) + if( dr3dr2.data ) { - double _t1[3], _t2[3], _t3[3], _dxdR2[3*9], _dxdt1[3*3], _W3[3*3]; - CvMat t1 = cvMat(3,1,CV_64F,_t1), t2 = cvMat(3,1,CV_64F,_t2); - CvMat t3 = cvMat(3,1,CV_64F,_t3); - CvMat dxdR2 = cvMat(3, 9, CV_64F, _dxdR2); - CvMat dxdt1 = cvMat(3, 3, CV_64F, _dxdt1); - CvMat W3 = cvMat(3, 3, CV_64F, _W3); - - CV_Assert( CV_IS_MAT(_tvec1) && CV_IS_MAT(_tvec2) ); - CV_Assert( CV_ARE_SIZES_EQ(_tvec1, _tvec2) && CV_ARE_SIZES_EQ(_tvec1, _rvec1) ); - - cvConvert( _tvec1, &t1 ); - cvConvert( _tvec2, &t2 ); - cvMatMulAdd( &R2, &t1, &t2, &t3 ); - - if( _tvec3 ) - cvConvert( &t3, _tvec3 ); - - if( dt3dr2 || dt3dt1 ) - { - cvCalcMatMulDeriv( &R2, &t1, &dxdR2, &dxdt1 ); - if( dt3dr2 ) - { - cvMatMul( &dxdR2, &dR2dr2, &W3 ); - cvConvert( &W3, dt3dr2 ); - } - if( dt3dt1 ) - cvConvert( &dxdt1, dt3dt1 ); - } + gemm(dr3dR3, dR3dR2, 1, noArray(), 0, W1, GEMM_1_T); + gemm(W1, dR2dr2, 1, noArray(), 0, W2, GEMM_2_T); + W2.convertTo(dr3dr2, rtype); } - if( dt3dt2 ) - cvSetIdentity( dt3dt2 ); - if( dt3dr1 ) - cvZero( dt3dr1 ); + if( dr3dt1.data ) + dr3dt1.setZero(); + if( dr3dt2.data ) + dr3dt2.setZero(); + + double _t1[3], _t2[3], _t3[3], _dxdR2[3*9], _dxdt1[3*3], _W3[3*3]; + Mat t1(3,1,CV_64F,_t1), t2(3,1,CV_64F,_t2); + Mat t3(3,1,CV_64F,_t3); + Mat dxdR2(3, 9, CV_64F, _dxdR2); + Mat dxdt1(3, 3, CV_64F, _dxdt1); + Mat W3(3, 3, CV_64F, _W3); + + tvec1.convertTo(t1, CV_64F); + tvec2.convertTo(t2, CV_64F); + gemm(R2, t1, 1, t2, 1, t3); + t3.convertTo(_tvec3, rtype); + + if( dt3dr2.data || dt3dt1.data ) + { + matMulDeriv(R2, t1, dxdR2, dxdt1); + if( dt3dr2.data ) + { + gemm(dxdR2, dR2dr2, 1, noArray(), 0, W3, GEMM_2_T); + W3.convertTo(dt3dr2, rtype); + } + if( dt3dt1.data ) + dxdt1.convertTo(dt3dt1, rtype); + } + + if( dt3dt2.data ) + setIdentity(dt3dt2); + if( dt3dr1.data ) + dt3dr1.setZero(); } -static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector"; +static const char* cvDistCoeffErr = + "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector"; -static void cvProjectPoints2Internal( const CvMat* objectPoints, - const CvMat* r_vec, - const CvMat* t_vec, - const CvMat* A, - const CvMat* distCoeffs, - CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL), - CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL), - CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL), - CvMat* dpdo CV_DEFAULT(NULL), - double aspectRatio CV_DEFAULT(0) ) +void cv::projectPoints( InputArray _objectPoints, + InputArray _rvec, InputArray _tvec, + InputArray _cameraMatrix, InputArray _distCoeffs, + OutputArray _imagePoints, OutputArray _dpdr, + OutputArray _dpdt, OutputArray _dpdf, + OutputArray _dpdc, OutputArray _dpdk, + OutputArray _dpdo, double aspectRatio) { - Ptr matM, _m; - Ptr _dpdr, _dpdt, _dpdc, _dpdf, _dpdk; - Ptr _dpdo; + Mat _m, objectPoints = _objectPoints.getMat(); + Mat dpdr, dpdt, dpdc, dpdf, dpdk, dpdo; - int i, j, count; - int calc_derivatives; - const CvPoint3D64f* M; - CvPoint2D64f* m; - double r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy; + int i, j; + double R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy; Matx33d matTilt = Matx33d::eye(); Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0); Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0); - CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k; - CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr ); + Mat matR( 3, 3, CV_64F, R ), _dRdr( 3, 9, CV_64F, dRdr ); double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0; double* dpdo_p = 0; int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0; int dpdo_step = 0; bool fixedAspectRatio = aspectRatio > FLT_EPSILON; - if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) || - !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) || - /*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) ) - CV_Error( CV_StsBadArg, "One of required arguments is not a valid matrix" ); - - int total = objectPoints->rows * objectPoints->cols * CV_MAT_CN(objectPoints->type); + int objpt_depth = objectPoints.depth(); + int objpt_cn = objectPoints.channels(); + int total = (int)(objectPoints.total()*objectPoints.channels()); + int count = total / 3; if(total % 3 != 0) { //we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" ); } count = total / 3; + CV_Assert(objpt_depth == CV_32F || objpt_depth == CV_64F); + CV_Assert((objectPoints.rows == 1 && objpt_cn == 3) || + (objectPoints.rows == count && objpt_cn*objectPoints.cols == 3) || + (objectPoints.rows == 3 && objpt_cn == 1 && objectPoints.cols == count)); - if( CV_IS_CONT_MAT(objectPoints->type) && - (CV_MAT_DEPTH(objectPoints->type) == CV_32F || CV_MAT_DEPTH(objectPoints->type) == CV_64F)&& - ((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) || - (objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3) || - (objectPoints->rows == 3 && CV_MAT_CN(objectPoints->type) == 1 && objectPoints->cols == count))) + Mat matM(objectPoints.size(), CV_64FC(objpt_cn)); + objectPoints.convertTo(matM, CV_64F); + if (objectPoints.rows == 3 && objectPoints.cols == count) { + Mat temp; + transpose(matM, temp); + matM = temp; + } + + CV_Assert( _imagePoints.needed() ); + _imagePoints.create(count, 1, CV_MAKETYPE(objpt_depth, 2), -1, true); + Mat ipoints = _imagePoints.getMat(); + ipoints.convertTo(_m, CV_64F); + const Point3d* M = matM.ptr(); + Point2d* m = _m.ptr(); + + Mat rvec = _rvec.getMat(), tvec = _tvec.getMat(); + if(!((rvec.depth() == CV_32F || rvec.depth() == CV_64F) && + (rvec.size() == Size(3, 3) || + (rvec.rows == 1 && rvec.cols*rvec.channels() == 3) || + (rvec.rows == 3 && rvec.cols*rvec.channels() == 1)))) { + CV_Error(CV_StsBadArg, "rvec must be 3x3 or 1x3 or 3x1 floating-point array"); + } + + if( rvec.size() == Size(3, 3) ) { - matM.reset(cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) )); - cvConvert(objectPoints, matM); + rvec.convertTo(matR, CV_64F); + Vec3d rvec_d; + Rodrigues(matR, rvec_d); + Rodrigues(rvec_d, matR, _dRdr); + rvec.convertTo(matR, CV_64F); } else { -// matM = cvCreateMat( 1, count, CV_64FC3 ); -// cvConvertPointsHomogeneous( objectPoints, matM ); - CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" ); + double r[3]; + Mat _r(rvec.size(), CV_64FC(rvec.channels()), r); + rvec.convertTo(_r, CV_64F); + Rodrigues(_r, matR, _dRdr); } - if( CV_IS_CONT_MAT(imagePoints->type) && - (CV_MAT_DEPTH(imagePoints->type) == CV_32F || CV_MAT_DEPTH(imagePoints->type) == CV_64F) && - ((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 2) || - (imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 2) || - (imagePoints->rows == 2 && CV_MAT_CN(imagePoints->type) == 1 && imagePoints->cols == count))) - { - _m.reset(cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) )); - cvConvert(imagePoints, _m); - } - else - { -// _m = cvCreateMat( 1, count, CV_64FC2 ); - CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" ); + if(!((tvec.depth() == CV_32F || tvec.depth() == CV_64F) && + ((tvec.rows == 1 && tvec.cols*tvec.channels() == 3) || + (tvec.rows == 3 && tvec.cols*tvec.channels() == 1)))) { + CV_Error(CV_StsBadArg, "tvec must be 1x3 or 3x1 floating-point array"); } - M = (CvPoint3D64f*)matM->data.db; - m = (CvPoint2D64f*)_m->data.db; + Mat _t(tvec.size(), CV_64FC(tvec.channels()), t); + tvec.convertTo(_t, CV_64F); - if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) || - (((r_vec->rows != 1 && r_vec->cols != 1) || - r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) && - ((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1))) - CV_Error( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 " - "floating-point rotation vector, or 3x3 rotation matrix" ); + Mat cameraMatrix = _cameraMatrix.getMat(); - if( r_vec->rows == 3 && r_vec->cols == 3 ) - { - _r = cvMat( 3, 1, CV_64FC1, r ); - cvRodrigues2( r_vec, &_r ); - cvRodrigues2( &_r, &matR, &_dRdr ); - cvCopy( r_vec, &matR ); - } - else - { - _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r ); - cvConvert( r_vec, &_r ); - cvRodrigues2( &_r, &matR, &_dRdr ); - } - - if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) || - (t_vec->rows != 1 && t_vec->cols != 1) || - t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 ) - CV_Error( CV_StsBadArg, - "Translation vector must be 1x3 or 3x1 floating-point vector" ); - - _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t ); - cvConvert( t_vec, &_t ); - - if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) || - A->rows != 3 || A->cols != 3 ) + if(cameraMatrix.size() != Size(3, 3) || cameraMatrix.channels() != 1) CV_Error( CV_StsBadArg, "Intrinsic parameters must be 3x3 floating-point matrix" ); + Mat _a(3, 3, CV_64F, a); + cameraMatrix.convertTo(_a, CV_64F); - cvConvert( A, &_a ); fx = a[0]; fy = a[4]; cx = a[2]; cy = a[5]; if( fixedAspectRatio ) fx = fy*aspectRatio; - if( distCoeffs ) + Mat distCoeffs = _distCoeffs.getMat(); + int ktotal = 0; + if( distCoeffs.data ) { - if( !CV_IS_MAT(distCoeffs) || - (CV_MAT_DEPTH(distCoeffs->type) != CV_64F && - CV_MAT_DEPTH(distCoeffs->type) != CV_32F) || - (distCoeffs->rows != 1 && distCoeffs->cols != 1) || - (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 && - distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 && - distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8 && - distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12 && - distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 14) ) + int kcn = distCoeffs.channels(); + ktotal = (int)distCoeffs.total()*kcn; + if( (distCoeffs.rows != 1 && distCoeffs.cols != 1) || + (ktotal != 4 && ktotal != 5 && ktotal != 8 && ktotal != 12 && ktotal != 14)) CV_Error( CV_StsBadArg, cvDistCoeffErr ); - _k = cvMat( distCoeffs->rows, distCoeffs->cols, - CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k ); - cvConvert( distCoeffs, &_k ); + Mat _k(distCoeffs.size(), CV_64FC(kcn), k); + distCoeffs.convertTo(_k, CV_64F); if(k[12] != 0 || k[13] != 0) - { - cv::computeTiltProjectionMatrix(k[12], k[13], - &matTilt, &dMatTiltdTauX, &dMatTiltdTauY); - } + computeTiltProjectionMatrix(k[12], k[13], &matTilt, &dMatTiltdTauX, &dMatTiltdTauY); } - if( dpdr ) + if( _dpdr.needed() ) { - if( !CV_IS_MAT(dpdr) || - (CV_MAT_TYPE(dpdr->type) != CV_32FC1 && - CV_MAT_TYPE(dpdr->type) != CV_64FC1) || - dpdr->rows != count*2 || dpdr->cols != 3 ) - CV_Error( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" ); - - if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 ) - { - _dpdr.reset(cvCloneMat(dpdr)); - } - else - _dpdr.reset(cvCreateMat( 2*count, 3, CV_64FC1 )); - dpdr_p = _dpdr->data.db; - dpdr_step = _dpdr->step/sizeof(dpdr_p[0]); + dpdr.create(count*2, 3, CV_64F); + dpdr_p = dpdr.ptr(); + dpdr_step = (int)dpdr.step1(); } - - if( dpdt ) + if( _dpdt.needed() ) { - if( !CV_IS_MAT(dpdt) || - (CV_MAT_TYPE(dpdt->type) != CV_32FC1 && - CV_MAT_TYPE(dpdt->type) != CV_64FC1) || - dpdt->rows != count*2 || dpdt->cols != 3 ) - CV_Error( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" ); - - if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 ) - { - _dpdt.reset(cvCloneMat(dpdt)); - } - else - _dpdt.reset(cvCreateMat( 2*count, 3, CV_64FC1 )); - dpdt_p = _dpdt->data.db; - dpdt_step = _dpdt->step/sizeof(dpdt_p[0]); + dpdt.create(count*2, 3, CV_64F); + dpdt_p = dpdt.ptr(); + dpdt_step = (int)dpdt.step1(); } - - if( dpdf ) + if( _dpdf.needed() ) { - if( !CV_IS_MAT(dpdf) || - (CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) || - dpdf->rows != count*2 || dpdf->cols != 2 ) - CV_Error( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" ); - - if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 ) - { - _dpdf.reset(cvCloneMat(dpdf)); - } - else - _dpdf.reset(cvCreateMat( 2*count, 2, CV_64FC1 )); - dpdf_p = _dpdf->data.db; - dpdf_step = _dpdf->step/sizeof(dpdf_p[0]); + dpdf.create(count*2, 2, CV_64F); + dpdf_p = dpdf.ptr(); + dpdf_step = (int)dpdf.step1(); } - - if( dpdc ) + if( _dpdc.needed() ) { - if( !CV_IS_MAT(dpdc) || - (CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) || - dpdc->rows != count*2 || dpdc->cols != 2 ) - CV_Error( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" ); - - if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 ) - { - _dpdc.reset(cvCloneMat(dpdc)); - } - else - _dpdc.reset(cvCreateMat( 2*count, 2, CV_64FC1 )); - dpdc_p = _dpdc->data.db; - dpdc_step = _dpdc->step/sizeof(dpdc_p[0]); + dpdc.create(count*2, 2, CV_64F); + dpdc_p = dpdc.ptr(); + dpdc_step = (int)dpdc.step1(); } - - if( dpdk ) + if( _dpdk.needed() ) { - if( !CV_IS_MAT(dpdk) || - (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) || - dpdk->rows != count*2 || (dpdk->cols != 14 && dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) ) - CV_Error( CV_StsBadArg, "dp/df must be 2Nx14, 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" ); - - if( !distCoeffs ) - CV_Error( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" ); - - if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 ) - { - _dpdk.reset(cvCloneMat(dpdk)); - } - else - _dpdk.reset(cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 )); - dpdk_p = _dpdk->data.db; - dpdk_step = _dpdk->step/sizeof(dpdk_p[0]); + dpdk.create(count*2, ktotal, CV_64F); + dpdk_p = dpdk.ptr(); + dpdk_step = (int)dpdk.step1(); } - - if( dpdo ) + if( _dpdo.needed() ) { - if( !CV_IS_MAT( dpdo ) || ( CV_MAT_TYPE( dpdo->type ) != CV_32FC1 - && CV_MAT_TYPE( dpdo->type ) != CV_64FC1 ) - || dpdo->rows != count * 2 || dpdo->cols != count * 3 ) - CV_Error( CV_StsBadArg, "dp/do must be 2Nx3N floating-point matrix" ); - - if( CV_MAT_TYPE( dpdo->type ) == CV_64FC1 ) - { - _dpdo.reset( cvCloneMat( dpdo ) ); - } - else - _dpdo.reset( cvCreateMat( 2 * count, 3 * count, CV_64FC1 ) ); - cvZero(_dpdo); - dpdo_p = _dpdo->data.db; - dpdo_step = _dpdo->step / sizeof( dpdo_p[0] ); + dpdo = Mat::zeros(count*2, count*3, CV_64F); + dpdo_p = dpdo.ptr(); + dpdo_step = (int)dpdo.step1(); } - calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk || dpdo; + bool calc_derivatives = dpdr.data || dpdt.data || dpdf.data || + dpdc.data || dpdk.data || dpdo.data; for( i = 0; i < count; i++ ) { @@ -808,7 +696,7 @@ static void cvProjectPoints2Internal( const CvMat* objectPoints, if( calc_derivatives ) { - if( dpdc_p ) + if( dpdc.data ) { dpdc_p[0] = 1; dpdc_p[1] = 0; // dp_xdc_x; dp_xdc_y dpdc_p[dpdc_step] = 0; @@ -834,8 +722,7 @@ static void cvProjectPoints2Internal( const CvMat* objectPoints, } for (int row = 0; row < 2; ++row) for (int col = 0; col < 2; ++col) - dMatTilt(row,col) = matTilt(row,col)*vecTilt(2) - - matTilt(2,col)*vecTilt(row); + dMatTilt(row,col) = matTilt(row,col)*vecTilt(2) - matTilt(2,col)*vecTilt(row); double invProjSquare = (invProj*invProj); dMatTilt *= invProjSquare; if( dpdk_p ) @@ -846,7 +733,7 @@ static void cvProjectPoints2Internal( const CvMat* objectPoints, dXdYd = dMatTilt*Vec2d(x*icdist2*r4, y*icdist2*r4); dpdk_p[1] = fx*dXdYd(0); dpdk_p[dpdk_step+1] = fy*dXdYd(1); - if( _dpdk->cols > 2 ) + if( dpdk.cols > 2 ) { dXdYd = dMatTilt*Vec2d(a1, a3); dpdk_p[2] = fx*dXdYd(0); @@ -854,13 +741,13 @@ static void cvProjectPoints2Internal( const CvMat* objectPoints, dXdYd = dMatTilt*Vec2d(a2, a1); dpdk_p[3] = fx*dXdYd(0); dpdk_p[dpdk_step+3] = fy*dXdYd(1); - if( _dpdk->cols > 4 ) + if( dpdk.cols > 4 ) { dXdYd = dMatTilt*Vec2d(x*icdist2*r6, y*icdist2*r6); dpdk_p[4] = fx*dXdYd(0); dpdk_p[dpdk_step+4] = fy*dXdYd(1); - if( _dpdk->cols > 5 ) + if( dpdk.cols > 5 ) { dXdYd = dMatTilt*Vec2d( x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2); @@ -874,7 +761,7 @@ static void cvProjectPoints2Internal( const CvMat* objectPoints, x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6); dpdk_p[7] = fx*dXdYd(0); dpdk_p[dpdk_step+7] = fy*dXdYd(1); - if( _dpdk->cols > 8 ) + if( dpdk.cols > 8 ) { dXdYd = dMatTilt*Vec2d(r2, 0); dpdk_p[8] = fx*dXdYd(0); //s1 @@ -888,7 +775,7 @@ static void cvProjectPoints2Internal( const CvMat* objectPoints, dXdYd = dMatTilt*Vec2d(0, r4); dpdk_p[11] = fx*dXdYd(0);//s4 dpdk_p[dpdk_step+11] = fy*dXdYd(1); //s4 - if( _dpdk->cols > 12 ) + if( dpdk.cols > 12 ) { dVecTilt = dMatTiltdTauX * Vec3d(xd0, yd0, 1); dpdk_p[12] = fx * invProjSquare * ( @@ -1004,58 +891,42 @@ static void cvProjectPoints2Internal( const CvMat* objectPoints, } } - if( _m != imagePoints ) - cvConvert( _m, imagePoints ); + _m.convertTo(_imagePoints, objpt_depth); - if( _dpdr != dpdr ) - cvConvert( _dpdr, dpdr ); + int depth = CV_64F;//cameraMatrix.depth(); + if( _dpdr.needed() ) + dpdr.convertTo(_dpdr, depth); - if( _dpdt != dpdt ) - cvConvert( _dpdt, dpdt ); + if( _dpdt.needed() ) + dpdt.convertTo(_dpdt, depth); - if( _dpdf != dpdf ) - cvConvert( _dpdf, dpdf ); + if( _dpdf.needed() ) + dpdf.convertTo(_dpdf, depth); - if( _dpdc != dpdc ) - cvConvert( _dpdc, dpdc ); + if( _dpdc.needed() ) + dpdc.convertTo(_dpdc, depth); - if( _dpdk != dpdk ) - cvConvert( _dpdk, dpdk ); + if( _dpdk.needed() ) + dpdk.convertTo(_dpdk, depth); - if( _dpdo != dpdo ) - cvConvert( _dpdo, dpdo ); + if( _dpdo.needed() ) + dpdo.convertTo(_dpdo, depth); } -static void cvProjectPoints2( const CvMat* objectPoints, - const CvMat* r_vec, - const CvMat* t_vec, - const CvMat* A, - const CvMat* distCoeffs, - CvMat* imagePoints, CvMat* dpdr, - CvMat* dpdt, CvMat* dpdf, - CvMat* dpdc, CvMat* dpdk, - double aspectRatio ) +cv::Vec3d cv::RQDecomp3x3( InputArray _Marr, + OutputArray _Rarr, + OutputArray _Qarr, + OutputArray _Qx, + OutputArray _Qy, + OutputArray _Qz ) { - cvProjectPoints2Internal( objectPoints, r_vec, t_vec, A, distCoeffs, imagePoints, dpdr, dpdt, - dpdf, dpdc, dpdk, NULL, aspectRatio ); -} + CV_INSTRUMENT_REGION(); -static void cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ, - CvMat *matrixQx, CvMat *matrixQy, CvMat *matrixQz, - CvPoint3D64f *eulerAngles) -{ - double matM[3][3], matR[3][3], matQ[3][3]; - CvMat M = cvMat(3, 3, CV_64F, matM); - CvMat R = cvMat(3, 3, CV_64F, matR); - CvMat Q = cvMat(3, 3, CV_64F, matQ); + Matx33d M, Q; double z, c, s; - - /* Validate parameters. */ - CV_Assert( CV_IS_MAT(matrixM) && CV_IS_MAT(matrixR) && CV_IS_MAT(matrixQ) && - matrixM->cols == 3 && matrixM->rows == 3 && - CV_ARE_SIZES_EQ(matrixM, matrixR) && CV_ARE_SIZES_EQ(matrixM, matrixQ)); - - cvConvert(matrixM, &M); + Mat Mmat = _Marr.getMat(); + int depth = Mmat.depth(); + Mmat.convertTo(M, CV_64F); /* Find Givens rotation Q_x for x axis (left multiplication). */ /* @@ -1063,18 +934,17 @@ static void cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ, Qx = ( 0 c s ), c = m33/sqrt(m32^2 + m33^2), s = m32/sqrt(m32^2 + m33^2) ( 0 -s c ) */ - s = matM[2][1]; - c = matM[2][2]; + s = M(2, 1); + c = M(2, 2); z = 1./std::sqrt(c * c + s * s + DBL_EPSILON); c *= z; s *= z; - double _Qx[3][3] = { {1, 0, 0}, {0, c, s}, {0, -s, c} }; - CvMat Qx = cvMat(3, 3, CV_64F, _Qx); + Matx33d Qx(1, 0, 0, 0, c, s, 0, -s, c); + Matx33d R = M*Qx; - cvMatMul(&M, &Qx, &R); - assert(fabs(matR[2][1]) < FLT_EPSILON); - matR[2][1] = 0; + assert(fabs(R(2, 1)) < FLT_EPSILON); + R(2, 1) = 0; /* Find Givens rotation for y axis. */ /* @@ -1082,18 +952,17 @@ static void cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ, Qy = ( 0 1 0 ), c = m33/sqrt(m31^2 + m33^2), s = -m31/sqrt(m31^2 + m33^2) ( s 0 c ) */ - s = -matR[2][0]; - c = matR[2][2]; + s = -R(2, 0); + c = R(2, 2); z = 1./std::sqrt(c * c + s * s + DBL_EPSILON); c *= z; s *= z; - double _Qy[3][3] = { {c, 0, -s}, {0, 1, 0}, {s, 0, c} }; - CvMat Qy = cvMat(3, 3, CV_64F, _Qy); - cvMatMul(&R, &Qy, &M); + Matx33d Qy(c, 0, -s, 0, 1, 0, s, 0, c); + M = R*Qy; - assert(fabs(matM[2][0]) < FLT_EPSILON); - matM[2][0] = 0; + CV_Assert(fabs(M(2, 0)) < FLT_EPSILON); + M(2, 0) = 0; /* Find Givens rotation for z axis. */ /* @@ -1102,38 +971,37 @@ static void cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ, ( 0 0 1 ) */ - s = matM[1][0]; - c = matM[1][1]; + s = M(1, 0); + c = M(1, 1); z = 1./std::sqrt(c * c + s * s + DBL_EPSILON); c *= z; s *= z; - double _Qz[3][3] = { {c, s, 0}, {-s, c, 0}, {0, 0, 1} }; - CvMat Qz = cvMat(3, 3, CV_64F, _Qz); + Matx33d Qz(c, s, 0, -s, c, 0, 0, 0, 1); + R = M*Qz; - cvMatMul(&M, &Qz, &R); - assert(fabs(matR[1][0]) < FLT_EPSILON); - matR[1][0] = 0; + CV_Assert(fabs(R(1, 0)) < FLT_EPSILON); + R(1, 0) = 0; // Solve the decomposition ambiguity. // Diagonal entries of R, except the last one, shall be positive. // Further rotate R by 180 degree if necessary - if( matR[0][0] < 0 ) + if( R(0, 0) < 0 ) { - if( matR[1][1] < 0 ) + if( R(1, 1) < 0 ) { // rotate around z for 180 degree, i.e. a rotation matrix of // [-1, 0, 0], // [ 0, -1, 0], // [ 0, 0, 1] - matR[0][0] *= -1; - matR[0][1] *= -1; - matR[1][1] *= -1; + R(0, 0) *= -1; + R(0, 1) *= -1; + R(1, 1) *= -1; - _Qz[0][0] *= -1; - _Qz[0][1] *= -1; - _Qz[1][0] *= -1; - _Qz[1][1] *= -1; + Qz(0, 0) *= -1; + Qz(0, 1) *= -1; + Qz(1, 0) *= -1; + Qz(1, 1) *= -1; } else { @@ -1141,20 +1009,20 @@ static void cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ, // [-1, 0, 0], // [ 0, 1, 0], // [ 0, 0, -1] - matR[0][0] *= -1; - matR[0][2] *= -1; - matR[1][2] *= -1; - matR[2][2] *= -1; + R(0, 0) *= -1; + R(0, 2) *= -1; + R(1, 2) *= -1; + R(2, 2) *= -1; - cvTranspose( &Qz, &Qz ); + Qz = Qz.t(); - _Qy[0][0] *= -1; - _Qy[0][2] *= -1; - _Qy[2][0] *= -1; - _Qy[2][2] *= -1; + Qy(0, 0) *= -1; + Qy(0, 2) *= -1; + Qy(2, 0) *= -1; + Qy(2, 2) *= -1; } } - else if( matR[1][1] < 0 ) + else if( R(1, 1) < 0 ) { // ??? for some reason, we never get here ??? @@ -1162,96 +1030,72 @@ static void cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ, // [ 1, 0, 0], // [ 0, -1, 0], // [ 0, 0, -1] - matR[0][1] *= -1; - matR[0][2] *= -1; - matR[1][1] *= -1; - matR[1][2] *= -1; - matR[2][2] *= -1; + R(0, 1) *= -1; + R(0, 2) *= -1; + R(1, 1) *= -1; + R(1, 2) *= -1; + R(2, 2) *= -1; - cvTranspose( &Qz, &Qz ); - cvTranspose( &Qy, &Qy ); + Qz = Qz.t(); + Qy = Qy.t(); - _Qx[1][1] *= -1; - _Qx[1][2] *= -1; - _Qx[2][1] *= -1; - _Qx[2][2] *= -1; + Qx(1, 1) *= -1; + Qx(1, 2) *= -1; + Qx(2, 1) *= -1; + Qx(2, 2) *= -1; } // calculate the euler angle - if( eulerAngles ) - { - eulerAngles->x = acos(_Qx[1][1]) * (_Qx[1][2] >= 0 ? 1 : -1) * (180.0 / CV_PI); - eulerAngles->y = acos(_Qy[0][0]) * (_Qy[2][0] >= 0 ? 1 : -1) * (180.0 / CV_PI); - eulerAngles->z = acos(_Qz[0][0]) * (_Qz[0][1] >= 0 ? 1 : -1) * (180.0 / CV_PI); - } + Vec3d eulerAngles( + acos(Qx(1, 1)) * (Qx(1, 2) >= 0 ? 1 : -1) * (180.0 / CV_PI), + acos(Qy(0, 0)) * (Qy(2, 0) >= 0 ? 1 : -1) * (180.0 / CV_PI), + acos(Qz(0, 0)) * (Qz(0, 1) >= 0 ? 1 : -1) * (180.0 / CV_PI)); /* Calculate orthogonal matrix. */ /* Q = QzT * QyT * QxT */ - cvGEMM( &Qz, &Qy, 1, 0, 0, &M, CV_GEMM_A_T + CV_GEMM_B_T ); - cvGEMM( &M, &Qx, 1, 0, 0, &Q, CV_GEMM_B_T ); + M = Qz.t()*Qy.t(); + Q = M*Qx.t(); /* Save R and Q matrices. */ - cvConvert( &R, matrixR ); - cvConvert( &Q, matrixQ ); + R.convertTo(_Rarr, depth); + Q.convertTo(_Qarr, depth); - if( matrixQx ) - cvConvert(&Qx, matrixQx); - if( matrixQy ) - cvConvert(&Qy, matrixQy); - if( matrixQz ) - cvConvert(&Qz, matrixQz); + if(_Qx.needed()) + Qx.convertTo(_Qx, depth); + if(_Qy.needed()) + Qy.convertTo(_Qy, depth); + if(_Qz.needed()) + Qz.convertTo(_Qz, depth); + return eulerAngles; } - -static void -cvDecomposeProjectionMatrix( const CvMat *projMatr, CvMat *calibMatr, - CvMat *rotMatr, CvMat *posVect, - CvMat *rotMatrX, CvMat *rotMatrY, - CvMat *rotMatrZ, CvPoint3D64f *eulerAngles) +void cv::decomposeProjectionMatrix( InputArray _projMatrix, OutputArray _cameraMatrix, + OutputArray _rotMatrix, OutputArray _transVect, + OutputArray _rotMatrixX, OutputArray _rotMatrixY, + OutputArray _rotMatrixZ, OutputArray _eulerAngles ) { - double tmpProjMatrData[16], tmpMatrixDData[16], tmpMatrixVData[16]; - CvMat tmpProjMatr = cvMat(4, 4, CV_64F, tmpProjMatrData); - CvMat tmpMatrixD = cvMat(4, 4, CV_64F, tmpMatrixDData); - CvMat tmpMatrixV = cvMat(4, 4, CV_64F, tmpMatrixVData); - CvMat tmpMatrixM; + CV_INSTRUMENT_REGION(); - /* Validate parameters. */ - if(projMatr == 0 || calibMatr == 0 || rotMatr == 0 || posVect == 0) - CV_Error(CV_StsNullPtr, "Some of parameters is a NULL pointer!"); - - if(!CV_IS_MAT(projMatr) || !CV_IS_MAT(calibMatr) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(posVect)) - CV_Error(CV_StsUnsupportedFormat, "Input parameters must be a matrices!"); - - if(projMatr->cols != 4 || projMatr->rows != 3) - CV_Error(CV_StsUnmatchedSizes, "Size of projection matrix must be 3x4!"); - - if(calibMatr->cols != 3 || calibMatr->rows != 3 || rotMatr->cols != 3 || rotMatr->rows != 3) - CV_Error(CV_StsUnmatchedSizes, "Size of calibration and rotation matrices must be 3x3!"); - - if(posVect->cols != 1 || posVect->rows != 4) - CV_Error(CV_StsUnmatchedSizes, "Size of position vector must be 4x1!"); - - /* Compute position vector. */ - cvSetZero(&tmpProjMatr); // Add zero row to make matrix square. - int i, k; - for(i = 0; i < 3; i++) - for(k = 0; k < 4; k++) - cvmSet(&tmpProjMatr, i, k, cvmGet(projMatr, i, k)); - - cvSVD(&tmpProjMatr, &tmpMatrixD, NULL, &tmpMatrixV, CV_SVD_MODIFY_A + CV_SVD_V_T); - - /* Save position vector. */ - for(i = 0; i < 4; i++) - cvmSet(posVect, i, 0, cvmGet(&tmpMatrixV, 3, i)); // Solution is last row of V. - - /* Compute calibration and rotation matrices via RQ decomposition. */ - cvGetCols(projMatr, &tmpMatrixM, 0, 3); // M is first square matrix of P. - - CV_Assert(cvDet(&tmpMatrixM) != 0.0); // So far only finite cameras could be decomposed, so M has to be nonsingular [det(M) != 0]. - - cvRQDecomp3x3(&tmpMatrixM, calibMatr, rotMatr, rotMatrX, rotMatrY, rotMatrZ, eulerAngles); + Mat projMatrix = _projMatrix.getMat(); + int depth = projMatrix.depth(); + Matx34d P; + projMatrix.convertTo(P, CV_64F); + Matx44d Px(P(0, 0), P(0, 1), P(0, 2), P(0, 3), + P(1, 0), P(1, 1), P(1, 2), P(1, 3), + P(2, 0), P(2, 1), P(2, 2), P(2, 3), + 0, 0, 0, 0), U, Vt; + Matx41d W; + SVDecomp(Px, W, U, Vt, SVD::MODIFY_A); + Vec4d t(Vt(3, 0), Vt(3, 1), Vt(3, 2), Vt(3, 3)); + Matx33d M(P(0, 0), P(0, 1), P(0, 2), + P(1, 0), P(1, 1), P(1, 2), + P(2, 0), P(2, 1), P(2, 2)); + t.convertTo(_transVect, depth); + Vec3d eulerAngles = RQDecomp3x3(M, _cameraMatrix, _rotMatrix, _rotMatrixX, _rotMatrixY, _rotMatrixZ); + if (_eulerAngles.needed()) + eulerAngles.convertTo(_eulerAngles, depth); } class SolvePnPCallback CV_FINAL : public LMSolver::Callback @@ -1283,17 +1127,8 @@ public: Mat Jac = _Jac.getMat(); Mat dpdr = Jac.colRange(0, 3); Mat dpdt = Jac.colRange(3, 6); - CvMat objpt_c = cvMat(objpt); - CvMat err_c = cvMat(err); - CvMat rvec_c = cvMat(rvec); - CvMat tvec_c = cvMat(tvec); - CvMat A_c = cvMat(cameraMatrix); - CvMat dk_c = cvMat(distCoeffs); - CvMat dpdr_c = cvMat(dpdr); - CvMat dpdt_c = cvMat(dpdt); - cvProjectPoints2( &objpt_c, &rvec_c, &tvec_c, &A_c, - distCoeffs.empty() ? 0 : &dk_c, - &err_c, &dpdr_c, &dpdt_c, 0, 0, 0, 0 ); + projectPoints( objpt, rvec, tvec, cameraMatrix, distCoeffs, + err, dpdr, dpdt, noArray(), noArray(), noArray(), noArray()); } else { @@ -1423,7 +1258,7 @@ void cv::findExtrinsicCameraParams2( const Mat& objectPoints, else { setIdentity(matR); - _t.setTo(Scalar::all(0)); + _t.setZero(); } Rodrigues( matR, _r ); @@ -1489,85 +1324,6 @@ void cv::findExtrinsicCameraParams2( const Mat& objectPoints, } -void cv::Rodrigues(InputArray _src, OutputArray _dst, OutputArray _jacobian) -{ - CV_INSTRUMENT_REGION(); - - Mat src = _src.getMat(); - const Size srcSz = src.size(); - CV_Check(srcSz, srcSz == Size(3, 1) || srcSz == Size(1, 3) || - (srcSz == Size(1, 1) && src.channels() == 3) || - srcSz == Size(3, 3), - "Input matrix must be 1x3 or 3x1 for a rotation vector, or 3x3 for a rotation matrix"); - - bool v2m = src.cols == 1 || src.rows == 1; - _dst.create(3, v2m ? 3 : 1, src.depth()); - Mat dst = _dst.getMat(); - CvMat _csrc = cvMat(src), _cdst = cvMat(dst), _cjacobian; - if( _jacobian.needed() ) - { - _jacobian.create(v2m ? Size(9, 3) : Size(3, 9), src.depth()); - _cjacobian = cvMat(_jacobian.getMat()); - } - bool ok = cvRodrigues2(&_csrc, &_cdst, _jacobian.needed() ? &_cjacobian : 0) > 0; - if( !ok ) - dst = Scalar(0); -} - -void cv::matMulDeriv( InputArray _Amat, InputArray _Bmat, - OutputArray _dABdA, OutputArray _dABdB ) -{ - CV_INSTRUMENT_REGION(); - - Mat A = _Amat.getMat(), B = _Bmat.getMat(); - _dABdA.create(A.rows*B.cols, A.rows*A.cols, A.type()); - _dABdB.create(A.rows*B.cols, B.rows*B.cols, A.type()); - Mat dABdA = _dABdA.getMat(), dABdB = _dABdB.getMat(); - CvMat matA = cvMat(A), matB = cvMat(B), c_dABdA = cvMat(dABdA), c_dABdB = cvMat(dABdB); - cvCalcMatMulDeriv(&matA, &matB, &c_dABdA, &c_dABdB); -} - - -void cv::composeRT( InputArray _rvec1, InputArray _tvec1, - InputArray _rvec2, InputArray _tvec2, - OutputArray _rvec3, OutputArray _tvec3, - OutputArray _dr3dr1, OutputArray _dr3dt1, - OutputArray _dr3dr2, OutputArray _dr3dt2, - OutputArray _dt3dr1, OutputArray _dt3dt1, - OutputArray _dt3dr2, OutputArray _dt3dt2 ) -{ - Mat rvec1 = _rvec1.getMat(), tvec1 = _tvec1.getMat(); - Mat rvec2 = _rvec2.getMat(), tvec2 = _tvec2.getMat(); - int rtype = rvec1.type(); - _rvec3.create(rvec1.size(), rtype); - _tvec3.create(tvec1.size(), rtype); - Mat rvec3 = _rvec3.getMat(), tvec3 = _tvec3.getMat(); - - CvMat c_rvec1 = cvMat(rvec1), c_tvec1 = cvMat(tvec1), c_rvec2 = cvMat(rvec2), - c_tvec2 = cvMat(tvec2), c_rvec3 = cvMat(rvec3), c_tvec3 = cvMat(tvec3); - CvMat c_dr3dr1, c_dr3dt1, c_dr3dr2, c_dr3dt2, c_dt3dr1, c_dt3dt1, c_dt3dr2, c_dt3dt2; - CvMat *p_dr3dr1=0, *p_dr3dt1=0, *p_dr3dr2=0, *p_dr3dt2=0, *p_dt3dr1=0, *p_dt3dt1=0, *p_dt3dr2=0, *p_dt3dt2=0; -#define CV_COMPOSE_RT_PARAM(name) \ - Mat name; \ - if (_ ## name.needed())\ - { \ - _ ## name.create(3, 3, rtype); \ - name = _ ## name.getMat(); \ - p_ ## name = &(c_ ## name = cvMat(name)); \ - } - - CV_COMPOSE_RT_PARAM(dr3dr1); CV_COMPOSE_RT_PARAM(dr3dt1); - CV_COMPOSE_RT_PARAM(dr3dr2); CV_COMPOSE_RT_PARAM(dr3dt2); - CV_COMPOSE_RT_PARAM(dt3dr1); CV_COMPOSE_RT_PARAM(dt3dt1); - CV_COMPOSE_RT_PARAM(dt3dr2); CV_COMPOSE_RT_PARAM(dt3dt2); -#undef CV_COMPOSE_RT_PARAM - - cvComposeRT(&c_rvec1, &c_tvec1, &c_rvec2, &c_tvec2, &c_rvec3, &c_tvec3, - p_dr3dr1, p_dr3dt1, p_dr3dr2, p_dr3dt2, - p_dt3dr1, p_dt3dt1, p_dt3dr2, p_dt3dt2); -} - - void cv::projectPoints( InputArray _opoints, InputArray _rvec, InputArray _tvec, @@ -1587,43 +1343,33 @@ void cv::projectPoints( InputArray _opoints, if (opoints.cols == 3) opoints = opoints.reshape(3); - CvMat dpdrot, dpdt, dpdf, dpdc, dpddist; - CvMat *pdpdrot=0, *pdpdt=0, *pdpdf=0, *pdpdc=0, *pdpddist=0; - CV_Assert( _ipoints.needed() ); - _ipoints.create(npoints, 1, CV_MAKETYPE(depth, 2), -1, true); - Mat imagePoints = _ipoints.getMat(); - CvMat c_imagePoints = cvMat(imagePoints); - CvMat c_objectPoints = cvMat(opoints); - Mat cameraMatrix = _cameraMatrix.getMat(); - - Mat rvec = _rvec.getMat(), tvec = _tvec.getMat(); - CvMat c_cameraMatrix = cvMat(cameraMatrix); - CvMat c_rvec = cvMat(rvec), c_tvec = cvMat(tvec); - double dc0buf[5]={0}; Mat dc0(5,1,CV_64F,dc0buf); Mat distCoeffs = _distCoeffs.getMat(); if( distCoeffs.empty() ) distCoeffs = dc0; - CvMat c_distCoeffs = cvMat(distCoeffs); int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1; - Mat jacobian; if( _jacobian.needed() ) { _jacobian.create(npoints*2, 3+3+2+2+ndistCoeffs, CV_64F); - jacobian = _jacobian.getMat(); - pdpdrot = &(dpdrot = cvMat(jacobian.colRange(0, 3))); - pdpdt = &(dpdt = cvMat(jacobian.colRange(3, 6))); - pdpdf = &(dpdf = cvMat(jacobian.colRange(6, 8))); - pdpdc = &(dpdc = cvMat(jacobian.colRange(8, 10))); - pdpddist = &(dpddist = cvMat(jacobian.colRange(10, 10+ndistCoeffs))); - } + Mat jacobian = _jacobian.getMat(); + Mat dpdr = jacobian.colRange(0, 3); + Mat dpdt = jacobian.colRange(3, 6); + Mat dpdf = jacobian.colRange(6, 8); + Mat dpdc = jacobian.colRange(8, 10); + Mat dpdk = jacobian.colRange(10, 10+ndistCoeffs); - cvProjectPoints2( &c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs, - &c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio ); + projectPoints(opoints, _rvec, _tvec, _cameraMatrix, distCoeffs, _ipoints, + dpdr, dpdt, dpdf, dpdc, dpdk, noArray(), aspectRatio); + } + else + { + projectPoints(opoints, _rvec, _tvec, _cameraMatrix, distCoeffs, _ipoints, + noArray(), noArray(), noArray(), noArray(), noArray(), noArray(), aspectRatio); + } } void cv::getUndistortRectangles(InputArray _cameraMatrix, InputArray _distCoeffs, @@ -1746,85 +1492,4 @@ cv::Mat cv::getOptimalNewCameraMatrix( InputArray _cameraMatrix, InputArray _dis return M; } -cv::Vec3d cv::RQDecomp3x3( InputArray _Mmat, - OutputArray _Rmat, - OutputArray _Qmat, - OutputArray _Qx, - OutputArray _Qy, - OutputArray _Qz ) -{ - CV_INSTRUMENT_REGION(); - - Mat M = _Mmat.getMat(); - _Rmat.create(3, 3, M.type()); - _Qmat.create(3, 3, M.type()); - Mat Rmat = _Rmat.getMat(); - Mat Qmat = _Qmat.getMat(); - Vec3d eulerAngles; - - CvMat matM = cvMat(M), matR = cvMat(Rmat), matQ = cvMat(Qmat); -#define CV_RQDecomp3x3_PARAM(name) \ - Mat name; \ - CvMat c_ ## name, *p ## name = NULL; \ - if( _ ## name.needed() ) \ - { \ - _ ## name.create(3, 3, M.type()); \ - name = _ ## name.getMat(); \ - c_ ## name = cvMat(name); p ## name = &c_ ## name; \ - } - - CV_RQDecomp3x3_PARAM(Qx); - CV_RQDecomp3x3_PARAM(Qy); - CV_RQDecomp3x3_PARAM(Qz); -#undef CV_RQDecomp3x3_PARAM - cvRQDecomp3x3(&matM, &matR, &matQ, pQx, pQy, pQz, (CvPoint3D64f*)&eulerAngles[0]); - return eulerAngles; -} - - -void cv::decomposeProjectionMatrix( InputArray _projMatrix, OutputArray _cameraMatrix, - OutputArray _rotMatrix, OutputArray _transVect, - OutputArray _rotMatrixX, OutputArray _rotMatrixY, - OutputArray _rotMatrixZ, OutputArray _eulerAngles ) -{ - CV_INSTRUMENT_REGION(); - - Mat projMatrix = _projMatrix.getMat(); - int type = projMatrix.type(); - _cameraMatrix.create(3, 3, type); - _rotMatrix.create(3, 3, type); - _transVect.create(4, 1, type); - Mat cameraMatrix = _cameraMatrix.getMat(); - Mat rotMatrix = _rotMatrix.getMat(); - Mat transVect = _transVect.getMat(); - CvMat c_projMatrix = cvMat(projMatrix), c_cameraMatrix = cvMat(cameraMatrix); - CvMat c_rotMatrix = cvMat(rotMatrix), c_transVect = cvMat(transVect); - CvPoint3D64f *p_eulerAngles = 0; - -#define CV_decomposeProjectionMatrix_PARAM(name) \ - Mat name; \ - CvMat c_ ## name, *p_ ## name = NULL; \ - if( _ ## name.needed() ) \ - { \ - _ ## name.create(3, 3, type); \ - name = _ ## name.getMat(); \ - c_ ## name = cvMat(name); p_ ## name = &c_ ## name; \ - } - - CV_decomposeProjectionMatrix_PARAM(rotMatrixX); - CV_decomposeProjectionMatrix_PARAM(rotMatrixY); - CV_decomposeProjectionMatrix_PARAM(rotMatrixZ); -#undef CV_decomposeProjectionMatrix_PARAM - - if( _eulerAngles.needed() ) - { - _eulerAngles.create(3, 1, CV_64F, -1, true); - p_eulerAngles = _eulerAngles.getMat().ptr(); - } - - cvDecomposeProjectionMatrix(&c_projMatrix, &c_cameraMatrix, &c_rotMatrix, - &c_transVect, p_rotMatrixX, p_rotMatrixY, - p_rotMatrixZ, p_eulerAngles); -} - /* End of file. */ diff --git a/modules/3d/src/compat_ptsetreg.cpp b/modules/3d/src/compat_ptsetreg.cpp deleted file mode 100644 index b87b5bac6d..0000000000 --- a/modules/3d/src/compat_ptsetreg.cpp +++ /dev/null @@ -1,322 +0,0 @@ -/*M/////////////////////////////////////////////////////////////////////////////////////// -// -// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. -// -// By downloading, copying, installing or using the software you agree to this license. -// If you do not agree to this license, do not download, install, -// copy or use the software. -// -// -// License Agreement -// For Open Source Computer Vision Library -// -// Copyright (C) 2000, Intel Corporation, all rights reserved. -// Copyright (C) 2013, OpenCV Foundation, all rights reserved. -// Third party copyrights are property of their respective owners. -// -// Redistribution and use in source and binary forms, with or without modification, -// are permitted provided that the following conditions are met: -// -// * Redistribution's of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// -// * Redistribution's in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * The name of the copyright holders may not be used to endorse or promote products -// derived from this software without specific prior written permission. -// -// This software is provided by the copyright holders and contributors "as is" and -// any express or implied warranties, including, but not limited to, the implied -// warranties of merchantability and fitness for a particular purpose are disclaimed. -// In no event shall the Intel Corporation or contributors be liable for any direct, -// indirect, incidental, special, exemplary, or consequential damages -// (including, but not limited to, procurement of substitute goods or services; -// loss of use, data, or profits; or business interruption) however caused -// and on any theory of liability, whether in contract, strict liability, -// or tort (including negligence or otherwise) arising in any way out of -// the use of this software, even if advised of the possibility of such damage. -// -//M*/ - -#include "precomp.hpp" -#include "opencv2/core/core_c.h" - -/************************************************************************************\ - Some backward compatibility stuff, to be moved to legacy or compat module -\************************************************************************************/ - -namespace cv { - -////////////////// Levenberg-Marquardt engine (the old variant) //////////////////////// - -CvLevMarq::CvLevMarq() -{ - lambdaLg10 = 0; state = DONE; - criteria = cvTermCriteria(0,0,0); - iters = 0; - completeSymmFlag = false; - errNorm = prevErrNorm = DBL_MAX; - solveMethod = cv::DECOMP_SVD; -} - -CvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag ) -{ - init(nparams, nerrs, criteria0, _completeSymmFlag); -} - -void CvLevMarq::clear() -{ - mask.release(); - prevParam.release(); - param.release(); - J.release(); - err.release(); - JtJ.release(); - JtJN.release(); - JtErr.release(); - JtJV.release(); - JtJW.release(); -} - -CvLevMarq::~CvLevMarq() -{ - clear(); -} - -void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag ) -{ - if( !param || param->rows != nparams || nerrs != (err ? err->rows : 0) ) - clear(); - mask.reset(cvCreateMat( nparams, 1, CV_8U )); - cvSet(mask, cvScalarAll(1)); - prevParam.reset(cvCreateMat( nparams, 1, CV_64F )); - param.reset(cvCreateMat( nparams, 1, CV_64F )); - JtJ.reset(cvCreateMat( nparams, nparams, CV_64F )); - JtErr.reset(cvCreateMat( nparams, 1, CV_64F )); - if( nerrs > 0 ) - { - J.reset(cvCreateMat( nerrs, nparams, CV_64F )); - err.reset(cvCreateMat( nerrs, 1, CV_64F )); - } - errNorm = prevErrNorm = DBL_MAX; - lambdaLg10 = -3; - criteria = criteria0; - if( criteria.type & CV_TERMCRIT_ITER ) - criteria.max_iter = MIN(MAX(criteria.max_iter,1),1000); - else - criteria.max_iter = 30; - if( criteria.type & CV_TERMCRIT_EPS ) - criteria.epsilon = MAX(criteria.epsilon, 0); - else - criteria.epsilon = DBL_EPSILON; - state = STARTED; - iters = 0; - completeSymmFlag = _completeSymmFlag; - solveMethod = cv::DECOMP_SVD; -} - -bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err ) -{ - matJ = _err = 0; - - assert( !err.empty() ); - if( state == DONE ) - { - _param = param; - return false; - } - - if( state == STARTED ) - { - _param = param; - cvZero( J ); - cvZero( err ); - matJ = J; - _err = err; - state = CALC_J; - return true; - } - - if( state == CALC_J ) - { - cvMulTransposed( J, JtJ, 1 ); - cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T ); - cvCopy( param, prevParam ); - step(); - if( iters == 0 ) - prevErrNorm = cvNorm(err, 0, CV_L2); - _param = param; - cvZero( err ); - _err = err; - state = CHECK_ERR; - return true; - } - - assert( state == CHECK_ERR ); - errNorm = cvNorm( err, 0, CV_L2 ); - if( errNorm > prevErrNorm ) - { - if( ++lambdaLg10 <= 16 ) - { - step(); - _param = param; - cvZero( err ); - _err = err; - state = CHECK_ERR; - return true; - } - } - - lambdaLg10 = MAX(lambdaLg10-1, -16); - if( ++iters >= criteria.max_iter || - cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon ) - { - _param = param; - state = DONE; - return true; - } - - prevErrNorm = errNorm; - _param = param; - cvZero(J); - matJ = J; - _err = err; - state = CALC_J; - return true; -} - - -bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm ) -{ - CV_Assert( !err ); - if( state == DONE ) - { - _param = param; - return false; - } - - if( state == STARTED ) - { - _param = param; - cvZero( JtJ ); - cvZero( JtErr ); - errNorm = 0; - _JtJ = JtJ; - _JtErr = JtErr; - _errNorm = &errNorm; - state = CALC_J; - return true; - } - - if( state == CALC_J ) - { - cvCopy( param, prevParam ); - step(); - _param = param; - prevErrNorm = errNorm; - errNorm = 0; - _errNorm = &errNorm; - state = CHECK_ERR; - return true; - } - - assert( state == CHECK_ERR ); - if( errNorm > prevErrNorm ) - { - if( ++lambdaLg10 <= 16 ) - { - step(); - _param = param; - errNorm = 0; - _errNorm = &errNorm; - state = CHECK_ERR; - return true; - } - } - - lambdaLg10 = MAX(lambdaLg10-1, -16); - if( ++iters >= criteria.max_iter || - cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon ) - { - _param = param; - _JtJ = JtJ; - _JtErr = JtErr; - state = DONE; - return false; - } - - prevErrNorm = errNorm; - cvZero( JtJ ); - cvZero( JtErr ); - _param = param; - _JtJ = JtJ; - _JtErr = JtErr; - state = CALC_J; - return true; -} - -static void subMatrix(const Mat& src, Mat& dst, - const std::vector& cols, - const std::vector& rows) -{ - int nonzeros_cols = countNonZero(cols); - Mat tmp(src.rows, nonzeros_cols, CV_64FC1); - - for (int i = 0, j = 0; i < (int)cols.size(); i++) - { - if (cols[i]) - { - src.col(i).copyTo(tmp.col(j++)); - } - } - - int nonzeros_rows = cv::countNonZero(rows); - dst.create(nonzeros_rows, nonzeros_cols, CV_64FC1); - for (int i = 0, j = 0; i < (int)rows.size(); i++) - { - if (rows[i]) - { - tmp.row(i).copyTo(dst.row(j++)); - } - } -} - -void CvLevMarq::step() -{ - using namespace cv; - const double LOG10 = log(10.); - double lambda = exp(lambdaLg10*LOG10); - int nparams = param->rows; - - Mat _JtJ = cvarrToMat(JtJ); - Mat _mask = cvarrToMat(mask); - - int nparams_nz = countNonZero(_mask); - if(!JtJN || JtJN->rows != nparams_nz) { - // prevent re-allocation in every step - JtJN.reset(cvCreateMat( nparams_nz, nparams_nz, CV_64F )); - JtJV.reset(cvCreateMat( nparams_nz, 1, CV_64F )); - JtJW.reset(cvCreateMat( nparams_nz, 1, CV_64F )); - } - - Mat _JtJN = cvarrToMat(JtJN); - Mat _JtErr = cvarrToMat(JtJV); - Mat_ nonzero_param = cvarrToMat(JtJW); - - subMatrix(cvarrToMat(JtErr), _JtErr, std::vector(1, 1), _mask); - subMatrix(_JtJ, _JtJN, _mask, _mask); - - if( !err ) - completeSymm( _JtJN, completeSymmFlag ); - - _JtJN.diag() *= 1. + lambda; - solve(_JtJN, _JtErr, nonzero_param, solveMethod); - - int j = 0; - for( int i = 0; i < nparams; i++ ) - param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? nonzero_param(j++) : 0); -} - -} diff --git a/modules/3d/src/epnp.cpp b/modules/3d/src/epnp.cpp index c66506d23a..09bff11362 100644 --- a/modules/3d/src/epnp.cpp +++ b/modules/3d/src/epnp.cpp @@ -61,21 +61,22 @@ void epnp::choose_control_points(void) // Take C1, C2, and C3 from PCA on the reference points: - CvMat * PW0 = cvCreateMat(number_of_correspondences, 3, CV_64F); + Mat PW0(number_of_correspondences, 3, CV_64F); double pw0tpw0[3 * 3] = {}, dc[3] = {}, uct[3 * 3] = {}; - CvMat PW0tPW0 = cvMat(3, 3, CV_64F, pw0tpw0); - CvMat DC = cvMat(3, 1, CV_64F, dc); - CvMat UCt = cvMat(3, 3, CV_64F, uct); + Mat PW0tPW0(3, 3, CV_64F, pw0tpw0); + Mat DC(3, 1, CV_64F, dc); + Mat UCt(3, 3, CV_64F, uct); - for(int i = 0; i < number_of_correspondences; i++) + for(int i = 0; i < number_of_correspondences; i++) { + double* PW0row = PW0.ptr(i); for(int j = 0; j < 3; j++) - PW0->data.db[3 * i + j] = pws[3 * i + j] - cws[0][j]; + PW0row[j] = pws[3 * i + j] - cws[0][j]; + } - cvMulTransposed(PW0, &PW0tPW0, 1); - cvSVD(&PW0tPW0, &DC, &UCt, 0, CV_SVD_MODIFY_A | CV_SVD_U_T); - - cvReleaseMat(&PW0); + mulTransposed(PW0, PW0tPW0, true); + SVDecomp(PW0tPW0, DC, UCt, noArray(), SVD::MODIFY_A); + transpose(UCt, UCt); for(int i = 1; i < 4; i++) { double k = sqrt(dc[i - 1] / number_of_correspondences); @@ -86,16 +87,14 @@ void epnp::choose_control_points(void) void epnp::compute_barycentric_coordinates(void) { - double cc[3 * 3] = {}, cc_inv[3 * 3] = {}; - CvMat CC = cvMat(3, 3, CV_64F, cc); - CvMat CC_inv = cvMat(3, 3, CV_64F, cc_inv); + Matx33d CC, CC_inv; for(int i = 0; i < 3; i++) for(int j = 1; j < 4; j++) - cc[3 * i + j - 1] = cws[j][i] - cws[0][i]; + CC(i, j - 1) = cws[j][i] - cws[0][i]; - cvInvert(&CC, &CC_inv, CV_SVD); - double * ci = cc_inv; + cv::invert(CC, CC_inv, DECOMP_SVD); + double * ci = CC_inv.val; for(int i = 0; i < number_of_correspondences; i++) { double * pi = &pws[0] + 3 * i; double * a = &alphas[0] + 4 * i; @@ -111,10 +110,10 @@ void epnp::compute_barycentric_coordinates(void) } } -void epnp::fill_M(CvMat * M, +void epnp::fill_M(Mat& M, const int row, const double * as, const double u, const double v) { - double * M1 = M->data.db + row * 12; + double * M1 = M.ptr(row); double * M2 = M1 + 12; for(int i = 0; i < 4; i++) { @@ -157,23 +156,23 @@ void epnp::compute_pose(Mat& R, Mat& t) choose_control_points(); compute_barycentric_coordinates(); - CvMat * M = cvCreateMat(2 * number_of_correspondences, 12, CV_64F); + Mat M(2 * number_of_correspondences, 12, CV_64F); for(int i = 0; i < number_of_correspondences; i++) fill_M(M, 2 * i, &alphas[0] + 4 * i, us[2 * i], us[2 * i + 1]); double mtm[12 * 12] = {}, d[12] = {}, ut[12 * 12] = {}; - CvMat MtM = cvMat(12, 12, CV_64F, mtm); - CvMat D = cvMat(12, 1, CV_64F, d); - CvMat Ut = cvMat(12, 12, CV_64F, ut); + Mat MtM(12, 12, CV_64F, mtm); + Mat D(12, 1, CV_64F, d); + Mat Ut(12, 12, CV_64F, ut); - cvMulTransposed(M, &MtM, 1); - cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T); - cvReleaseMat(&M); + mulTransposed(M, MtM, true); + SVDecomp(MtM, D, Ut, noArray(), SVD::MODIFY_A); + transpose(Ut, Ut); double l_6x10[6 * 10] = {}, rho[6] = {}; - CvMat L_6x10 = cvMat(6, 10, CV_64F, l_6x10); - CvMat Rho = cvMat(6, 1, CV_64F, rho); + Mat L_6x10(6, 10, CV_64F, l_6x10); + Mat Rho(6, 1, CV_64F, rho); compute_L_6x10(ut, l_6x10); compute_rho(rho); @@ -181,16 +180,16 @@ void epnp::compute_pose(Mat& R, Mat& t) double Betas[4][4] = {}, rep_errors[4] = {}; double Rs[4][3][3] = {}, ts[4][3] = {}; - find_betas_approx_1(&L_6x10, &Rho, Betas[1]); - gauss_newton(&L_6x10, &Rho, Betas[1]); + find_betas_approx_1(L_6x10, Rho, Betas[1]); + gauss_newton(L_6x10, Rho, Betas[1]); rep_errors[1] = compute_R_and_t(ut, Betas[1], Rs[1], ts[1]); - find_betas_approx_2(&L_6x10, &Rho, Betas[2]); - gauss_newton(&L_6x10, &Rho, Betas[2]); + find_betas_approx_2(L_6x10, Rho, Betas[2]); + gauss_newton(L_6x10, Rho, Betas[2]); rep_errors[2] = compute_R_and_t(ut, Betas[2], Rs[2], ts[2]); - find_betas_approx_3(&L_6x10, &Rho, Betas[3]); - gauss_newton(&L_6x10, &Rho, Betas[3]); + find_betas_approx_3(L_6x10, Rho, Betas[3]); + gauss_newton(L_6x10, Rho, Betas[3]); rep_errors[3] = compute_R_and_t(ut, Betas[3], Rs[3], ts[3]); int N = 1; @@ -245,13 +244,13 @@ void epnp::estimate_R_and_t(double R[3][3], double t[3]) pw0[j] /= number_of_correspondences; } - double abt[3 * 3] = {}, abt_d[3] = {}, abt_u[3 * 3] = {}, abt_v[3 * 3] = {}; - CvMat ABt = cvMat(3, 3, CV_64F, abt); - CvMat ABt_D = cvMat(3, 1, CV_64F, abt_d); - CvMat ABt_U = cvMat(3, 3, CV_64F, abt_u); - CvMat ABt_V = cvMat(3, 3, CV_64F, abt_v); + double abt[3 * 3] = {}, abt_d[3] = {}, abt_u[3 * 3] = {}, abt_vt[3 * 3] = {}; + Mat ABt(3, 3, CV_64F, abt); + Mat ABt_D(3, 1, CV_64F, abt_d); + Mat ABt_U(3, 3, CV_64F, abt_u); + Mat ABt_Vt(3, 3, CV_64F, abt_vt); - cvSetZero(&ABt); + ABt.setTo(Scalar::all(0.)); for(int i = 0; i < number_of_correspondences; i++) { double * pc = &pcs[3 * i]; double * pw = &pws[3 * i]; @@ -263,15 +262,11 @@ void epnp::estimate_R_and_t(double R[3][3], double t[3]) } } - cvSVD(&ABt, &ABt_D, &ABt_U, &ABt_V, CV_SVD_MODIFY_A); + SVDecomp(ABt, ABt_D, ABt_U, ABt_Vt, SVD::MODIFY_A); + Mat mR(3, 3, CV_64F, R); + gemm(ABt_U, ABt_Vt, 1, noArray(), 0, mR); - for(int i = 0; i < 3; i++) - for(int j = 0; j < 3; j++) - R[i][j] = dot(abt_u + 3 * i, abt_v + 3 * j); - - const double det = - R[0][0] * R[1][1] * R[2][2] + R[0][1] * R[1][2] * R[2][0] + R[0][2] * R[1][0] * R[2][1] - - R[0][2] * R[1][1] * R[2][0] - R[0][1] * R[1][0] * R[2][2] - R[0][0] * R[1][2] * R[2][1]; + const double det = determinant(mR); if (det < 0) { R[2][0] = -R[2][0]; @@ -334,21 +329,21 @@ double epnp::reprojection_error(const double R[3][3], const double t[3]) // betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44] // betas_approx_1 = [B11 B12 B13 B14] -void epnp::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, - double * betas) +void epnp::find_betas_approx_1(const Mat& L_6x10, const Mat& Rho, double* betas) { double l_6x4[6 * 4] = {}, b4[4] = {}; - CvMat L_6x4 = cvMat(6, 4, CV_64F, l_6x4); - CvMat B4 = cvMat(4, 1, CV_64F, b4); + Mat L_6x4(6, 4, CV_64F, l_6x4); + Mat B4(4, 1, CV_64F, b4); for(int i = 0; i < 6; i++) { - cvmSet(&L_6x4, i, 0, cvmGet(L_6x10, i, 0)); - cvmSet(&L_6x4, i, 1, cvmGet(L_6x10, i, 1)); - cvmSet(&L_6x4, i, 2, cvmGet(L_6x10, i, 3)); - cvmSet(&L_6x4, i, 3, cvmGet(L_6x10, i, 6)); + L_6x4.at(i, 0) = L_6x10.at(i, 0); + L_6x4.at(i, 1) = L_6x10.at(i, 1); + L_6x4.at(i, 2) = L_6x10.at(i, 3); + L_6x4.at(i, 3) = L_6x10.at(i, 6); } - cvSolve(&L_6x4, Rho, &B4, CV_SVD); + solve(L_6x4, Rho, B4, DECOMP_SVD); + CV_Assert(B4.ptr() == b4); if (b4[0] < 0) { betas[0] = sqrt(-b4[0]); @@ -366,20 +361,20 @@ void epnp::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, // betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44] // betas_approx_2 = [B11 B12 B22 ] -void epnp::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, - double * betas) +void epnp::find_betas_approx_2(const Mat& L_6x10, const Mat& Rho, double* betas) { double l_6x3[6 * 3] = {}, b3[3] = {}; - CvMat L_6x3 = cvMat(6, 3, CV_64F, l_6x3); - CvMat B3 = cvMat(3, 1, CV_64F, b3); + Mat L_6x3(6, 3, CV_64F, l_6x3); + Mat B3(3, 1, CV_64F, b3); for(int i = 0; i < 6; i++) { - cvmSet(&L_6x3, i, 0, cvmGet(L_6x10, i, 0)); - cvmSet(&L_6x3, i, 1, cvmGet(L_6x10, i, 1)); - cvmSet(&L_6x3, i, 2, cvmGet(L_6x10, i, 2)); + L_6x3.at(i, 0) = L_6x10.at(i, 0); + L_6x3.at(i, 1) = L_6x10.at(i, 1); + L_6x3.at(i, 2) = L_6x10.at(i, 2); } - cvSolve(&L_6x3, Rho, &B3, CV_SVD); + solve(L_6x3, Rho, B3, DECOMP_SVD); + CV_Assert(B3.ptr() == b3); if (b3[0] < 0) { betas[0] = sqrt(-b3[0]); @@ -390,7 +385,6 @@ void epnp::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, } if (b3[1] < 0) betas[0] = -betas[0]; - betas[2] = 0.0; betas[3] = 0.0; } @@ -398,22 +392,22 @@ void epnp::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, // betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44] // betas_approx_3 = [B11 B12 B22 B13 B23 ] -void epnp::find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, - double * betas) +void epnp::find_betas_approx_3(const Mat& L_6x10, const Mat& Rho, double * betas) { double l_6x5[6 * 5] = {}, b5[5] = {}; - CvMat L_6x5 = cvMat(6, 5, CV_64F, l_6x5); - CvMat B5 = cvMat(5, 1, CV_64F, b5); + Mat L_6x5(6, 5, CV_64F, l_6x5); + Mat B5(5, 1, CV_64F, b5); for(int i = 0; i < 6; i++) { - cvmSet(&L_6x5, i, 0, cvmGet(L_6x10, i, 0)); - cvmSet(&L_6x5, i, 1, cvmGet(L_6x10, i, 1)); - cvmSet(&L_6x5, i, 2, cvmGet(L_6x10, i, 2)); - cvmSet(&L_6x5, i, 3, cvmGet(L_6x10, i, 3)); - cvmSet(&L_6x5, i, 4, cvmGet(L_6x10, i, 4)); + L_6x5.at(i, 0) = L_6x10.at(i, 0); + L_6x5.at(i, 1) = L_6x10.at(i, 1); + L_6x5.at(i, 2) = L_6x10.at(i, 2); + L_6x5.at(i, 3) = L_6x10.at(i, 3); + L_6x5.at(i, 4) = L_6x10.at(i, 4); } - cvSolve(&L_6x5, Rho, &B5, CV_SVD); + solve(L_6x5, Rho, B5, DECOMP_SVD); + CV_Assert(B5.ptr() == b5); if (b5[0] < 0) { betas[0] = sqrt(-b5[0]); @@ -479,19 +473,19 @@ void epnp::compute_rho(double * rho) rho[5] = dist2(cws[2], cws[3]); } -void epnp::compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho, - const double betas[4], CvMat * A, CvMat * b) +void epnp::compute_A_and_b_gauss_newton(const Mat& L_6x10, const Mat& Rho, + const double betas[4], Mat& A, Mat& b) { for(int i = 0; i < 6; i++) { - const double * rowL = l_6x10 + i * 10; - double * rowA = A->data.db + i * 4; + const double * rowL = L_6x10.ptr(i); + double * rowA = A.ptr(i); rowA[0] = 2 * rowL[0] * betas[0] + rowL[1] * betas[1] + rowL[3] * betas[2] + rowL[6] * betas[3]; rowA[1] = rowL[1] * betas[0] + 2 * rowL[2] * betas[1] + rowL[4] * betas[2] + rowL[7] * betas[3]; rowA[2] = rowL[3] * betas[0] + rowL[4] * betas[1] + 2 * rowL[5] * betas[2] + rowL[8] * betas[3]; rowA[3] = rowL[6] * betas[0] + rowL[7] * betas[1] + rowL[8] * betas[2] + 2 * rowL[9] * betas[3]; - cvmSet(b, i, 0, rho[i] - + b.at(i) = Rho.at(i) - ( rowL[0] * betas[0] * betas[0] + rowL[1] * betas[0] * betas[1] + @@ -503,33 +497,32 @@ void epnp::compute_A_and_b_gauss_newton(const double * l_6x10, const double * rh rowL[7] * betas[1] * betas[3] + rowL[8] * betas[2] * betas[3] + rowL[9] * betas[3] * betas[3] - )); + ); } } -void epnp::gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double betas[4]) +void epnp::gauss_newton(const Mat& L_6x10, const Mat& Rho, double betas[4]) { const int iterations_number = 5; double a[6*4] = {}, b[6] = {}, x[4] = {}; - CvMat A = cvMat(6, 4, CV_64F, a); - CvMat B = cvMat(6, 1, CV_64F, b); - CvMat X = cvMat(4, 1, CV_64F, x); + Mat A(6, 4, CV_64F, a); + Mat B(6, 1, CV_64F, b); + Mat X(4, 1, CV_64F, x); for(int k = 0; k < iterations_number; k++) { - compute_A_and_b_gauss_newton(L_6x10->data.db, Rho->data.db, - betas, &A, &B); - qr_solve(&A, &B, &X); + compute_A_and_b_gauss_newton(L_6x10, Rho, betas, A, B); + qr_solve(A, B, X); for(int i = 0; i < 4; i++) betas[i] += x[i]; } } -void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X) +void epnp::qr_solve(Mat& A, Mat& b, Mat& X) { - const int nr = A->rows; - const int nc = A->cols; + const int nr = A.rows; + const int nc = A.cols; if (nc <= 0 || nr <= 0) return; @@ -545,7 +538,7 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X) A2 = new double[nr]; } - double * pA = A->data.db, * ppAkk = pA; + double * pA = A.ptr(), * ppAkk = pA; for(int k = 0; k < nc; k++) { double * ppAik1 = ppAkk, eta = fabs(*ppAik1); @@ -597,7 +590,7 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X) } // b <- Qt b - double * ppAjj = pA, * pb = b->data.db; + double * ppAjj = pA, * pb = b.ptr(); for(int j = 0; j < nc; j++) { double * ppAij = ppAjj, tau = 0; @@ -617,7 +610,7 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X) } // X = R-1 b - double * pX = X->data.db; + double * pX = X.ptr(); pX[nc - 1] = pb[nc - 1] / A2[nc - 1]; for(int i = nc - 2; i >= 0; i--) { diff --git a/modules/3d/src/epnp.h b/modules/3d/src/epnp.h index a6db4b7078..3e047b3b04 100644 --- a/modules/3d/src/epnp.h +++ b/modules/3d/src/epnp.h @@ -6,7 +6,6 @@ #define epnp_h #include "precomp.hpp" -#include "opencv2/core/core_c.h" namespace cv { @@ -46,16 +45,16 @@ class epnp { double reprojection_error(const double R[3][3], const double t[3]); void choose_control_points(void); void compute_barycentric_coordinates(void); - void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v); + void fill_M(Mat& M, const int row, const double * alphas, const double u, const double v); void compute_ccs(const double * betas, const double * ut); void compute_pcs(void); void solve_for_sign(void); - void find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, double * betas); - void find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, double * betas); - void find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, double * betas); - void qr_solve(CvMat * A, CvMat * b, CvMat * X); + void find_betas_approx_1(const Mat& L_6x10, const Mat& Rho, double * betas); + void find_betas_approx_2(const Mat& L_6x10, const Mat& Rho, double * betas); + void find_betas_approx_3(const Mat& L_6x10, const Mat& Rho, double * betas); + void qr_solve(Mat& A, Mat& b, Mat& X); double dot(const double * v1, const double * v2); double dist2(const double * p1, const double * p2); @@ -63,9 +62,9 @@ class epnp { void compute_rho(double * rho); void compute_L_6x10(const double * ut, double * l_6x10); - void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]); - void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho, - const double cb[4], CvMat * A, CvMat * b); + void gauss_newton(const Mat& L_6x10, const Mat& Rho, double current_betas[4]); + void compute_A_and_b_gauss_newton(const Mat& L_6x10, const Mat& Rho, + const double cb[4], Mat& A, Mat& b); double compute_R_and_t(const double * ut, const double * betas, double R[3][3], double t[3]); diff --git a/modules/3d/src/levmarq.cpp b/modules/3d/src/levmarq.cpp index 0dfca5dd3b..c539585b51 100644 --- a/modules/3d/src/levmarq.cpp +++ b/modules/3d/src/levmarq.cpp @@ -76,133 +76,53 @@ namespace cv { +static void subMatrix(const Mat& src, Mat& dst, + const Mat& mask) +{ + CV_Assert(src.type() == CV_64F && dst.type() == CV_64F); + int m = src.rows, n = src.cols; + int i1 = 0, j1 = 0; + for(int i = 0; i < m; i++) + { + if(mask.at(i)) + { + const double* srcptr = src.ptr(i); + double* dstptr = dst.ptr(i1++); + + for(int j = j1 = 0; j < n; j++) + { + if(n < m || mask.at(j)) + dstptr[j1++] = srcptr[j]; + } + } + } +} + class LMSolverImpl CV_FINAL : public LMSolver { public: LMSolverImpl(const Ptr& _cb, int _maxIters, double _eps = FLT_EPSILON) - : cb(_cb), epsx(_eps), epsf(_eps), maxIters(_maxIters) + : cb(_cb), eps(_eps), maxIters(_maxIters) { - printInterval = 0; } - int run(InputOutputArray _param0) const CV_OVERRIDE + int run(InputOutputArray param0) const CV_OVERRIDE { - Mat param0 = _param0.getMat(), x, xd, r, rd, J, A, Ap, v, temp_d, d; - int ptype = param0.type(); - - CV_Assert( (param0.cols == 1 || param0.rows == 1) && (ptype == CV_32F || ptype == CV_64F)); - CV_Assert( cb ); - - int lx = param0.rows + param0.cols - 1; - param0.convertTo(x, CV_64F); - - if( x.cols != 1 ) - transpose(x, x); - - if( !cb->compute(x, r, J) ) - return -1; - double S = norm(r, NORM_L2SQR); - int nfJ = 2; - - mulTransposed(J, A, true); - gemm(J, r, 1, noArray(), 0, v, GEMM_1_T); - - Mat D = A.diag().clone(); - - const double Rlo = 0.25, Rhi = 0.75; - double lambda = 1, lc = 0.75; - int i, iter = 0; - - if( printInterval != 0 ) - { - printf("************************************************************************************\n"); - printf("\titr\tnfJ\t\tSUM(r^2)\t\tx\t\tdx\t\tl\t\tlc\n"); - printf("************************************************************************************\n"); - } - - for( ;; ) - { - CV_Assert( A.type() == CV_64F && A.rows == lx ); - A.copyTo(Ap); - for( i = 0; i < lx; i++ ) - Ap.at(i, i) += lambda*D.at(i); - solve(Ap, v, d, DECOMP_EIG); - subtract(x, d, xd); - if( !cb->compute(xd, rd, noArray()) ) - return -1; - nfJ++; - double Sd = norm(rd, NORM_L2SQR); - gemm(A, d, -1, v, 2, temp_d); - double dS = d.dot(temp_d); - double R = (S - Sd)/(fabs(dS) > DBL_EPSILON ? dS : 1); - - if( R > Rhi ) + return LMSolver::run(param0, noArray(), 0, + TermCriteria(TermCriteria::COUNT | TermCriteria::EPS, maxIters, eps), DECOMP_SVD, + [&](Mat& param, Mat* err, Mat* J)->bool { - lambda *= 0.5; - if( lambda < lc ) - lambda = 0; - } - else if( R < Rlo ) - { - // find new nu if R too low - double t = d.dot(v); - double nu = (Sd - S)/(fabs(t) > DBL_EPSILON ? t : 1) + 2; - nu = std::min(std::max(nu, 2.), 10.); - if( lambda == 0 ) - { - invert(A, Ap, DECOMP_EIG); - double maxval = DBL_EPSILON; - for( i = 0; i < lx; i++ ) - maxval = std::max(maxval, std::abs(Ap.at(i,i))); - lambda = lc = 1./maxval; - nu *= 0.5; - } - lambda *= nu; - } - - if( Sd < S ) - { - nfJ++; - S = Sd; - std::swap(x, xd); - if( !cb->compute(x, r, J) ) - return -1; - mulTransposed(J, A, true); - gemm(J, r, 1, noArray(), 0, v, GEMM_1_T); - } - - iter++; - bool proceed = iter < maxIters && norm(d, NORM_INF) >= epsx && norm(r, NORM_INF) >= epsf; - - if( printInterval != 0 && (iter % printInterval == 0 || iter == 1 || !proceed) ) - { - printf("%c%10d %10d %15.4e %16.4e %17.4e %16.4e %17.4e\n", - (proceed ? ' ' : '*'), iter, nfJ, S, x.at(0), d.at(0), lambda, lc); - } - - if(!proceed) - break; - } - - if( param0.size != x.size ) - transpose(x, x); - - x.convertTo(param0, ptype); - if( iter == maxIters ) - iter = -iter; - - return iter; + return cb->compute(param, err ? _OutputArray(*err) : _OutputArray(), + J ? _OutputArray(*J) : _OutputArray()); + }); } void setMaxIters(int iters) CV_OVERRIDE { CV_Assert(iters > 0); maxIters = iters; } int getMaxIters() const CV_OVERRIDE { return maxIters; } Ptr cb; - - double epsx; - double epsf; + double eps; int maxIters; - int printInterval; }; @@ -216,4 +136,167 @@ Ptr LMSolver::create(const Ptr& cb, int maxIters, return makePtr(cb, maxIters, eps); } +static int LMSolver_run(InputOutputArray _param0, InputArray _mask, + int nerrs, const TermCriteria& termcrit, + int solveMethod, bool LtoR, + std::function* cb, + std::function* cb_alt) +{ + int lambdaLg10 = -3; + Mat mask = _mask.getMat(); + Mat param0 = _param0.getMat(); + Mat x, xd, r, rd, J, A, Ap, v, temp_d, d, Am, vm, dm; + int ptype = param0.type(); + int maxIters = termcrit.type & TermCriteria::COUNT ? termcrit.maxCount : 1000; + double epsx = termcrit.type & TermCriteria::EPS ? termcrit.epsilon : 0, epsf = epsx; + + CV_Assert( (param0.cols == 1 || param0.rows == 1) && (ptype == CV_32F || ptype == CV_64F)); + CV_Assert( cb || cb_alt ); + + int lx = param0.rows + param0.cols - 1; + param0.convertTo(x, CV_64F); + d.create(lx, 1, CV_64F); + + CV_Assert(!mask.data || + (mask.depth() == CV_8U && + (mask.cols == 1 || mask.rows == 1) && + (mask.rows + mask.cols - 1 == lx))); + int lxm = mask.data ? countNonZero(mask) : lx; + if (lxm < lx) { + Am.create(lxm, lxm, CV_64F); + vm.create(lxm, 1, CV_64F); + } + + if( x.cols != 1 ) + transpose(x, x); + + A.create(lx, lx, CV_64F); + v.create(lx, 1, CV_64F); + + if (nerrs > 0) { + J.create(nerrs, lx, CV_64F); + r.create(nerrs, 1, CV_64F); + rd.create(nerrs, 1, CV_64F); + } + + double S = 0; + int nfJ = 1; + if (cb_alt) { + if( !(*cb_alt)(x, &v, &A, &S) ) + return -1; + completeSymm(A, LtoR); + } else { + if( !(*cb)(x, &r, &J) ) + return -1; + S = norm(r, NORM_L2SQR); + mulTransposed(J, A, true); + gemm(J, r, 1, noArray(), 0, v, GEMM_1_T); + } + + int i, iter = 0; + + for( ;; ) + { + CV_Assert( A.type() == CV_64F && A.rows == lx ); + A.copyTo(Ap); + double lambda = exp(lambdaLg10*log(10.)); + for( i = 0; i < lx; i++ ) + Ap.at(i, i) *= (1 + lambda); + if (lxm < lx) { + // remove masked-out rows & cols from JtJ and JtErr + subMatrix(Ap, Am, mask); + subMatrix(v, vm, mask); + solve(Am, vm, dm, solveMethod); + int j = 0; + // 'unpack' the param delta + for(i = j = 0; i < lx; i++) + d.at(i) = mask.at(i) != 0 ? dm.at(j++) : 0.; + } else { + solve(Ap, v, d, solveMethod); + } + subtract(x, d, xd); + + double Sd = 0.; + + if (cb_alt) { + if( !(*cb_alt)(xd, 0, 0, &Sd) ) + return -1; + } else { + if( !(*cb)(xd, &rd, 0) ) + return -1; + Sd = norm(rd, NORM_L2SQR); + } + + nfJ++; + if( Sd < S ) + { + nfJ++; + S = Sd; + lambdaLg10 = MAX(lambdaLg10-1, -16); + iter++; + std::swap(x, xd); + if (cb_alt) { + v.setZero(); + A.setZero(); + Sd = 0.; + if( !(*cb_alt)(x, &v, &A, &Sd) ) + return -1; + completeSymm(A, LtoR); + } else { + r.setZero(); + J.setZero(); + if( !(*cb)(x, &r, &J) ) + return -1; + mulTransposed(J, A, true); + gemm(J, r, 1, noArray(), 0, v, GEMM_1_T); + } + } else { + iter += lambdaLg10 == 16; + lambdaLg10 = MIN(lambdaLg10+1, 16); + } + + bool proceed = iter < maxIters && norm(d, NORM_INF) >= epsx && S >= epsf*epsf; + + /*if(lxm < lx) + { + printf("lambda=%g. delta:", lambda); + int j; + for(i = j = 0; i < lx; i++) { + double delta = d.at(i); + j += delta != 0; + if(j < 10) + printf(" %.2g", delta); + } + printf("\n"); + printf("%c %d %d, err=%g, param[0]=%g, d[0]=%g, lg10(lambda)=%d\n", + (proceed ? ' ' : '*'), iter, nfJ, S, x.at(0), d.at(0), lambdaLg10); + }*/ + if(!proceed) + break; + } + + if( param0.size() != x.size() ) + transpose(x, x); + + x.convertTo(param0, ptype); + if( iter == maxIters ) + iter = -iter; + + return iter; +} + +int LMSolver::run(InputOutputArray param, InputArray mask, int nerrs, + const TermCriteria& termcrit, int solveMethod, + std::function cb) +{ + return LMSolver_run(param, mask, nerrs, termcrit, solveMethod, true, &cb, 0); +} + +int LMSolver::runAlt(InputOutputArray param, InputArray mask, + const TermCriteria& termcrit, int solveMethod, bool LtoR, + std::function cb_alt) +{ + return LMSolver_run(param, mask, 0, termcrit, solveMethod, LtoR, 0, &cb_alt); +} + } diff --git a/modules/3d/src/triangulate.cpp b/modules/3d/src/triangulate.cpp index ee8d1217e6..458cd85927 100644 --- a/modules/3d/src/triangulate.cpp +++ b/modules/3d/src/triangulate.cpp @@ -40,85 +40,112 @@ //M*/ #include "precomp.hpp" -#include "opencv2/core/core_c.h" +#include +#include namespace cv { -// cvCorrectMatches function is Copyright (C) 2009, Jostein Austvik Jacobsen. -// cvTriangulatePoints function is derived from icvReconstructPointsFor3View, originally by Valery Mosyagin. +// correctMatches function is Copyright (C) 2009, Jostein Austvik Jacobsen. +// triangulatePoints function is derived from reconstructPointsFor3View, originally by Valery Mosyagin. // HZ, R. Hartley and A. Zisserman, Multiple View Geometry in Computer Vision, Cambridge Univ. Press, 2003. - - -// This method is the same as icvReconstructPointsFor3View, with only a few numbers adjusted for two-view geometry -static void -icvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2, CvMat* projPoints1, CvMat* projPoints2, CvMat* points4D) +// This method is the same as reconstructPointsFor3View, with only a few numbers adjusted for two-view geometry +void triangulatePoints( InputArray _P1, InputArray _P2, + InputArray _points1, InputArray _points2, + OutputArray _points4D ) { - if( projMatr1 == 0 || projMatr2 == 0 || - projPoints1 == 0 || projPoints2 == 0 || - points4D == 0) - CV_Error( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); + CV_INSTRUMENT_REGION(); - if( !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) || - !CV_IS_MAT(projPoints1) || !CV_IS_MAT(projPoints2) || - !CV_IS_MAT(points4D) ) - CV_Error( CV_StsUnsupportedFormat, "Input parameters must be matrices" ); + Mat points1 = _points1.getMat(), points2 = _points2.getMat(); + int depth1 = points1.depth(), depth2 = points2.depth(); + const float *p1f = depth1 == CV_32F ? points1.ptr() : 0; + const float *p2f = depth2 == CV_32F ? points2.ptr() : 0; + const double *p1d = depth1 == CV_64F ? points1.ptr() : 0; + const double *p2d = depth2 == CV_64F ? points2.ptr() : 0; + int pstep1, ystep1, pstep2, ystep2, npoints1, npoints2; - int numPoints = projPoints1->cols; + CV_Assert(depth1 == depth2 && (depth1 == CV_32F || depth1 == CV_64F)); - if( numPoints < 1 ) - CV_Error( CV_StsOutOfRange, "Number of points must be more than zero" ); - - if( projPoints2->cols != numPoints || points4D->cols != numPoints ) - CV_Error( CV_StsUnmatchedSizes, "Number of points must be the same" ); - - if( projPoints1->rows != 2 || projPoints2->rows != 2) - CV_Error( CV_StsUnmatchedSizes, "Number of proj points coordinates must be == 2" ); - - if( points4D->rows != 4 ) - CV_Error( CV_StsUnmatchedSizes, "Number of world points coordinates must be == 4" ); - - if( projMatr1->cols != 4 || projMatr1->rows != 3 || - projMatr2->cols != 4 || projMatr2->rows != 3) - CV_Error( CV_StsUnmatchedSizes, "Size of projection matrices must be 3x4" ); - - // preallocate SVD matrices on stack - cv::Matx matrA; - cv::Matx matrU; - cv::Matx matrW; - cv::Matx matrV; - - CvMat* projPoints[2] = {projPoints1, projPoints2}; - CvMat* projMatrs[2] = {projMatr1, projMatr2}; - - /* Solve system for each point */ - for( int i = 0; i < numPoints; i++ )/* For each point */ + if ((points1.rows == 1 || points1.cols == 1) && points1.channels() == 2) { - /* Fill matrix for current point */ - for( int j = 0; j < 2; j++ )/* For each view */ - { - double x,y; - x = cvmGet(projPoints[j],0,i); - y = cvmGet(projPoints[j],1,i); - for( int k = 0; k < 4; k++ ) - { - matrA(j*2+0, k) = x * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],0,k); - matrA(j*2+1, k) = y * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],1,k); - } - } - /* Solve system for current point */ - cv::SVD::compute(matrA, matrW, matrU, matrV); + npoints1 = points1.rows + points1.cols - 1; + ystep1 = 1; + pstep1 = points1.rows == 1 ? 2 : (int)(points1.step/points1.elemSize()); + } + else + { + npoints1 = points1.cols; + ystep1 = (int)(points1.step/points1.elemSize()); + pstep1 = 1; + } - /* Copy computed point */ - cvmSet(points4D,0,i,matrV(3,0));/* X */ - cvmSet(points4D,1,i,matrV(3,1));/* Y */ - cvmSet(points4D,2,i,matrV(3,2));/* Z */ - cvmSet(points4D,3,i,matrV(3,3));/* W */ + if ((points2.rows == 1 || points2.cols == 1) && points2.channels() == 2) + { + npoints2 = points2.rows + points2.cols - 1; + ystep2 = 1; + pstep2 = points2.rows == 1 ? 2 : (int)(points2.step/points2.elemSize()); + } + else + { + npoints2 = points2.cols; + ystep2 = (int)(points2.step/points2.elemSize()); + pstep2 = 1; + } + + CV_Assert(npoints1 == npoints2); + + _points4D.create(4, npoints1, depth1); + Mat points4D = _points4D.getMat(); + + Matx matrA; + Matx matrU; + Matx matrW; + Matx matrV; + size_t step4 = 4*sizeof(double); + Matx P1; + Matx P2; + _P1.getMat().convertTo(P1, CV_64F); + _P2.getMat().convertTo(P2, CV_64F); + + // Solve system for each point + for( int i = 0; i < npoints1; i++ ) + { + // Fill matrix for current point + double x1 = p1f ? (double)p1f[pstep1*i] : p1d[pstep1*i]; + double y1 = p1f ? (double)p1f[pstep1*i + ystep1] : p1d[pstep1*i + ystep1]; + double x2 = p2f ? (double)p2f[pstep2*i] : p2d[pstep2*i]; + double y2 = p2f ? (double)p2f[pstep2*i + ystep2] : p2d[pstep2*i + ystep2]; + + for(int k = 0; k < 4; k++) + { + matrA(k, 0) = x1*P1(2, k) - P1(0, k); + matrA(k, 1) = y1*P1(2, k) - P1(1, k); + matrA(k, 2) = x2*P2(2, k) - P2(0, k); + matrA(k, 3) = y2*P2(2, k) - P2(1, k); + } + + // Solve system for current point + hal::SVD64f(matrA.val, step4, matrW.val, matrU.val, step4, matrV.val, step4, 4, 4, 4); + + // Copy computed point + if(depth1 == CV_32F) + { + points4D.at(0, i) = (float)matrV(3, 0); + points4D.at(1, i) = (float)matrV(3, 1); + points4D.at(2, i) = (float)matrV(3, 2); + points4D.at(3, i) = (float)matrV(3, 3); + } + else + { + points4D.at(0, i) = matrV(3, 0); + points4D.at(1, i) = matrV(3, 1); + points4D.at(2, i) = matrV(3, 2); + points4D.at(3, i) = matrV(3, 3); + } } } - /* * The Optimal Triangulation Method (see HZ for details) * For each given point correspondence points1[i] <-> points2[i], and a fundamental matrix F, @@ -127,182 +154,118 @@ icvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2, CvMat* projPoints1, CvM * is the geometric distance between points a and b) subject to the epipolar constraint * new_points2' * F * new_points1 = 0. * - * F_ : 3x3 fundamental matrix - * points1_ : 1xN matrix containing the first set of points - * points2_ : 1xN matrix containing the second set of points - * new_points1 : the optimized points1_. if this is NULL, the corrected points are placed back in points1_ - * new_points2 : the optimized points2_. if this is NULL, the corrected points are placed back in points2_ + * _F : 3x3 fundamental matrix + * _points1 : 1xN matrix containing the first set of points + * _points2 : 1xN matrix containing the second set of points + * _newPoints1 : the optimized _points1. + * _newPoints2 : the optimized -points2. */ -static void -icvCorrectMatches(CvMat *F_, CvMat *points1_, CvMat *points2_, CvMat *new_points1, CvMat *new_points2) +void correctMatches( InputArray _F, InputArray _points1, InputArray _points2, + OutputArray _newPoints1, OutputArray _newPoints2 ) { - cv::Ptr tmp33; - cv::Ptr tmp31, tmp31_2; - cv::Ptr T1i, T2i; - cv::Ptr R1, R2; - cv::Ptr TFT, TFTt, RTFTR; - cv::Ptr U, S, V; - cv::Ptr e1, e2; - cv::Ptr polynomial; - cv::Ptr result; - cv::Ptr points1, points2; - cv::Ptr F; + CV_INSTRUMENT_REGION(); - if (!CV_IS_MAT(F_) || !CV_IS_MAT(points1_) || !CV_IS_MAT(points2_) ) - CV_Error( CV_StsUnsupportedFormat, "Input parameters must be matrices" ); - if (!( F_->cols == 3 && F_->rows == 3)) - CV_Error( CV_StsUnmatchedSizes, "The fundamental matrix must be a 3x3 matrix"); - if (!(((F_->type & CV_MAT_TYPE_MASK) >> 3) == 0 )) - CV_Error( CV_StsUnsupportedFormat, "The fundamental matrix must be a single-channel matrix" ); - if (!(points1_->rows == 1 && points2_->rows == 1 && points1_->cols == points2_->cols)) - CV_Error( CV_StsUnmatchedSizes, "The point-matrices must have one row, and an equal number of columns" ); - if (((points1_->type & CV_MAT_TYPE_MASK) >> 3) != 1 ) + Mat points1 = _points1.getMat(), points2 = _points2.getMat(); + + int depth1 = points1.depth(), depth2 = points2.depth(); + CV_Assert((depth1 == CV_32F || depth1 == CV_64F) && depth1 == depth2); + + CV_Assert(points1.size() == points2.size()); + CV_Assert(points1.rows == 1 || points1.cols == 1); + if (points1.channels() != 2) CV_Error( CV_StsUnmatchedSizes, "The first set of points must contain two channels; one for x and one for y" ); - if (((points2_->type & CV_MAT_TYPE_MASK) >> 3) != 1 ) + if (points2.channels() != 2) CV_Error( CV_StsUnmatchedSizes, "The second set of points must contain two channels; one for x and one for y" ); - if (new_points1 != NULL) { - CV_Assert(CV_IS_MAT(new_points1)); - if (new_points1->cols != points1_->cols || new_points1->rows != 1) - CV_Error( CV_StsUnmatchedSizes, "The first output matrix must have the same dimensions as the input matrices" ); - if (CV_MAT_CN(new_points1->type) != 2) - CV_Error( CV_StsUnsupportedFormat, "The first output matrix must have two channels; one for x and one for y" ); - } - if (new_points2 != NULL) { - CV_Assert(CV_IS_MAT(new_points2)); - if (new_points2->cols != points2_->cols || new_points2->rows != 1) - CV_Error( CV_StsUnmatchedSizes, "The second output matrix must have the same dimensions as the input matrices" ); - if (CV_MAT_CN(new_points2->type) != 2) - CV_Error( CV_StsUnsupportedFormat, "The second output matrix must have two channels; one for x and one for y" ); - } + + _newPoints1.create(points1.size(), points1.type()); + _newPoints2.create(points2.size(), points2.type()); + Mat newPoints1 = _newPoints1.getMat(), newPoints2 = _newPoints2.getMat(); + + Matx33d F, U, Vt; + Matx31d S; + int npoints = points1.rows + points1.cols - 1; // Make sure F uses double precision - F.reset(cvCreateMat(3,3,CV_64FC1)); - cvConvert(F_, F); + _F.getMat().convertTo(F, CV_64F); - // Make sure points1 uses double precision - points1.reset(cvCreateMat(points1_->rows,points1_->cols,CV_64FC2)); - cvConvert(points1_, points1); - - // Make sure points2 uses double precision - points2.reset(cvCreateMat(points2_->rows,points2_->cols,CV_64FC2)); - cvConvert(points2_, points2); - - tmp33.reset(cvCreateMat(3,3,CV_64FC1)); - tmp31.reset(cvCreateMat(3,1,CV_64FC1)), tmp31_2.reset(cvCreateMat(3,1,CV_64FC1)); - T1i.reset(cvCreateMat(3,3,CV_64FC1)), T2i.reset(cvCreateMat(3,3,CV_64FC1)); - R1.reset(cvCreateMat(3,3,CV_64FC1)), R2.reset(cvCreateMat(3,3,CV_64FC1)); - TFT.reset(cvCreateMat(3,3,CV_64FC1)), TFTt.reset(cvCreateMat(3,3,CV_64FC1)), RTFTR.reset(cvCreateMat(3,3,CV_64FC1)); - U.reset(cvCreateMat(3,3,CV_64FC1)); - S.reset(cvCreateMat(3,3,CV_64FC1)); - V.reset(cvCreateMat(3,3,CV_64FC1)); - e1.reset(cvCreateMat(3,1,CV_64FC1)), e2.reset(cvCreateMat(3,1,CV_64FC1)); - - double x1, y1, x2, y2; - double scale; - double f1, f2, a, b, c, d; - polynomial.reset(cvCreateMat(1,7,CV_64FC1)); - result.reset(cvCreateMat(1,6,CV_64FC2)); - double t_min, s_val, t, s; - for (int p = 0; p < points1->cols; ++p) { + for (int p = 0; p < npoints; ++p) { // Replace F by T2-t * F * T1-t - x1 = points1->data.db[p*2]; - y1 = points1->data.db[p*2+1]; - x2 = points2->data.db[p*2]; - y2 = points2->data.db[p*2+1]; + double x1, y1, x2, y2; + if (depth1 == CV_32F) { + Point2f p1 = points1.at(p); + Point2f p2 = points2.at(p); + x1 = p1.x; y1 = p1.y; + x2 = p2.x; y2 = p2.y; + } else { + Point2d p1 = points1.at(p); + Point2d p2 = points2.at(p); + x1 = p1.x; y1 = p1.y; + x2 = p2.x; y2 = p2.y; + } - cvSetZero(T1i); - cvSetReal2D(T1i,0,0,1); - cvSetReal2D(T1i,1,1,1); - cvSetReal2D(T1i,2,2,1); - cvSetReal2D(T1i,0,2,x1); - cvSetReal2D(T1i,1,2,y1); - cvSetZero(T2i); - cvSetReal2D(T2i,0,0,1); - cvSetReal2D(T2i,1,1,1); - cvSetReal2D(T2i,2,2,1); - cvSetReal2D(T2i,0,2,x2); - cvSetReal2D(T2i,1,2,y2); - cvGEMM(T2i,F,1,0,0,tmp33,CV_GEMM_A_T); - cvSetZero(TFT); - cvGEMM(tmp33,T1i,1,0,0,TFT); + Matx33d T1i(1, 0, x1, + 0, 1, y1, + 0, 0, 1); + Matx33d T2i(1, 0, x2, + 0, 1, y2, + 0, 0, 1); + Matx33d TFT = T2i.t()*F*T1i; // Compute the right epipole e1 from F * e1 = 0 - cvSetZero(U); - cvSetZero(S); - cvSetZero(V); - cvSVD(TFT,S,U,V); - scale = sqrt(cvGetReal2D(V,0,2)*cvGetReal2D(V,0,2) + cvGetReal2D(V,1,2)*cvGetReal2D(V,1,2)); - cvSetReal2D(e1,0,0,cvGetReal2D(V,0,2)/scale); - cvSetReal2D(e1,1,0,cvGetReal2D(V,1,2)/scale); - cvSetReal2D(e1,2,0,cvGetReal2D(V,2,2)/scale); - if (cvGetReal2D(e1,2,0) < 0) { - cvSetReal2D(e1,0,0,-cvGetReal2D(e1,0,0)); - cvSetReal2D(e1,1,0,-cvGetReal2D(e1,1,0)); - cvSetReal2D(e1,2,0,-cvGetReal2D(e1,2,0)); - } + SVDecomp(TFT, S, U, Vt); + double scale = sqrt(Vt(2, 0)*Vt(2, 0) + Vt(2, 1)*Vt(2, 1)); + + Vec3d e1(Vt(2, 0)/scale, Vt(2, 1)/scale, Vt(2, 2)/scale); + if (e1(2) < 0) + e1 = -e1; // Compute the left epipole e2 from e2' * F = 0 => F' * e2 = 0 - cvSetZero(TFTt); - cvTranspose(TFT, TFTt); - cvSetZero(U); - cvSetZero(S); - cvSetZero(V); - cvSVD(TFTt,S,U,V); - cvSetZero(e2); - scale = sqrt(cvGetReal2D(V,0,2)*cvGetReal2D(V,0,2) + cvGetReal2D(V,1,2)*cvGetReal2D(V,1,2)); - cvSetReal2D(e2,0,0,cvGetReal2D(V,0,2)/scale); - cvSetReal2D(e2,1,0,cvGetReal2D(V,1,2)/scale); - cvSetReal2D(e2,2,0,cvGetReal2D(V,2,2)/scale); - if (cvGetReal2D(e2,2,0) < 0) { - cvSetReal2D(e2,0,0,-cvGetReal2D(e2,0,0)); - cvSetReal2D(e2,1,0,-cvGetReal2D(e2,1,0)); - cvSetReal2D(e2,2,0,-cvGetReal2D(e2,2,0)); - } + scale = sqrt(U(0, 2)*U(0, 2) + U(1, 2)*U(1, 2)); + + Vec3d e2(U(0, 2)/scale, U(1, 2)/scale, U(2, 2)/scale); + if (e2(2) < 0) + e2 = -e2; // Replace F by R2 * F * R1' - cvSetZero(R1); - cvSetReal2D(R1,0,0,cvGetReal2D(e1,0,0)); - cvSetReal2D(R1,0,1,cvGetReal2D(e1,1,0)); - cvSetReal2D(R1,1,0,-cvGetReal2D(e1,1,0)); - cvSetReal2D(R1,1,1,cvGetReal2D(e1,0,0)); - cvSetReal2D(R1,2,2,1); - cvSetZero(R2); - cvSetReal2D(R2,0,0,cvGetReal2D(e2,0,0)); - cvSetReal2D(R2,0,1,cvGetReal2D(e2,1,0)); - cvSetReal2D(R2,1,0,-cvGetReal2D(e2,1,0)); - cvSetReal2D(R2,1,1,cvGetReal2D(e2,0,0)); - cvSetReal2D(R2,2,2,1); - cvGEMM(R2,TFT,1,0,0,tmp33); - cvGEMM(tmp33,R1,1,0,0,RTFTR,CV_GEMM_B_T); + Matx33d R1_t(e1(0), -e1(1), 0, + e1(1), e1(0), 0, + 0, 0, 1); + Matx33d R2(e2(0), e2(1), 0, + -e2(1), e2(0), 0, + 0, 0, 1); + Matx33d RTFTR = R2*TFT*R1_t; // Set f1 = e1(3), f2 = e2(3), a = F22, b = F23, c = F32, d = F33 - f1 = cvGetReal2D(e1,2,0); - f2 = cvGetReal2D(e2,2,0); - a = cvGetReal2D(RTFTR,1,1); - b = cvGetReal2D(RTFTR,1,2); - c = cvGetReal2D(RTFTR,2,1); - d = cvGetReal2D(RTFTR,2,2); + double f1 = e1(2); + double f2 = e2(2); + double a = RTFTR(1,1); + double b = RTFTR(1,2); + double c = RTFTR(2,1); + double d = RTFTR(2,2); // Form the polynomial g(t) = k6*t^6 + k5*t^5 + k4*t^4 + k3*t^3 + k2*t^2 + k1*t + k0 // from f1, f2, a, b, c and d - cvSetReal2D(polynomial,0,6,( +b*c*c*f1*f1*f1*f1*a-a*a*d*f1*f1*f1*f1*c )); - cvSetReal2D(polynomial,0,5,( +f2*f2*f2*f2*c*c*c*c+2*a*a*f2*f2*c*c-a*a*d*d*f1*f1*f1*f1+b*b*c*c*f1*f1*f1*f1+a*a*a*a )); - cvSetReal2D(polynomial,0,4,( +4*a*a*a*b+2*b*c*c*f1*f1*a+4*f2*f2*f2*f2*c*c*c*d+4*a*b*f2*f2*c*c+4*a*a*f2*f2*c*d-2*a*a*d*f1*f1*c-a*d*d*f1*f1*f1*f1*b+b*b*c*f1*f1*f1*f1*d )); - cvSetReal2D(polynomial,0,3,( +6*a*a*b*b+6*f2*f2*f2*f2*c*c*d*d+2*b*b*f2*f2*c*c+2*a*a*f2*f2*d*d-2*a*a*d*d*f1*f1+2*b*b*c*c*f1*f1+8*a*b*f2*f2*c*d )); - cvSetReal2D(polynomial,0,2,( +4*a*b*b*b+4*b*b*f2*f2*c*d+4*f2*f2*f2*f2*c*d*d*d-a*a*d*c+b*c*c*a+4*a*b*f2*f2*d*d-2*a*d*d*f1*f1*b+2*b*b*c*f1*f1*d )); - cvSetReal2D(polynomial,0,1,( +f2*f2*f2*f2*d*d*d*d+b*b*b*b+2*b*b*f2*f2*d*d-a*a*d*d+b*b*c*c )); - cvSetReal2D(polynomial,0,0,( -a*d*d*b+b*b*c*d )); + Vec polynomial( + -a*d*d*b+b*b*c*d, + +f2*f2*f2*f2*d*d*d*d+b*b*b*b+2*b*b*f2*f2*d*d-a*a*d*d+b*b*c*c, + +4*a*b*b*b+4*b*b*f2*f2*c*d+4*f2*f2*f2*f2*c*d*d*d-a*a*d*c+b*c*c*a+4*a*b*f2*f2*d*d-2*a*d*d*f1*f1*b+2*b*b*c*f1*f1*d, + +6*a*a*b*b+6*f2*f2*f2*f2*c*c*d*d+2*b*b*f2*f2*c*c+2*a*a*f2*f2*d*d-2*a*a*d*d*f1*f1+2*b*b*c*c*f1*f1+8*a*b*f2*f2*c*d, + +4*a*a*a*b+2*b*c*c*f1*f1*a+4*f2*f2*f2*f2*c*c*c*d+4*a*b*f2*f2*c*c+4*a*a*f2*f2*c*d-2*a*a*d*f1*f1*c-a*d*d*f1*f1*f1*f1*b+b*b*c*f1*f1*f1*f1*d, + +f2*f2*f2*f2*c*c*c*c+2*a*a*f2*f2*c*c-a*a*d*d*f1*f1*f1*f1+b*b*c*c*f1*f1*f1*f1+a*a*a*a, + +b*c*c*f1*f1*f1*f1*a-a*a*d*f1*f1*f1*f1*c); // Solve g(t) for t to get 6 roots - cvSetZero(result); - cvSolvePoly(polynomial, result, 100, 20); + double rdata[6*2]; + Mat result(6, 1, CV_64FC2, rdata); + solvePoly(polynomial, result); // Evaluate the cost function s(t) at the real part of the 6 roots - t_min = DBL_MAX; - s_val = 1./(f1*f1) + (c*c)/(a*a+f2*f2*c*c); + double t_min = DBL_MAX; + double s_val = 1./(f1*f1) + (c*c)/(a*a+f2*f2*c*c); for (int ti = 0; ti < 6; ++ti) { - t = result->data.db[2*ti]; - s = (t*t)/(1 + f1*f1*t*t) + ((c*t + d)*(c*t + d))/((a*t + b)*(a*t + b) + f2*f2*(c*t + d)*(c*t + d)); + Vec2d root_i = result.at(ti); + double t = root_i(0); + double s = (t*t)/(1 + f1*f1*t*t) + ((c*t + d)*(c*t + d))/((a*t + b)*(a*t + b) + f2*f2*(c*t + d)*(c*t + d)); if (s < s_val) { s_val = s; t_min = t; @@ -310,83 +273,27 @@ icvCorrectMatches(CvMat *F_, CvMat *points1_, CvMat *points2_, CvMat *new_points } // find the optimal x1 and y1 as the points on l1 and l2 closest to the origin - tmp31->data.db[0] = t_min*t_min*f1; - tmp31->data.db[1] = t_min; - tmp31->data.db[2] = t_min*t_min*f1*f1+1; - tmp31->data.db[0] /= tmp31->data.db[2]; - tmp31->data.db[1] /= tmp31->data.db[2]; - tmp31->data.db[2] /= tmp31->data.db[2]; - cvGEMM(T1i,R1,1,0,0,tmp33,CV_GEMM_B_T); - cvGEMM(tmp33,tmp31,1,0,0,tmp31_2); - x1 = tmp31_2->data.db[0]; - y1 = tmp31_2->data.db[1]; + scale = t_min*t_min*f1*f1+1; + Vec3d tmp31(t_min*t_min*f1/scale, t_min/scale, 1); + Vec3d tmp31_2 = T1i*(R1_t*tmp31); + x1 = tmp31_2(0); + y1 = tmp31_2(1); - tmp31->data.db[0] = f2*pow(c*t_min+d,2); - tmp31->data.db[1] = -(a*t_min+b)*(c*t_min+d); - tmp31->data.db[2] = f2*f2*pow(c*t_min+d,2) + pow(a*t_min+b,2); - tmp31->data.db[0] /= tmp31->data.db[2]; - tmp31->data.db[1] /= tmp31->data.db[2]; - tmp31->data.db[2] /= tmp31->data.db[2]; - cvGEMM(T2i,R2,1,0,0,tmp33,CV_GEMM_B_T); - cvGEMM(tmp33,tmp31,1,0,0,tmp31_2); - x2 = tmp31_2->data.db[0]; - y2 = tmp31_2->data.db[1]; + scale = f2*f2*(c*t_min+d)*(c*t_min+d) + (a*t_min+b)*(a*t_min+b); + tmp31 = Vec3d(f2*(c*t_min+d)*(c*t_min+d)/scale, -(a*t_min+b)*(c*t_min+d)/scale, 1); + tmp31_2 = T2i*(R2.t()*tmp31); + x2 = tmp31_2(0); + y2 = tmp31_2(1); // Return the points in the matrix format that the user wants - points1->data.db[p*2] = x1; - points1->data.db[p*2+1] = y1; - points2->data.db[p*2] = x2; - points2->data.db[p*2+1] = y2; + if (depth1 == CV_32F) { + newPoints1.at(p) = Point2f((float)x1, (float)y1); + newPoints2.at(p) = Point2f((float)x2, (float)y2); + } else { + newPoints1.at(p) = Point2d(x1, y1); + newPoints2.at(p) = Point2d(x2, y2); + } } - - if( new_points1 ) - cvConvert( points1, new_points1 ); - if( new_points2 ) - cvConvert( points2, new_points2 ); -} - -void triangulatePoints( InputArray _projMatr1, InputArray _projMatr2, - InputArray _projPoints1, InputArray _projPoints2, - OutputArray _points4D ) -{ - CV_INSTRUMENT_REGION(); - - Mat matr1 = _projMatr1.getMat(), matr2 = _projMatr2.getMat(); - Mat points1 = _projPoints1.getMat(), points2 = _projPoints2.getMat(); - - if((points1.rows == 1 || points1.cols == 1) && points1.channels() == 2) - points1 = points1.reshape(1, static_cast(points1.total())).t(); - - if((points2.rows == 1 || points2.cols == 1) && points2.channels() == 2) - points2 = points2.reshape(1, static_cast(points2.total())).t(); - - CvMat cvMatr1 = cvMat(matr1), cvMatr2 = cvMat(matr2); - CvMat cvPoints1 = cvMat(points1), cvPoints2 = cvMat(points2); - - _points4D.create(4, points1.cols, points1.type()); - Mat cvPoints4D_ = _points4D.getMat(); - CvMat cvPoints4D = cvMat(cvPoints4D_); - - icvTriangulatePoints(&cvMatr1, &cvMatr2, &cvPoints1, &cvPoints2, &cvPoints4D); -} - -void correctMatches( InputArray _F, InputArray _points1, InputArray _points2, - OutputArray _newPoints1, OutputArray _newPoints2 ) -{ - CV_INSTRUMENT_REGION(); - - Mat F = _F.getMat(); - Mat points1 = _points1.getMat(), points2 = _points2.getMat(); - - CvMat cvPoints1 = cvMat(points1), cvPoints2 = cvMat(points2); - CvMat cvF = cvMat(F); - - _newPoints1.create(points1.size(), points1.type()); - _newPoints2.create(points2.size(), points2.type()); - Mat cvNewPoints1_ = _newPoints1.getMat(), cvNewPoints2_ = _newPoints2.getMat(); - CvMat cvNewPoints1 = cvMat(cvNewPoints1_), cvNewPoints2 = cvMat(cvNewPoints2_); - - icvCorrectMatches(&cvF, &cvPoints1, &cvPoints2, &cvNewPoints1, &cvNewPoints2); } } diff --git a/modules/3d/test/test_solvepnp_ransac.cpp b/modules/3d/test/test_solvepnp_ransac.cpp index 43b90dff92..60c765b8af 100644 --- a/modules/3d/test/test_solvepnp_ransac.cpp +++ b/modules/3d/test/test_solvepnp_ransac.cpp @@ -1960,6 +1960,7 @@ TEST(Calib3d_SolvePnPRansac, minPoints) TEST(Calib3d_SolvePnPRansac, inputShape) { + double eps = 2e-6; //https://github.com/opencv/opencv/issues/14423 Mat matK = Mat::eye(3,3,CV_64FC1); Mat distCoeff = Mat::zeros(1,5,CV_64FC1); @@ -1987,8 +1988,8 @@ TEST(Calib3d_SolvePnPRansac, inputShape) Mat rvec, Tvec; solvePnPRansac(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec); - EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-6); - EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-6); + EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), eps); + EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), eps); } { //1xN 3-channel @@ -2012,8 +2013,8 @@ TEST(Calib3d_SolvePnPRansac, inputShape) Mat rvec, Tvec; solvePnPRansac(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec); - EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-6); - EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-6); + EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), eps); + EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), eps); } { //Nx1 3-channel @@ -2037,8 +2038,8 @@ TEST(Calib3d_SolvePnPRansac, inputShape) Mat rvec, Tvec; solvePnPRansac(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec); - EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-6); - EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-6); + EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), eps); + EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), eps); } { //vector @@ -2056,8 +2057,8 @@ TEST(Calib3d_SolvePnPRansac, inputShape) Mat rvec, Tvec; solvePnPRansac(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec); - EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-6); - EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-6); + EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), eps); + EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), eps); } { //vector @@ -2075,8 +2076,8 @@ TEST(Calib3d_SolvePnPRansac, inputShape) Mat rvec, Tvec; solvePnPRansac(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec); - EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-6); - EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-6); + EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), eps); + EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), eps); } } diff --git a/modules/calib/include/opencv2/calib.hpp b/modules/calib/include/opencv2/calib.hpp index efcdd5d9e1..34c2f5c997 100644 --- a/modules/calib/include/opencv2/calib.hpp +++ b/modules/calib/include/opencv2/calib.hpp @@ -843,7 +843,7 @@ CV_EXPORTS_AS(calibrateCameraExtended) double calibrateCamera( InputArrayOfArray OutputArray stdDeviationsExtrinsics, OutputArray perViewErrors, int flags = 0, TermCriteria criteria = TermCriteria( - TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) ); + TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON) ); /** @overload */ CV_EXPORTS_W double calibrateCamera( InputArrayOfArrays objectPoints, @@ -851,7 +851,7 @@ CV_EXPORTS_W double calibrateCamera( InputArrayOfArrays objectPoints, InputOutputArray cameraMatrix, InputOutputArray distCoeffs, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, int flags = 0, TermCriteria criteria = TermCriteria( - TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) ); + TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON) ); /** @brief Finds the camera intrinsic and extrinsic parameters from several views of a calibration pattern. @@ -920,7 +920,7 @@ CV_EXPORTS_AS(calibrateCameraROExtended) double calibrateCameraRO( InputArrayOfA OutputArray stdDeviationsObjPoints, OutputArray perViewErrors, int flags = 0, TermCriteria criteria = TermCriteria( - TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) ); + TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON) ); /** @overload */ CV_EXPORTS_W double calibrateCameraRO( InputArrayOfArrays objectPoints, @@ -929,7 +929,7 @@ CV_EXPORTS_W double calibrateCameraRO( InputArrayOfArrays objectPoints, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, OutputArray newObjPoints, int flags = 0, TermCriteria criteria = TermCriteria( - TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) ); + TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON) ); /** @brief Computes useful camera characteristics from the camera intrinsic matrix. @@ -1085,7 +1085,7 @@ CV_EXPORTS_AS(stereoCalibrateExtended) double stereoCalibrate( InputArrayOfArray InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2, Size imageSize, InputOutputArray R,InputOutputArray T, OutputArray E, OutputArray F, OutputArray perViewErrors, int flags = CALIB_FIX_INTRINSIC, - TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) ); + TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6) ); /// @overload CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints, @@ -1094,7 +1094,7 @@ CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints, InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2, Size imageSize, OutputArray R,OutputArray T, OutputArray E, OutputArray F, int flags = CALIB_FIX_INTRINSIC, - TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) ); + TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6) ); /** @brief Computes Hand-Eye calibration: \f$_{}^{g}\textrm{T}_c\f$ diff --git a/modules/calib/src/calibration.cpp b/modules/calib/src/calibration.cpp index f4db51d324..a80d38ec37 100644 --- a/modules/calib/src/calibration.cpp +++ b/modules/calib/src/calibration.cpp @@ -41,10 +41,10 @@ //M*/ #include "precomp.hpp" -#include "opencv2/imgproc/imgproc_c.h" #include "distortion_model.hpp" #include #include +#include /* This is straight-forward port v3 of Matlab calibration engine by Jean-Yves Bouguet @@ -56,1004 +56,6 @@ namespace cv { -// reimplementation of dAB.m -static void cvCalcMatMulDeriv( const CvMat* A, const CvMat* B, CvMat* dABdA, CvMat* dABdB ) -{ - int i, j, M, N, L; - int bstep; - - CV_Assert( CV_IS_MAT(A) && CV_IS_MAT(B) ); - CV_Assert( CV_ARE_TYPES_EQ(A, B) && - (CV_MAT_TYPE(A->type) == CV_32F || CV_MAT_TYPE(A->type) == CV_64F) ); - CV_Assert( A->cols == B->rows ); - - M = A->rows; - L = A->cols; - N = B->cols; - bstep = B->step/CV_ELEM_SIZE(B->type); - - if( dABdA ) - { - CV_Assert( CV_ARE_TYPES_EQ(A, dABdA) && - dABdA->rows == A->rows*B->cols && dABdA->cols == A->rows*A->cols ); - } - - if( dABdB ) - { - CV_Assert( CV_ARE_TYPES_EQ(A, dABdB) && - dABdB->rows == A->rows*B->cols && dABdB->cols == B->rows*B->cols ); - } - - if( CV_MAT_TYPE(A->type) == CV_32F ) - { - for( i = 0; i < M*N; i++ ) - { - int i1 = i / N, i2 = i % N; - - if( dABdA ) - { - float* dcda = (float*)(dABdA->data.ptr + dABdA->step*i); - const float* b = (const float*)B->data.ptr + i2; - - for( j = 0; j < M*L; j++ ) - dcda[j] = 0; - for( j = 0; j < L; j++ ) - dcda[i1*L + j] = b[j*bstep]; - } - - if( dABdB ) - { - float* dcdb = (float*)(dABdB->data.ptr + dABdB->step*i); - const float* a = (const float*)(A->data.ptr + A->step*i1); - - for( j = 0; j < L*N; j++ ) - dcdb[j] = 0; - for( j = 0; j < L; j++ ) - dcdb[j*N + i2] = a[j]; - } - } - } - else - { - for( i = 0; i < M*N; i++ ) - { - int i1 = i / N, i2 = i % N; - - if( dABdA ) - { - double* dcda = (double*)(dABdA->data.ptr + dABdA->step*i); - const double* b = (const double*)B->data.ptr + i2; - - for( j = 0; j < M*L; j++ ) - dcda[j] = 0; - for( j = 0; j < L; j++ ) - dcda[i1*L + j] = b[j*bstep]; - } - - if( dABdB ) - { - double* dcdb = (double*)(dABdB->data.ptr + dABdB->step*i); - const double* a = (const double*)(A->data.ptr + A->step*i1); - - for( j = 0; j < L*N; j++ ) - dcdb[j] = 0; - for( j = 0; j < L; j++ ) - dcdb[j*N + i2] = a[j]; - } - } - } -} - -static int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian=0 ) -{ - double J[27] = {0}; - CvMat matJ = cvMat( 3, 9, CV_64F, J ); - - if( !CV_IS_MAT(src) ) - CV_Error( !src ? CV_StsNullPtr : CV_StsBadArg, "Input argument is not a valid matrix" ); - - if( !CV_IS_MAT(dst) ) - CV_Error( !dst ? CV_StsNullPtr : CV_StsBadArg, - "The first output argument is not a valid matrix" ); - - int depth = CV_MAT_DEPTH(src->type); - int elem_size = CV_ELEM_SIZE(depth); - - if( depth != CV_32F && depth != CV_64F ) - CV_Error( CV_StsUnsupportedFormat, "The matrices must have 32f or 64f data type" ); - - if( !CV_ARE_DEPTHS_EQ(src, dst) ) - CV_Error( CV_StsUnmatchedFormats, "All the matrices must have the same data type" ); - - if( jacobian ) - { - if( !CV_IS_MAT(jacobian) ) - CV_Error( CV_StsBadArg, "Jacobian is not a valid matrix" ); - - if( !CV_ARE_DEPTHS_EQ(src, jacobian) || CV_MAT_CN(jacobian->type) != 1 ) - CV_Error( CV_StsUnmatchedFormats, "Jacobian must have 32fC1 or 64fC1 datatype" ); - - if( (jacobian->rows != 9 || jacobian->cols != 3) && - (jacobian->rows != 3 || jacobian->cols != 9)) - CV_Error( CV_StsBadSize, "Jacobian must be 3x9 or 9x3" ); - } - - if( src->cols == 1 || src->rows == 1 ) - { - int step = src->rows > 1 ? src->step / elem_size : 1; - - if( src->rows + src->cols*CV_MAT_CN(src->type) - 1 != 3 ) - CV_Error( CV_StsBadSize, "Input matrix must be 1x3, 3x1 or 3x3" ); - - if( dst->rows != 3 || dst->cols != 3 || CV_MAT_CN(dst->type) != 1 ) - CV_Error( CV_StsBadSize, "Output matrix must be 3x3, single-channel floating point matrix" ); - - Point3d r; - if( depth == CV_32F ) - { - r.x = src->data.fl[0]; - r.y = src->data.fl[step]; - r.z = src->data.fl[step*2]; - } - else - { - r.x = src->data.db[0]; - r.y = src->data.db[step]; - r.z = src->data.db[step*2]; - } - - double theta = norm(r); - - if( theta < DBL_EPSILON ) - { - cvSetIdentity( dst ); - - if( jacobian ) - { - memset( J, 0, sizeof(J) ); - J[5] = J[15] = J[19] = -1; - J[7] = J[11] = J[21] = 1; - } - } - else - { - double c = cos(theta); - double s = sin(theta); - double c1 = 1. - c; - double itheta = theta ? 1./theta : 0.; - - r *= itheta; - - Matx33d rrt( r.x*r.x, r.x*r.y, r.x*r.z, r.x*r.y, r.y*r.y, r.y*r.z, r.x*r.z, r.y*r.z, r.z*r.z ); - Matx33d r_x( 0, -r.z, r.y, - r.z, 0, -r.x, - -r.y, r.x, 0 ); - - // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] - Matx33d R = c*Matx33d::eye() + c1*rrt + s*r_x; - - Mat(R).convertTo(cvarrToMat(dst), dst->type); - - if( jacobian ) - { - const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - double drrt[] = { r.x+r.x, r.y, r.z, r.y, 0, 0, r.z, 0, 0, - 0, r.x, 0, r.x, r.y+r.y, r.z, 0, r.z, 0, - 0, 0, r.x, 0, 0, r.y, r.x, r.y, r.z+r.z }; - double d_r_x_[] = { 0, 0, 0, 0, 0, -1, 0, 1, 0, - 0, 0, 1, 0, 0, 0, -1, 0, 0, - 0, -1, 0, 1, 0, 0, 0, 0, 0 }; - for( int i = 0; i < 3; i++ ) - { - double ri = i == 0 ? r.x : i == 1 ? r.y : r.z; - double a0 = -s*ri, a1 = (s - 2*c1*itheta)*ri, a2 = c1*itheta; - double a3 = (c - s*itheta)*ri, a4 = s*itheta; - for( int k = 0; k < 9; k++ ) - J[i*9+k] = a0*I[k] + a1*rrt.val[k] + a2*drrt[i*9+k] + - a3*r_x.val[k] + a4*d_r_x_[i*9+k]; - } - } - } - } - else if( src->cols == 3 && src->rows == 3 ) - { - Matx33d U, Vt; - Vec3d W; - double theta, s, c; - int step = dst->rows > 1 ? dst->step / elem_size : 1; - - if( (dst->rows != 1 || dst->cols*CV_MAT_CN(dst->type) != 3) && - (dst->rows != 3 || dst->cols != 1 || CV_MAT_CN(dst->type) != 1)) - CV_Error( CV_StsBadSize, "Output matrix must be 1x3 or 3x1" ); - - Matx33d R = cvarrToMat(src); - - if( !checkRange(R, true, NULL, -100, 100) ) - { - cvZero(dst); - if( jacobian ) - cvZero(jacobian); - return 0; - } - - SVD::compute(R, W, U, Vt); - R = U*Vt; - - Point3d r(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1)); - - s = std::sqrt((r.x*r.x + r.y*r.y + r.z*r.z)*0.25); - c = (R(0, 0) + R(1, 1) + R(2, 2) - 1)*0.5; - c = c > 1. ? 1. : c < -1. ? -1. : c; - theta = acos(c); - - if( s < 1e-5 ) - { - double t; - - if( c > 0 ) - r = Point3d(0, 0, 0); - else - { - t = (R(0, 0) + 1)*0.5; - r.x = std::sqrt(MAX(t,0.)); - t = (R(1, 1) + 1)*0.5; - r.y = std::sqrt(MAX(t,0.))*(R(0, 1) < 0 ? -1. : 1.); - t = (R(2, 2) + 1)*0.5; - r.z = std::sqrt(MAX(t,0.))*(R(0, 2) < 0 ? -1. : 1.); - if( fabs(r.x) < fabs(r.y) && fabs(r.x) < fabs(r.z) && (R(1, 2) > 0) != (r.y*r.z > 0) ) - r.z = -r.z; - theta /= norm(r); - r *= theta; - } - - if( jacobian ) - { - memset( J, 0, sizeof(J) ); - if( c > 0 ) - { - J[5] = J[15] = J[19] = -0.5; - J[7] = J[11] = J[21] = 0.5; - } - } - } - else - { - double vth = 1/(2*s); - - if( jacobian ) - { - double t, dtheta_dtr = -1./s; - // var1 = [vth;theta] - // var = [om1;var1] = [om1;vth;theta] - double dvth_dtheta = -vth*c/s; - double d1 = 0.5*dvth_dtheta*dtheta_dtr; - double d2 = 0.5*dtheta_dtr; - // dvar1/dR = dvar1/dtheta*dtheta/dR = [dvth/dtheta; 1] * dtheta/dtr * dtr/dR - double dvardR[5*9] = - { - 0, 0, 0, 0, 0, 1, 0, -1, 0, - 0, 0, -1, 0, 0, 0, 1, 0, 0, - 0, 1, 0, -1, 0, 0, 0, 0, 0, - d1, 0, 0, 0, d1, 0, 0, 0, d1, - d2, 0, 0, 0, d2, 0, 0, 0, d2 - }; - // var2 = [om;theta] - double dvar2dvar[] = - { - vth, 0, 0, r.x, 0, - 0, vth, 0, r.y, 0, - 0, 0, vth, r.z, 0, - 0, 0, 0, 0, 1 - }; - double domegadvar2[] = - { - theta, 0, 0, r.x*vth, - 0, theta, 0, r.y*vth, - 0, 0, theta, r.z*vth - }; - - CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR ); - CvMat _dvar2dvar = cvMat( 4, 5, CV_64FC1, dvar2dvar ); - CvMat _domegadvar2 = cvMat( 3, 4, CV_64FC1, domegadvar2 ); - double t0[3*5]; - CvMat _t0 = cvMat( 3, 5, CV_64FC1, t0 ); - - cvMatMul( &_domegadvar2, &_dvar2dvar, &_t0 ); - cvMatMul( &_t0, &_dvardR, &matJ ); - - // transpose every row of matJ (treat the rows as 3x3 matrices) - CV_SWAP(J[1], J[3], t); CV_SWAP(J[2], J[6], t); CV_SWAP(J[5], J[7], t); - CV_SWAP(J[10], J[12], t); CV_SWAP(J[11], J[15], t); CV_SWAP(J[14], J[16], t); - CV_SWAP(J[19], J[21], t); CV_SWAP(J[20], J[24], t); CV_SWAP(J[23], J[25], t); - } - - vth *= theta; - r *= vth; - } - - if( depth == CV_32F ) - { - dst->data.fl[0] = (float)r.x; - dst->data.fl[step] = (float)r.y; - dst->data.fl[step*2] = (float)r.z; - } - else - { - dst->data.db[0] = r.x; - dst->data.db[step] = r.y; - dst->data.db[step*2] = r.z; - } - } - else - { - CV_Error(CV_StsBadSize, "Input matrix must be 1x3 or 3x1 for a rotation vector, or 3x3 for a rotation matrix"); - } - - if( jacobian ) - { - if( depth == CV_32F ) - { - if( jacobian->rows == matJ.rows ) - cvConvert( &matJ, jacobian ); - else - { - float Jf[3*9]; - CvMat _Jf = cvMat( matJ.rows, matJ.cols, CV_32FC1, Jf ); - cvConvert( &matJ, &_Jf ); - cvTranspose( &_Jf, jacobian ); - } - } - else if( jacobian->rows == matJ.rows ) - cvCopy( &matJ, jacobian ); - else - cvTranspose( &matJ, jacobian ); - } - - return 1; -} - -// reimplementation of compose_motion.m -static void cvComposeRT( const CvMat* _rvec1, const CvMat* _tvec1, - const CvMat* _rvec2, const CvMat* _tvec2, - CvMat* _rvec3, CvMat* _tvec3, - CvMat* dr3dr1=0, CvMat* dr3dt1=0, - CvMat* dr3dr2=0, CvMat* dr3dt2=0, - CvMat* dt3dr1=0, CvMat* dt3dt1=0, - CvMat* dt3dr2=0, CvMat* dt3dt2=0 ) -{ - double _r1[3], _r2[3]; - double _R1[9], _d1[9*3], _R2[9], _d2[9*3]; - CvMat r1 = cvMat(3,1,CV_64F,_r1), r2 = cvMat(3,1,CV_64F,_r2); - CvMat R1 = cvMat(3,3,CV_64F,_R1), R2 = cvMat(3,3,CV_64F,_R2); - CvMat dR1dr1 = cvMat(9,3,CV_64F,_d1), dR2dr2 = cvMat(9,3,CV_64F,_d2); - - CV_Assert( CV_IS_MAT(_rvec1) && CV_IS_MAT(_rvec2) ); - - CV_Assert( CV_MAT_TYPE(_rvec1->type) == CV_32F || - CV_MAT_TYPE(_rvec1->type) == CV_64F ); - - CV_Assert( _rvec1->rows == 3 && _rvec1->cols == 1 && CV_ARE_SIZES_EQ(_rvec1, _rvec2) ); - - cvConvert( _rvec1, &r1 ); - cvConvert( _rvec2, &r2 ); - - cvRodrigues2( &r1, &R1, &dR1dr1 ); - cvRodrigues2( &r2, &R2, &dR2dr2 ); - - if( _rvec3 || dr3dr1 || dr3dr2 ) - { - double _r3[3], _R3[9], _dR3dR1[9*9], _dR3dR2[9*9], _dr3dR3[9*3]; - double _W1[9*3], _W2[3*3]; - CvMat r3 = cvMat(3,1,CV_64F,_r3), R3 = cvMat(3,3,CV_64F,_R3); - CvMat dR3dR1 = cvMat(9,9,CV_64F,_dR3dR1), dR3dR2 = cvMat(9,9,CV_64F,_dR3dR2); - CvMat dr3dR3 = cvMat(3,9,CV_64F,_dr3dR3); - CvMat W1 = cvMat(3,9,CV_64F,_W1), W2 = cvMat(3,3,CV_64F,_W2); - - cvMatMul( &R2, &R1, &R3 ); - cvCalcMatMulDeriv( &R2, &R1, &dR3dR2, &dR3dR1 ); - - cvRodrigues2( &R3, &r3, &dr3dR3 ); - - if( _rvec3 ) - cvConvert( &r3, _rvec3 ); - - if( dr3dr1 ) - { - cvMatMul( &dr3dR3, &dR3dR1, &W1 ); - cvMatMul( &W1, &dR1dr1, &W2 ); - cvConvert( &W2, dr3dr1 ); - } - - if( dr3dr2 ) - { - cvMatMul( &dr3dR3, &dR3dR2, &W1 ); - cvMatMul( &W1, &dR2dr2, &W2 ); - cvConvert( &W2, dr3dr2 ); - } - } - - if( dr3dt1 ) - cvZero( dr3dt1 ); - if( dr3dt2 ) - cvZero( dr3dt2 ); - - if( _tvec3 || dt3dr2 || dt3dt1 ) - { - double _t1[3], _t2[3], _t3[3], _dxdR2[3*9], _dxdt1[3*3], _W3[3*3]; - CvMat t1 = cvMat(3,1,CV_64F,_t1), t2 = cvMat(3,1,CV_64F,_t2); - CvMat t3 = cvMat(3,1,CV_64F,_t3); - CvMat dxdR2 = cvMat(3, 9, CV_64F, _dxdR2); - CvMat dxdt1 = cvMat(3, 3, CV_64F, _dxdt1); - CvMat W3 = cvMat(3, 3, CV_64F, _W3); - - CV_Assert( CV_IS_MAT(_tvec1) && CV_IS_MAT(_tvec2) ); - CV_Assert( CV_ARE_SIZES_EQ(_tvec1, _tvec2) && CV_ARE_SIZES_EQ(_tvec1, _rvec1) ); - - cvConvert( _tvec1, &t1 ); - cvConvert( _tvec2, &t2 ); - cvMatMulAdd( &R2, &t1, &t2, &t3 ); - - if( _tvec3 ) - cvConvert( &t3, _tvec3 ); - - if( dt3dr2 || dt3dt1 ) - { - cvCalcMatMulDeriv( &R2, &t1, &dxdR2, &dxdt1 ); - if( dt3dr2 ) - { - cvMatMul( &dxdR2, &dR2dr2, &W3 ); - cvConvert( &W3, dt3dr2 ); - } - if( dt3dt1 ) - cvConvert( &dxdt1, dt3dt1 ); - } - } - - if( dt3dt2 ) - cvSetIdentity( dt3dt2 ); - if( dt3dr1 ) - cvZero( dt3dr1 ); -} - -static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector"; - -static void cvProjectPoints2Internal( const CvMat* objectPoints, - const CvMat* r_vec, - const CvMat* t_vec, - const CvMat* A, - const CvMat* distCoeffs, - CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL), - CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL), - CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL), - CvMat* dpdo CV_DEFAULT(NULL), - double aspectRatio CV_DEFAULT(0) ) -{ - Ptr matM, _m; - Ptr _dpdr, _dpdt, _dpdc, _dpdf, _dpdk; - Ptr _dpdo; - - int i, j, count; - int calc_derivatives; - const CvPoint3D64f* M; - CvPoint2D64f* m; - double r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy; - Matx33d matTilt = Matx33d::eye(); - Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0); - Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0); - CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k; - CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr ); - double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0; - double* dpdo_p = 0; - int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0; - int dpdo_step = 0; - bool fixedAspectRatio = aspectRatio > FLT_EPSILON; - - if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) || - !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) || - /*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) ) - CV_Error( CV_StsBadArg, "One of required arguments is not a valid matrix" ); - - int total = objectPoints->rows * objectPoints->cols * CV_MAT_CN(objectPoints->type); - if(total % 3 != 0) - { - //we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data - CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" ); - } - count = total / 3; - - if( CV_IS_CONT_MAT(objectPoints->type) && - (CV_MAT_DEPTH(objectPoints->type) == CV_32F || CV_MAT_DEPTH(objectPoints->type) == CV_64F)&& - ((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) || - (objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3) || - (objectPoints->rows == 3 && CV_MAT_CN(objectPoints->type) == 1 && objectPoints->cols == count))) - { - matM.reset(cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) )); - cvConvert(objectPoints, matM); - } - else - { -// matM = cvCreateMat( 1, count, CV_64FC3 ); -// cvConvertPointsHomogeneous( objectPoints, matM ); - CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" ); - } - - if( CV_IS_CONT_MAT(imagePoints->type) && - (CV_MAT_DEPTH(imagePoints->type) == CV_32F || CV_MAT_DEPTH(imagePoints->type) == CV_64F) && - ((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 2) || - (imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 2) || - (imagePoints->rows == 2 && CV_MAT_CN(imagePoints->type) == 1 && imagePoints->cols == count))) - { - _m.reset(cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) )); - cvConvert(imagePoints, _m); - } - else - { -// _m = cvCreateMat( 1, count, CV_64FC2 ); - CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" ); - } - - M = (CvPoint3D64f*)matM->data.db; - m = (CvPoint2D64f*)_m->data.db; - - if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) || - (((r_vec->rows != 1 && r_vec->cols != 1) || - r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) && - ((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1))) - CV_Error( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 " - "floating-point rotation vector, or 3x3 rotation matrix" ); - - if( r_vec->rows == 3 && r_vec->cols == 3 ) - { - _r = cvMat( 3, 1, CV_64FC1, r ); - cvRodrigues2( r_vec, &_r ); - cvRodrigues2( &_r, &matR, &_dRdr ); - cvCopy( r_vec, &matR ); - } - else - { - _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r ); - cvConvert( r_vec, &_r ); - cvRodrigues2( &_r, &matR, &_dRdr ); - } - - if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) || - (t_vec->rows != 1 && t_vec->cols != 1) || - t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 ) - CV_Error( CV_StsBadArg, - "Translation vector must be 1x3 or 3x1 floating-point vector" ); - - _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t ); - cvConvert( t_vec, &_t ); - - if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) || - A->rows != 3 || A->cols != 3 ) - CV_Error( CV_StsBadArg, "Intrinsic parameters must be 3x3 floating-point matrix" ); - - cvConvert( A, &_a ); - fx = a[0]; fy = a[4]; - cx = a[2]; cy = a[5]; - - if( fixedAspectRatio ) - fx = fy*aspectRatio; - - if( distCoeffs ) - { - if( !CV_IS_MAT(distCoeffs) || - (CV_MAT_DEPTH(distCoeffs->type) != CV_64F && - CV_MAT_DEPTH(distCoeffs->type) != CV_32F) || - (distCoeffs->rows != 1 && distCoeffs->cols != 1) || - (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 && - distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 && - distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8 && - distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12 && - distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 14) ) - CV_Error( CV_StsBadArg, cvDistCoeffErr ); - - _k = cvMat( distCoeffs->rows, distCoeffs->cols, - CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k ); - cvConvert( distCoeffs, &_k ); - if(k[12] != 0 || k[13] != 0) - { - computeTiltProjectionMatrix(k[12], k[13], &matTilt, &dMatTiltdTauX, &dMatTiltdTauY); - } - } - - if( dpdr ) - { - if( !CV_IS_MAT(dpdr) || - (CV_MAT_TYPE(dpdr->type) != CV_32FC1 && - CV_MAT_TYPE(dpdr->type) != CV_64FC1) || - dpdr->rows != count*2 || dpdr->cols != 3 ) - CV_Error( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" ); - - if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 ) - { - _dpdr.reset(cvCloneMat(dpdr)); - } - else - _dpdr.reset(cvCreateMat( 2*count, 3, CV_64FC1 )); - dpdr_p = _dpdr->data.db; - dpdr_step = _dpdr->step/sizeof(dpdr_p[0]); - } - - if( dpdt ) - { - if( !CV_IS_MAT(dpdt) || - (CV_MAT_TYPE(dpdt->type) != CV_32FC1 && - CV_MAT_TYPE(dpdt->type) != CV_64FC1) || - dpdt->rows != count*2 || dpdt->cols != 3 ) - CV_Error( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" ); - - if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 ) - { - _dpdt.reset(cvCloneMat(dpdt)); - } - else - _dpdt.reset(cvCreateMat( 2*count, 3, CV_64FC1 )); - dpdt_p = _dpdt->data.db; - dpdt_step = _dpdt->step/sizeof(dpdt_p[0]); - } - - if( dpdf ) - { - if( !CV_IS_MAT(dpdf) || - (CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) || - dpdf->rows != count*2 || dpdf->cols != 2 ) - CV_Error( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" ); - - if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 ) - { - _dpdf.reset(cvCloneMat(dpdf)); - } - else - _dpdf.reset(cvCreateMat( 2*count, 2, CV_64FC1 )); - dpdf_p = _dpdf->data.db; - dpdf_step = _dpdf->step/sizeof(dpdf_p[0]); - } - - if( dpdc ) - { - if( !CV_IS_MAT(dpdc) || - (CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) || - dpdc->rows != count*2 || dpdc->cols != 2 ) - CV_Error( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" ); - - if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 ) - { - _dpdc.reset(cvCloneMat(dpdc)); - } - else - _dpdc.reset(cvCreateMat( 2*count, 2, CV_64FC1 )); - dpdc_p = _dpdc->data.db; - dpdc_step = _dpdc->step/sizeof(dpdc_p[0]); - } - - if( dpdk ) - { - if( !CV_IS_MAT(dpdk) || - (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) || - dpdk->rows != count*2 || (dpdk->cols != 14 && dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) ) - CV_Error( CV_StsBadArg, "dp/df must be 2Nx14, 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" ); - - if( !distCoeffs ) - CV_Error( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" ); - - if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 ) - { - _dpdk.reset(cvCloneMat(dpdk)); - } - else - _dpdk.reset(cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 )); - dpdk_p = _dpdk->data.db; - dpdk_step = _dpdk->step/sizeof(dpdk_p[0]); - } - - if( dpdo ) - { - if( !CV_IS_MAT( dpdo ) || ( CV_MAT_TYPE( dpdo->type ) != CV_32FC1 - && CV_MAT_TYPE( dpdo->type ) != CV_64FC1 ) - || dpdo->rows != count * 2 || dpdo->cols != count * 3 ) - CV_Error( CV_StsBadArg, "dp/do must be 2Nx3N floating-point matrix" ); - - if( CV_MAT_TYPE( dpdo->type ) == CV_64FC1 ) - { - _dpdo.reset( cvCloneMat( dpdo ) ); - } - else - _dpdo.reset( cvCreateMat( 2 * count, 3 * count, CV_64FC1 ) ); - cvZero(_dpdo); - dpdo_p = _dpdo->data.db; - dpdo_step = _dpdo->step / sizeof( dpdo_p[0] ); - } - - calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk || dpdo; - - for( i = 0; i < count; i++ ) - { - double X = M[i].x, Y = M[i].y, Z = M[i].z; - double x = R[0]*X + R[1]*Y + R[2]*Z + t[0]; - double y = R[3]*X + R[4]*Y + R[5]*Z + t[1]; - double z = R[6]*X + R[7]*Y + R[8]*Z + t[2]; - double r2, r4, r6, a1, a2, a3, cdist, icdist2; - double xd, yd, xd0, yd0, invProj; - Vec3d vecTilt; - Vec3d dVecTilt; - Matx22d dMatTilt; - Vec2d dXdYd; - - double z0 = z; - z = z ? 1./z : 1; - x *= z; y *= z; - - r2 = x*x + y*y; - r4 = r2*r2; - r6 = r4*r2; - a1 = 2*x*y; - a2 = r2 + 2*x*x; - a3 = r2 + 2*y*y; - cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6; - icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6); - xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4; - yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4; - - // additional distortion by projecting onto a tilt plane - vecTilt = matTilt*Vec3d(xd0, yd0, 1); - invProj = vecTilt(2) ? 1./vecTilt(2) : 1; - xd = invProj * vecTilt(0); - yd = invProj * vecTilt(1); - - m[i].x = xd*fx + cx; - m[i].y = yd*fy + cy; - - if( calc_derivatives ) - { - if( dpdc_p ) - { - dpdc_p[0] = 1; dpdc_p[1] = 0; // dp_xdc_x; dp_xdc_y - dpdc_p[dpdc_step] = 0; - dpdc_p[dpdc_step+1] = 1; - dpdc_p += dpdc_step*2; - } - - if( dpdf_p ) - { - if( fixedAspectRatio ) - { - dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; // dp_xdf_x; dp_xdf_y - dpdf_p[dpdf_step] = 0; - dpdf_p[dpdf_step+1] = yd; - } - else - { - dpdf_p[0] = xd; dpdf_p[1] = 0; - dpdf_p[dpdf_step] = 0; - dpdf_p[dpdf_step+1] = yd; - } - dpdf_p += dpdf_step*2; - } - for (int row = 0; row < 2; ++row) - for (int col = 0; col < 2; ++col) - dMatTilt(row,col) = matTilt(row,col)*vecTilt(2) - - matTilt(2,col)*vecTilt(row); - double invProjSquare = (invProj*invProj); - dMatTilt *= invProjSquare; - if( dpdk_p ) - { - dXdYd = dMatTilt*Vec2d(x*icdist2*r2, y*icdist2*r2); - dpdk_p[0] = fx*dXdYd(0); - dpdk_p[dpdk_step] = fy*dXdYd(1); - dXdYd = dMatTilt*Vec2d(x*icdist2*r4, y*icdist2*r4); - dpdk_p[1] = fx*dXdYd(0); - dpdk_p[dpdk_step+1] = fy*dXdYd(1); - if( _dpdk->cols > 2 ) - { - dXdYd = dMatTilt*Vec2d(a1, a3); - dpdk_p[2] = fx*dXdYd(0); - dpdk_p[dpdk_step+2] = fy*dXdYd(1); - dXdYd = dMatTilt*Vec2d(a2, a1); - dpdk_p[3] = fx*dXdYd(0); - dpdk_p[dpdk_step+3] = fy*dXdYd(1); - if( _dpdk->cols > 4 ) - { - dXdYd = dMatTilt*Vec2d(x*icdist2*r6, y*icdist2*r6); - dpdk_p[4] = fx*dXdYd(0); - dpdk_p[dpdk_step+4] = fy*dXdYd(1); - - if( _dpdk->cols > 5 ) - { - dXdYd = dMatTilt*Vec2d( - x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2); - dpdk_p[5] = fx*dXdYd(0); - dpdk_p[dpdk_step+5] = fy*dXdYd(1); - dXdYd = dMatTilt*Vec2d( - x*cdist*(-icdist2)*icdist2*r4, y*cdist*(-icdist2)*icdist2*r4); - dpdk_p[6] = fx*dXdYd(0); - dpdk_p[dpdk_step+6] = fy*dXdYd(1); - dXdYd = dMatTilt*Vec2d( - x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6); - dpdk_p[7] = fx*dXdYd(0); - dpdk_p[dpdk_step+7] = fy*dXdYd(1); - if( _dpdk->cols > 8 ) - { - dXdYd = dMatTilt*Vec2d(r2, 0); - dpdk_p[8] = fx*dXdYd(0); //s1 - dpdk_p[dpdk_step+8] = fy*dXdYd(1); //s1 - dXdYd = dMatTilt*Vec2d(r4, 0); - dpdk_p[9] = fx*dXdYd(0); //s2 - dpdk_p[dpdk_step+9] = fy*dXdYd(1); //s2 - dXdYd = dMatTilt*Vec2d(0, r2); - dpdk_p[10] = fx*dXdYd(0);//s3 - dpdk_p[dpdk_step+10] = fy*dXdYd(1); //s3 - dXdYd = dMatTilt*Vec2d(0, r4); - dpdk_p[11] = fx*dXdYd(0);//s4 - dpdk_p[dpdk_step+11] = fy*dXdYd(1); //s4 - if( _dpdk->cols > 12 ) - { - dVecTilt = dMatTiltdTauX * Vec3d(xd0, yd0, 1); - dpdk_p[12] = fx * invProjSquare * ( - dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0)); - dpdk_p[dpdk_step+12] = fy*invProjSquare * ( - dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1)); - dVecTilt = dMatTiltdTauY * Vec3d(xd0, yd0, 1); - dpdk_p[13] = fx * invProjSquare * ( - dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0)); - dpdk_p[dpdk_step+13] = fy * invProjSquare * ( - dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1)); - } - } - } - } - } - dpdk_p += dpdk_step*2; - } - - if( dpdt_p ) - { - double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z }; - for( j = 0; j < 3; j++ ) - { - double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j]; - double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt; - double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt); - double da1dt = 2*(x*dydt[j] + y*dxdt[j]); - double dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt + - k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt); - double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt + - k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt); - dXdYd = dMatTilt*Vec2d(dmxdt, dmydt); - dpdt_p[j] = fx*dXdYd(0); - dpdt_p[dpdt_step+j] = fy*dXdYd(1); - } - dpdt_p += dpdt_step*2; - } - - if( dpdr_p ) - { - double dx0dr[] = - { - X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2], - X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11], - X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20] - }; - double dy0dr[] = - { - X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5], - X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14], - X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23] - }; - double dz0dr[] = - { - X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8], - X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17], - X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26] - }; - for( j = 0; j < 3; j++ ) - { - double dxdr = z*(dx0dr[j] - x*dz0dr[j]); - double dydr = z*(dy0dr[j] - y*dz0dr[j]); - double dr2dr = 2*x*dxdr + 2*y*dydr; - double dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr; - double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr; - double da1dr = 2*(x*dydr + y*dxdr); - double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr + - k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr); - double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr + - k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr); - dXdYd = dMatTilt*Vec2d(dmxdr, dmydr); - dpdr_p[j] = fx*dXdYd(0); - dpdr_p[dpdr_step+j] = fy*dXdYd(1); - } - dpdr_p += dpdr_step*2; - } - - if( dpdo_p ) - { - double dxdo[] = { z * ( R[0] - x * z * z0 * R[6] ), - z * ( R[1] - x * z * z0 * R[7] ), - z * ( R[2] - x * z * z0 * R[8] ) }; - double dydo[] = { z * ( R[3] - y * z * z0 * R[6] ), - z * ( R[4] - y * z * z0 * R[7] ), - z * ( R[5] - y * z * z0 * R[8] ) }; - for( j = 0; j < 3; j++ ) - { - double dr2do = 2 * x * dxdo[j] + 2 * y * dydo[j]; - double dr4do = 2 * r2 * dr2do; - double dr6do = 3 * r4 * dr2do; - double da1do = 2 * y * dxdo[j] + 2 * x * dydo[j]; - double da2do = dr2do + 4 * x * dxdo[j]; - double da3do = dr2do + 4 * y * dydo[j]; - double dcdist_do - = k[0] * dr2do + k[1] * dr4do + k[4] * dr6do; - double dicdist2_do = -icdist2 * icdist2 - * ( k[5] * dr2do + k[6] * dr4do + k[7] * dr6do ); - double dxd0_do = cdist * icdist2 * dxdo[j] - + x * icdist2 * dcdist_do + x * cdist * dicdist2_do - + k[2] * da1do + k[3] * da2do + k[8] * dr2do - + k[9] * dr4do; - double dyd0_do = cdist * icdist2 * dydo[j] - + y * icdist2 * dcdist_do + y * cdist * dicdist2_do - + k[2] * da3do + k[3] * da1do + k[10] * dr2do - + k[11] * dr4do; - dXdYd = dMatTilt * Vec2d( dxd0_do, dyd0_do ); - dpdo_p[i * 3 + j] = fx * dXdYd( 0 ); - dpdo_p[dpdo_step + i * 3 + j] = fy * dXdYd( 1 ); - } - dpdo_p += dpdo_step * 2; - } - } - } - - if( _m != imagePoints ) - cvConvert( _m, imagePoints ); - - if( _dpdr != dpdr ) - cvConvert( _dpdr, dpdr ); - - if( _dpdt != dpdt ) - cvConvert( _dpdt, dpdt ); - - if( _dpdf != dpdf ) - cvConvert( _dpdf, dpdf ); - - if( _dpdc != dpdc ) - cvConvert( _dpdc, dpdc ); - - if( _dpdk != dpdk ) - cvConvert( _dpdk, dpdk ); - - if( _dpdo != dpdo ) - cvConvert( _dpdo, dpdo ); -} - -static void cvProjectPoints2( const CvMat* objectPoints, - const CvMat* r_vec, - const CvMat* t_vec, - const CvMat* A, - const CvMat* distCoeffs, - CvMat* imagePoints, CvMat* dpdr=0, - CvMat* dpdt=0, CvMat* dpdf=0, - CvMat* dpdc=0, CvMat* dpdk=0, - double aspectRatio=0 ) -{ - cvProjectPoints2Internal( objectPoints, r_vec, t_vec, A, distCoeffs, imagePoints, dpdr, dpdt, - dpdf, dpdc, dpdk, NULL, aspectRatio ); -} - -static void cvFindExtrinsicCameraParams2( const CvMat* objectPoints, - const CvMat* imagePoints, const CvMat* A, - const CvMat* distCoeffs, CvMat* rvec, CvMat* tvec, - int useExtrinsicGuess ) -{ - cv::Mat objpt = cvarrToMat(objectPoints), imgpt = cvarrToMat(imagePoints); - cv::Mat cameraMatrix = cvarrToMat(A), dk, rv0=cvarrToMat(rvec), tv0=cvarrToMat(tvec), rv, tv; - if (distCoeffs) - dk = cvarrToMat(distCoeffs); - solvePnP(objpt, imgpt, cameraMatrix, dk, rv, tv, useExtrinsicGuess != 0, SOLVEPNP_ITERATIVE ); - CV_Assert(rv.size() == rv0.size() && tv.size() == tv0.size()); - rv.convertTo(rv0, rv0.type()); - tv.convertTo(tv0, tv0.type()); -} - static void initIntrinsicParams2D( const Mat& objectPoints, const Mat& imagePoints, const Mat& npoints, Size imageSize, OutputArray cameraMatrix, @@ -1139,131 +141,235 @@ static void initIntrinsicParams2D( const Mat& objectPoints, _a.copyTo(cameraMatrix); } -static void subMatrix(const Mat& src, Mat& dst, const std::vector& cols, - const std::vector& rows) { - int nonzeros_cols = countNonZero(cols); - Mat tmp(src.rows, nonzeros_cols, CV_64FC1); - - for (int i = 0, j = 0; i < (int)cols.size(); i++) +static void subMatrix(const Mat& src, Mat& dst, + const std::vector& cols, + const std::vector& rows) +{ + CV_Assert(src.type() == CV_64F && dst.type() == CV_64F); + int m = (int)rows.size(), n = (int)cols.size(); + int i1 = 0, j1 = 0; + const uchar* colsdata = cols.empty() ? 0 : &cols[0]; + for(int i = 0; i < m; i++) { - if (cols[i]) + if(rows[i]) { - src.col(i).copyTo(tmp.col(j++)); - } - } + const double* srcptr = src.ptr(i); + double* dstptr = dst.ptr(i1++); - int nonzeros_rows = countNonZero(rows); - dst.create(nonzeros_rows, nonzeros_cols, CV_64FC1); - for (int i = 0, j = 0; i < (int)rows.size(); i++) - { - if (rows[i]) - { - tmp.row(i).copyTo(dst.row(j++)); + for(int j = j1 = 0; j < n; j++) + { + if(colsdata[j]) + dstptr[j1++] = srcptr[j]; + } } } } -static double cvCalibrateCamera2Internal( const CvMat* objectPoints, - const CvMat* imagePoints, const CvMat* npoints, - CvSize imageSize, int iFixedPoint, CvMat* cameraMatrix, CvMat* distCoeffs, - CvMat* rvecs, CvMat* tvecs, CvMat* newObjPoints, CvMat* stdDevs, - CvMat* perViewErrors, int flags, CvTermCriteria termCrit ) +static void cameraCalcJErr(const Mat& objectPoints, const Mat& imagePoints, + const Mat& npoints, Mat& allErrors, + Mat& _param, Mat* _JtErr, Mat* _JtJ, double* _errnorm, + double aspectRatio, Mat* perViewErrors, + int flags, bool optimizeObjPoints) { const int NINTRINSIC = CALIB_NINTRINSIC; + int ni = 0, nimages = (int)npoints.total(); + double k[14] = {0}; + Mat _k(14, 1, CV_64F, k); + double* param = _param.ptr(); + int nparams = (int)_param.total(); + bool calcJ = _JtErr != 0; + int ni0 = npoints.at(0); + Mat _Je(ni0*2, 6, CV_64F), _Ji(ni0*2, NINTRINSIC, CV_64F), _Jo, _err(ni*2, 1, CV_64F); + + if( flags & CALIB_FIX_ASPECT_RATIO ) + { + param[0] = param[1]*aspectRatio; + //pparam[0] = pparam[1]*aspectRatio; + } + + Matx33d A(param[0], 0, param[2], + 0, param[1], param[3], + 0, 0, 1); + std::copy(param + 4, param + 4 + 14, k); + + if (_JtJ) + _JtJ->setZero(); + if (_JtErr) + _JtErr->setZero(); + + if(optimizeObjPoints) + _Jo.create(ni0*2, ni0*3, CV_64F); + double reprojErr = 0; + int maxPoints = 0; + for( int i = 0; i < nimages; i++ ) + maxPoints = max(maxPoints, npoints.at(i)); + + for( int i = 0, pos = 0; i < nimages; i++, pos += ni ) + { + ni = npoints.at(i); + Mat _ri = _param.rowRange(NINTRINSIC + i*6, NINTRINSIC + i*6 + 3); + Mat _ti = _param.rowRange(NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6); + + Mat _Mi = objectPoints.colRange(pos, pos + ni); + if( optimizeObjPoints ) + { + _Mi = _param.rowRange(NINTRINSIC + nimages * 6, + NINTRINSIC + nimages * 6 + ni * 3); + _Mi = _Mi.reshape(3, 1); + } + Mat _mi = imagePoints.colRange(pos, pos + ni); + Mat _me = allErrors.colRange(pos, pos + ni); + + _Je.resize(ni*2); + _Ji.resize(ni*2); + _err.resize(ni*2); + if (optimizeObjPoints) + _Jo.resize(ni*2); + Mat _mp = _err.reshape(2, 1); + + if( calcJ ) + { + Mat _dpdr = _Je.colRange(0, 3); + Mat _dpdt = _Je.colRange(3, 6); + Mat _dpdf = _Ji.colRange(0, 2); + Mat _dpdc = _Ji.colRange(2, 4); + Mat _dpdk = _Ji.colRange(4, NINTRINSIC); + Mat _dpdo = _Jo.empty() ? Mat() : _Jo.colRange(0, ni * 3); + double* dpdr_p = _dpdr.ptr(); + double* dpdt_p = _dpdt.ptr(); + double* dpdf_p = _dpdf.ptr(); + double* dpdc_p = _dpdc.ptr(); + double* dpdk_p = _dpdk.ptr(); + double* dpdo_p = _dpdo.ptr(); + + projectPoints(_Mi, _ri, _ti, A, _k, _mp, _dpdr, _dpdt, + (flags & CALIB_FIX_FOCAL_LENGTH) ? _OutputArray() : _OutputArray(_dpdf), + (flags & CALIB_FIX_PRINCIPAL_POINT) ? _OutputArray() : _OutputArray(_dpdc), + _dpdk, _Jo.empty() ? _OutputArray() : _OutputArray(_dpdo), + (flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0.); + CV_Assert(_mp.ptr() == _err.ptr() && + dpdr_p == _dpdr.ptr() && dpdt_p == _dpdt.ptr() && + dpdf_p == _dpdf.ptr() && dpdc_p == _dpdc.ptr() && + dpdk_p == _dpdk.ptr() && dpdo_p == _dpdo.ptr()); + } + else + projectPoints( _Mi, _ri, _ti, A, _k, _mp, + noArray(), noArray(), noArray(), + noArray(), noArray(), noArray(), 0.); + + subtract( _mp, _mi, _mp ); + _mp.copyTo(_me); + + if( calcJ ) + { + Mat JtJ = *_JtJ, JtErr = *_JtErr; + + // 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; + if( optimizeObjPoints ) + { + JtJ(Rect(NINTRINSIC + nimages * 6, 0, maxPoints * 3, NINTRINSIC)) += _Ji.t() * _Jo; + JtJ(Rect(NINTRINSIC + nimages * 6, NINTRINSIC + i * 6, maxPoints * 3, 6)) + += _Je.t() * _Jo; + JtJ(Rect(NINTRINSIC + nimages * 6, NINTRINSIC + nimages * 6, maxPoints * 3, maxPoints * 3)) + += _Jo.t() * _Jo; + } + + JtErr.rowRange(0, NINTRINSIC) += _Ji.t() * _err; + JtErr.rowRange(NINTRINSIC + i * 6, NINTRINSIC + (i + 1) * 6) = _Je.t() * _err; + if( optimizeObjPoints ) + { + JtErr.rowRange(NINTRINSIC + nimages * 6, nparams) += _Jo.t() * _err; + } + } + + double viewErr = norm(_err, NORM_L2SQR); + /*if (i == 0 || i == nimages-1) { + printf("image %d.", i); + for(int j = 0; j < 10; j++) { + printf(" %.2g", _err.at(j)); + } + printf("\n"); + }*/ + + if( perViewErrors ) + perViewErrors->at(i) = std::sqrt(viewErr / ni); + + reprojErr += viewErr; + } + + if(_errnorm) + *_errnorm = reprojErr; +} + +static double calibrateCameraInternal( const Mat& objectPoints, + const Mat& imagePoints, const Mat& npoints, + Size imageSize, int iFixedPoint, Mat& cameraMatrix, Mat& distCoeffs, + Mat* rvecs, Mat* tvecs, Mat* newObjPoints, Mat* stdDevs, + Mat* perViewErrors, int flags, const TermCriteria& termCrit ) +{ + const int NINTRINSIC = CALIB_NINTRINSIC; Matx33d A; double k[14] = {0}; - CvMat matA = cvMat(3, 3, CV_64F, A.val), _k; - int i, nimages, maxPoints = 0, ni = 0, pos, total = 0, nparams, npstep, cn; + Mat matA(3, 3, CV_64F, A.val); + int i, maxPoints = 0, ni = 0, pos, total = 0, nparams, cn; double aspectRatio = 0.; + int nimages = npoints.checkVector(1, CV_32S); + CV_Assert(nimages >= 1); + int ndistCoeffs = (int)distCoeffs.total(); + bool releaseObject = iFixedPoint > 0 && iFixedPoint < npoints.at(0) - 1; // 0. check the parameters & allocate buffers - if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(imagePoints) || - !CV_IS_MAT(npoints) || !CV_IS_MAT(cameraMatrix) || !CV_IS_MAT(distCoeffs) ) - CV_Error( CV_StsBadArg, "One of required vector arguments is not a valid matrix" ); - if( imageSize.width <= 0 || imageSize.height <= 0 ) CV_Error( CV_StsOutOfRange, "image width and height must be positive" ); - if( CV_MAT_TYPE(npoints->type) != CV_32SC1 || - (npoints->rows != 1 && npoints->cols != 1) ) - CV_Error( CV_StsUnsupportedFormat, - "the array of point counters must be 1-dimensional integer vector" ); if(flags & CALIB_TILTED_MODEL) { //when the tilted sensor model is used the distortion coefficients matrix must have 14 parameters - if (distCoeffs->cols*distCoeffs->rows != 14) + if (ndistCoeffs != 14) CV_Error( CV_StsBadArg, "The tilted sensor model must have 14 parameters in the distortion matrix" ); } else { //when the thin prism model is used the distortion coefficients matrix must have 12 parameters if(flags & CALIB_THIN_PRISM_MODEL) - if (distCoeffs->cols*distCoeffs->rows != 12) + if (ndistCoeffs != 12) CV_Error( CV_StsBadArg, "Thin prism model must have 12 parameters in the distortion matrix" ); } - nimages = npoints->rows*npoints->cols; - npstep = npoints->rows == 1 ? 1 : npoints->step/CV_ELEM_SIZE(npoints->type); - if( rvecs ) { - cn = CV_MAT_CN(rvecs->type); - if( !CV_IS_MAT(rvecs) || - (CV_MAT_DEPTH(rvecs->type) != CV_32F && CV_MAT_DEPTH(rvecs->type) != CV_64F) || - ((rvecs->rows != nimages || (rvecs->cols*cn != 3 && rvecs->cols*cn != 9)) && - (rvecs->rows != 1 || rvecs->cols != nimages || cn != 3)) ) - CV_Error( CV_StsBadArg, "the output array of rotation vectors must be 3-channel " - "1xn or nx1 array or 1-channel nx3 or nx9 array, where n is the number of views" ); + cn = rvecs->channels(); + CV_Assert(rvecs->depth() == CV_32F || rvecs->depth() == CV_64F); + CV_Assert(rvecs->rows == nimages); + CV_Assert((rvecs->rows == nimages && (rvecs->cols*cn == 3 || rvecs->cols*cn == 3)) || + (rvecs->rows == 1 && rvecs->cols == nimages && cn == 3)); } if( tvecs ) { - cn = CV_MAT_CN(tvecs->type); - if( !CV_IS_MAT(tvecs) || - (CV_MAT_DEPTH(tvecs->type) != CV_32F && CV_MAT_DEPTH(tvecs->type) != CV_64F) || - ((tvecs->rows != nimages || tvecs->cols*cn != 3) && - (tvecs->rows != 1 || tvecs->cols != nimages || cn != 3)) ) - CV_Error( CV_StsBadArg, "the output array of translation vectors must be 3-channel " - "1xn or nx1 array or 1-channel nx3 array, where n is the number of views" ); + cn = tvecs->channels(); + CV_Assert(tvecs->depth() == CV_32F || tvecs->depth() == CV_64F); + CV_Assert(tvecs->rows == nimages); + CV_Assert((tvecs->rows == nimages && tvecs->cols*cn == 3) || + (tvecs->rows == 1 && tvecs->cols == nimages && cn == 3)); } - bool releaseObject = iFixedPoint > 0 && iFixedPoint < npoints->data.i[0] - 1; + CV_Assert(cameraMatrix.type() == CV_32F || cameraMatrix.type() == CV_64F); + CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3); - if( stdDevs && !releaseObject ) - { - cn = CV_MAT_CN(stdDevs->type); - if( !CV_IS_MAT(stdDevs) || - (CV_MAT_DEPTH(stdDevs->type) != CV_32F && CV_MAT_DEPTH(stdDevs->type) != CV_64F) || - ((stdDevs->rows != (nimages*6 + NINTRINSIC) || stdDevs->cols*cn != 1) && - (stdDevs->rows != 1 || stdDevs->cols != (nimages*6 + NINTRINSIC) || cn != 1)) ) -#define STR__(x) #x -#define STR_(x) STR__(x) - CV_Error( CV_StsBadArg, "the output array of standard deviations vectors must be 1-channel " - "1x(n*6 + NINTRINSIC) or (n*6 + NINTRINSIC)x1 array, where n is the number of views," - " NINTRINSIC = " STR_(CV_CALIB_NINTRINSIC)); - } - - if( (CV_MAT_TYPE(cameraMatrix->type) != CV_32FC1 && - CV_MAT_TYPE(cameraMatrix->type) != CV_64FC1) || - cameraMatrix->rows != 3 || cameraMatrix->cols != 3 ) - CV_Error( CV_StsBadArg, - "Intrinsic parameters must be 3x3 floating-point matrix" ); - - if( (CV_MAT_TYPE(distCoeffs->type) != CV_32FC1 && - CV_MAT_TYPE(distCoeffs->type) != CV_64FC1) || - (distCoeffs->cols != 1 && distCoeffs->rows != 1) || - (distCoeffs->cols*distCoeffs->rows != 4 && - distCoeffs->cols*distCoeffs->rows != 5 && - distCoeffs->cols*distCoeffs->rows != 8 && - distCoeffs->cols*distCoeffs->rows != 12 && - distCoeffs->cols*distCoeffs->rows != 14) ) - CV_Error( CV_StsBadArg, cvDistCoeffErr ); + CV_Assert(distCoeffs.type() == CV_32F || distCoeffs.type() == CV_64F); + CV_Assert(distCoeffs.rows == 1 || distCoeffs.cols == 1); + CV_Assert(ndistCoeffs == 4 || ndistCoeffs == 5 || ndistCoeffs == 8 || + ndistCoeffs == 12 || ndistCoeffs == 14); for( i = 0; i < nimages; i++ ) { - ni = npoints->data.i[i*npstep]; + ni = npoints.at(i); if( ni < 4 ) { CV_Error_( CV_StsOutOfRange, ("The number of points in the view #%d is < 4", i)); @@ -1274,61 +380,56 @@ static double cvCalibrateCamera2Internal( const CvMat* objectPoints, if( newObjPoints ) { - cn = CV_MAT_CN(newObjPoints->type); - if( !CV_IS_MAT(newObjPoints) || - (CV_MAT_DEPTH(newObjPoints->type) != CV_32F && CV_MAT_DEPTH(newObjPoints->type) != CV_64F) || - ((newObjPoints->rows != maxPoints || newObjPoints->cols*cn != 3) && - (newObjPoints->rows != 1 || newObjPoints->cols != maxPoints || cn != 3)) ) - CV_Error( CV_StsBadArg, "the output array of refined object points must be 3-channel " - "1xn or nx1 array or 1-channel nx3 array, where n is the number of object points per view" ); + cn = newObjPoints->channels(); + CV_Assert(newObjPoints->depth() == CV_32F || newObjPoints->depth() == CV_64F); + CV_Assert(rvecs->rows == nimages); + CV_Assert((newObjPoints->rows == maxPoints && newObjPoints->cols*cn == 3) || + (newObjPoints->rows == 1 && newObjPoints->cols == maxPoints && cn == 3)); } - if( stdDevs && releaseObject ) + if( stdDevs ) { - cn = CV_MAT_CN(stdDevs->type); - if( !CV_IS_MAT(stdDevs) || - (CV_MAT_DEPTH(stdDevs->type) != CV_32F && CV_MAT_DEPTH(stdDevs->type) != CV_64F) || - ((stdDevs->rows != (nimages*6 + NINTRINSIC + maxPoints*3) || stdDevs->cols*cn != 1) && - (stdDevs->rows != 1 || stdDevs->cols != (nimages*6 + NINTRINSIC + maxPoints*3) || cn != 1)) ) - CV_Error( CV_StsBadArg, "the output array of standard deviations vectors must be 1-channel " - "1x(n*6 + NINTRINSIC + m*3) or (n*6 + NINTRINSIC + m*3)x1 array, where n is the number of views," - " NINTRINSIC = " STR_(CV_CALIB_NINTRINSIC) ", m is the number of object points per view"); + cn = stdDevs->channels(); + CV_Assert(stdDevs->depth() == CV_32F || stdDevs->depth() == CV_64F); + int nstddev = nimages*6 + NINTRINSIC + (releaseObject ? maxPoints*3 : 0); + + CV_Assert((stdDevs->rows == nstddev && stdDevs->cols*cn == 1) || + (stdDevs->rows == 1 && stdDevs->cols == nstddev && cn == 1)); } Mat matM( 1, total, CV_64FC3 ); Mat _m( 1, total, CV_64FC2 ); Mat allErrors(1, total, CV_64FC2); - if(CV_MAT_CN(objectPoints->type) == 3) { - cvarrToMat(objectPoints).convertTo(matM, CV_64F); - } else { - convertPointsToHomogeneous(cvarrToMat(objectPoints), matM, CV_64F); - } + if(objectPoints.channels() == 3) + objectPoints.convertTo(matM, CV_64F); + else + convertPointsToHomogeneous(objectPoints, matM, CV_64F); - if(CV_MAT_CN(imagePoints->type) == 2) { - cvarrToMat(imagePoints).convertTo(_m, CV_64F); - } else { - convertPointsFromHomogeneous(cvarrToMat(imagePoints), _m, CV_64F); - } + if(imagePoints.channels() == 2) + imagePoints.convertTo(_m, CV_64F); + else + convertPointsFromHomogeneous(imagePoints, _m, CV_64F); nparams = NINTRINSIC + nimages*6; if( releaseObject ) nparams += maxPoints * 3; - _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 ) + Mat _k( distCoeffs.rows, distCoeffs.cols, CV_64F, k); + if( distCoeffs.total() < 8 ) { - if( distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) < 5 ) + if( distCoeffs.total() < 5 ) flags |= CALIB_FIX_K3; flags |= CALIB_FIX_K4 | CALIB_FIX_K5 | CALIB_FIX_K6; } const double minValidAspectRatio = 0.01; const double maxValidAspectRatio = 100.0; + cameraMatrix.convertTo(matA, CV_64F); + // 1. initialize intrinsic parameters & LM solver if( flags & CALIB_USE_INTRINSIC_GUESS ) { - cvConvert( cameraMatrix, &matA ); if( A(0, 0) <= 0 || A(1, 1) <= 0 ) CV_Error( CV_StsOutOfRange, "Focal length (fx and fy) must be positive" ); if( A(0, 2) < 0 || A(0, 2) >= imageSize.width || @@ -1351,7 +452,7 @@ static double cvCalibrateCamera2Internal( const CvMat* objectPoints, CV_Error( CV_StsOutOfRange, "The specified aspect ratio (= cameraMatrix[0][0] / cameraMatrix[1][1]) is incorrect" ); } - cvConvert( distCoeffs, &_k ); + distCoeffs.convertTo(_k, CV_64F); } else { @@ -1365,35 +466,37 @@ static double cvCalibrateCamera2Internal( const CvMat* objectPoints, if( flags & CALIB_FIX_ASPECT_RATIO ) { - aspectRatio = cvmGet(cameraMatrix,0,0); - aspectRatio /= cvmGet(cameraMatrix,1,1); + aspectRatio = A(0, 0); + aspectRatio /= A(1, 1); if( aspectRatio < minValidAspectRatio || aspectRatio > maxValidAspectRatio ) CV_Error( CV_StsOutOfRange, "The specified aspect ratio (= cameraMatrix[0][0] / cameraMatrix[1][1]) is incorrect" ); } - Mat _npoints = cvarrToMat(npoints); - initIntrinsicParams2D( matM, _m, _npoints, imageSize, A, aspectRatio ); + initIntrinsicParams2D( matM, _m, npoints, imageSize, A, aspectRatio ); } - CvLevMarq solver( nparams, 0, termCrit ); + //std::cout << "A0: " << A << std::endl; + //std::cout << "dist0:" << _k << std::endl; Mat _Ji( maxPoints*2, NINTRINSIC, CV_64FC1, Scalar(0)); Mat _Je( maxPoints*2, 6, CV_64FC1 ); Mat _err( maxPoints*2, 1, CV_64FC1 ); + Mat param0( nparams, 1, CV_64FC1 ); + Mat mask0 = Mat::ones( nparams, 1, CV_8UC1 ); - const bool allocJo = (solver.state == CvLevMarq::CALC_J) || stdDevs || releaseObject; + const bool allocJo = stdDevs || releaseObject; Mat _Jo = allocJo ? Mat( maxPoints*2, maxPoints*3, CV_64FC1, Scalar(0) ) : Mat(); + int solveMethod = DECOMP_EIG; if(flags & CALIB_USE_LU) { - solver.solveMethod = DECOMP_LU; + solveMethod = DECOMP_LU; } else if(flags & CALIB_USE_QR) { - solver.solveMethod = DECOMP_QR; + solveMethod = DECOMP_QR; } - { - double* param = solver.param->data.db; - uchar* mask = solver.mask->data.ptr; + double* param = param0.ptr(); + uchar* mask = mask0.ptr(); param[0] = A(0, 0); param[1] = A(1, 1); param[2] = A(0, 2); param[3] = A(1, 2); std::copy(k, k + 14, param + 4); @@ -1419,9 +522,7 @@ static double cvCalibrateCamera2Internal( const CvMat* objectPoints, mask[ 4] = !(flags & CALIB_FIX_K1); mask[ 5] = !(flags & CALIB_FIX_K2); if( flags & CALIB_FIX_TANGENT_DIST ) - { - mask[6] = mask[7] = 0; - } + mask[6] = mask[7] = 0; mask[ 8] = !(flags & CALIB_FIX_K3); mask[ 9] = !(flags & CALIB_FIX_K4); mask[10] = !(flags & CALIB_FIX_K5); @@ -1435,10 +536,7 @@ static double cvCalibrateCamera2Internal( const CvMat* objectPoints, mask[15] = 0; } if(flags & CALIB_FIX_TAUX_TAUY) - { - mask[16] = 0; - mask[17] = 0; - } + mask[16] = mask[17] = 0; if(releaseObject) { @@ -1454,297 +552,106 @@ static double cvCalibrateCamera2Internal( const CvMat* objectPoints, mask[NINTRINSIC + nimages * 6 + iFixedPoint * 3 + 2] = 0; mask[nparams - 1] = 0; } - } // 2. initialize extrinsic parameters for( i = 0, pos = 0; i < nimages; i++, pos += ni ) { - CvMat _ri, _ti; - ni = npoints->data.i[i*npstep]; + ni = npoints.at(i); - cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 ); - cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 ); + Mat _ri = param0.rowRange(NINTRINSIC + i*6, NINTRINSIC + i*6 + 3); + Mat _ti = param0.rowRange(NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6); - CvMat _Mi = cvMat(matM.colRange(pos, pos + ni)); - CvMat _mi = cvMat(_m.colRange(pos, pos + ni)); + Mat _Mi = matM.colRange(pos, pos + ni); + Mat _mi = _m.colRange(pos, pos + ni); - cvFindExtrinsicCameraParams2( &_Mi, &_mi, &matA, &_k, &_ri, &_ti, 0 ); + solvePnP(_Mi, _mi, matA, _k, _ri, _ti, false); } + //std::cout << "single camera calib. param before LM: " << param0.t() << "\n"; + //std::cout << "single camera calib. mask: " << mask0.t() << "\n"; + // 3. run the optimization - for(;;) + LMSolver::runAlt(param0, mask0, termCrit, solveMethod, false, + [&](Mat& _param, Mat* _JtErr, Mat* _JtJ, double* _errnorm) { - const CvMat* _param = 0; - CvMat *_JtJ = 0, *_JtErr = 0; - double* _errNorm = 0; - bool proceed = solver.updateAlt( _param, _JtJ, _JtErr, _errNorm ); - double *param = solver.param->data.db, *pparam = solver.prevParam->data.db; - bool calcJ = solver.state == CvLevMarq::CALC_J || (!proceed && stdDevs); + cameraCalcJErr(matM, _m, npoints, allErrors, _param, _JtErr, _JtJ, _errnorm, + aspectRatio, perViewErrors, flags, releaseObject); + return true; + }); - if( flags & CALIB_FIX_ASPECT_RATIO ) - { - param[0] = param[1]*aspectRatio; - pparam[0] = pparam[1]*aspectRatio; - } + //std::cout << "single camera calib. param after LM: " << param0.t() << "\n"; - 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 + 14, k); + Mat JtErr(nparams, 1, CV_64F), JtJ(nparams, nparams, CV_64F), JtJinv, JtJN; + double reprojErr = 0; + JtErr.setZero(); JtJ.setZero(); + cameraCalcJErr(matM, _m, npoints, allErrors, param0, + stdDevs ? &JtErr : 0, stdDevs ? &JtJ : 0, &reprojErr, + aspectRatio, 0, flags, releaseObject); + if (stdDevs) + { + int nparams_nz = countNonZero(mask0); + JtJN.create(nparams_nz, nparams_nz, CV_64F); - if ( !proceed && !stdDevs && !perViewErrors ) - break; - else if ( !proceed && stdDevs ) - cvZero(_JtJ); - - reprojErr = 0; - - for( i = 0, pos = 0; i < nimages; i++, pos += ni ) - { - 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 ); - - CvMat _Mi = cvMat(matM.colRange(pos, pos + ni)); - if( releaseObject ) + subMatrix(JtJ, JtJN, mask0, mask0); + completeSymm(JtJN, false); + cv::invert(JtJN, JtJinv, DECOMP_EIG); + // sigma2 is deviation of the noise + // see any papers about variance of the least squares estimator for + // detailed description of the variance estimation methods + double sigma2 = norm(allErrors, NORM_L2SQR) / (total - nparams_nz); + int j = 0; + for ( int s = 0; s < nparams; s++ ) + if( mask0.at(s) ) { - cvGetRows( solver.param, &_Mi, NINTRINSIC + nimages * 6, - NINTRINSIC + nimages * 6 + ni * 3 ); - cvReshape( &_Mi, &_Mi, 3, 1 ); - } - CvMat _mi = cvMat(_m.colRange(pos, pos + ni)); - CvMat _me = cvMat(allErrors.colRange(pos, pos + ni)); - - _Je.resize(ni*2); _Ji.resize(ni*2); _err.resize(ni*2); - _Jo.resize(ni*2); - - CvMat _mp = cvMat(_err.reshape(2, 1)); - - if( calcJ ) - { - CvMat _dpdr = cvMat(_Je.colRange(0, 3)); - CvMat _dpdt = cvMat(_Je.colRange(3, 6)); - CvMat _dpdf = cvMat(_Ji.colRange(0, 2)); - CvMat _dpdc = cvMat(_Ji.colRange(2, 4)); - CvMat _dpdk = cvMat(_Ji.colRange(4, NINTRINSIC)); - CvMat _dpdo = _Jo.empty() ? CvMat() : cvMat(_Jo.colRange(0, ni * 3)); - - cvProjectPoints2Internal( &_Mi, &_ri, &_ti, &matA, &_k, &_mp, &_dpdr, &_dpdt, - (flags & CALIB_FIX_FOCAL_LENGTH) ? nullptr : &_dpdf, - (flags & CALIB_FIX_PRINCIPAL_POINT) ? nullptr : &_dpdc, &_dpdk, - (_Jo.empty()) ? nullptr: &_dpdo, - (flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0); + stdDevs->at(s) = std::sqrt(JtJinv.at(j,j) * sigma2); + j++; } else - cvProjectPoints2Internal( &_Mi, &_ri, &_ti, &matA, &_k, &_mp, 0, 0, 0, 0, 0, 0 ); - - cvSub( &_mp, &_mi, &_mp ); - if (perViewErrors || stdDevs) - cvCopy(&_mp, &_me); - - if( calcJ ) - { - Mat JtJ(cvarrToMat(_JtJ)), JtErr(cvarrToMat(_JtErr)); - - // 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; - if( releaseObject ) - { - JtJ(Rect(NINTRINSIC + nimages * 6, 0, maxPoints * 3, NINTRINSIC)) += _Ji.t() * _Jo; - JtJ(Rect(NINTRINSIC + nimages * 6, NINTRINSIC + i * 6, maxPoints * 3, 6)) - += _Je.t() * _Jo; - JtJ(Rect(NINTRINSIC + nimages * 6, NINTRINSIC + nimages * 6, maxPoints * 3, maxPoints * 3)) - += _Jo.t() * _Jo; - } - - JtErr.rowRange(0, NINTRINSIC) += _Ji.t() * _err; - JtErr.rowRange(NINTRINSIC + i * 6, NINTRINSIC + (i + 1) * 6) = _Je.t() * _err; - if( releaseObject ) - { - JtErr.rowRange(NINTRINSIC + nimages * 6, nparams) += _Jo.t() * _err; - } - } - - double viewErr = norm(_err, NORM_L2SQR); - - if( perViewErrors ) - perViewErrors->data.db[i] = std::sqrt(viewErr / ni); - - reprojErr += viewErr; - } - if( _errNorm ) - *_errNorm = reprojErr; - - if( !proceed ) - { - if( stdDevs ) - { - Mat mask = cvarrToMat(solver.mask); - int nparams_nz = countNonZero(mask); - Mat JtJinv, JtJN; - JtJN.create(nparams_nz, nparams_nz, CV_64F); - subMatrix(cvarrToMat(_JtJ), JtJN, mask, mask); - completeSymm(JtJN, false); - cv::invert(JtJN, JtJinv, DECOMP_SVD); - //sigma2 is deviation of the noise - //see any papers about variance of the least squares estimator for - //detailed description of the variance estimation methods - double sigma2 = norm(allErrors, NORM_L2SQR) / (total - nparams_nz); - Mat stdDevsM = cvarrToMat(stdDevs); - int j = 0; - for ( int s = 0; s < nparams; s++ ) - if( mask.data[s] ) - { - stdDevsM.at(s) = std::sqrt(JtJinv.at(j,j) * sigma2); - j++; - } - else - stdDevsM.at(s) = 0.; - } - break; - } + stdDevs->at(s) = 0.; } // 4. store the results - cvConvert( &matA, cameraMatrix ); - cvConvert( &_k, distCoeffs ); + A = Matx33d(param[0], 0, param[2], 0, param[1], param[3], 0, 0, 1); + A.convertTo(cameraMatrix, cameraMatrix.type()); + _k = Mat(distCoeffs.size(), CV_64F, param + 4); + _k.convertTo(distCoeffs, distCoeffs.type()); if( newObjPoints && releaseObject ) { - CvMat _Mi; - cvGetRows( solver.param, &_Mi, NINTRINSIC + nimages * 6, - NINTRINSIC + nimages * 6 + maxPoints * 3 ); - cvReshape( &_Mi, &_Mi, 3, 1 ); - cvConvert( &_Mi, newObjPoints ); + Mat _Mi = param0.rowRange(NINTRINSIC + nimages * 6, + NINTRINSIC + nimages * 6 + maxPoints * 3); + _Mi.reshape(3, 1).convertTo(*newObjPoints, newObjPoints->type()); } for( i = 0, pos = 0; i < nimages; i++ ) { - CvMat src, dst; - if( rvecs ) { - src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 ); - if( rvecs->rows == nimages && rvecs->cols*CV_MAT_CN(rvecs->type) == 9 ) + Mat src = Mat(3, 1, CV_64F, param + NINTRINSIC + i*6); + if( rvecs->rows == nimages && rvecs->cols*rvecs->channels() == 9 ) { - dst = cvMat( 3, 3, CV_MAT_DEPTH(rvecs->type), - rvecs->data.ptr + rvecs->step*i ); - cvRodrigues2( &src, &matA ); - cvConvert( &matA, &dst ); + Mat dst(3, 3, rvecs->depth(), rvecs->ptr(i)); + Rodrigues(src, A); + A.convertTo(dst, dst.type()); } else { - dst = cvMat( 3, 1, CV_MAT_DEPTH(rvecs->type), rvecs->rows == 1 ? - rvecs->data.ptr + i*CV_ELEM_SIZE(rvecs->type) : - rvecs->data.ptr + rvecs->step*i ); - cvConvert( &src, &dst ); + Mat dst(3, 1, rvecs->depth(), rvecs->rows == 1 ? + rvecs->data + i*rvecs->elemSize1() : rvecs->ptr(i)); + src.convertTo(dst, dst.type()); } } if( tvecs ) { - src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 + 3 ); - dst = cvMat( 3, 1, CV_MAT_DEPTH(tvecs->type), tvecs->rows == 1 ? - tvecs->data.ptr + i*CV_ELEM_SIZE(tvecs->type) : - tvecs->data.ptr + tvecs->step*i ); - cvConvert( &src, &dst ); - } + Mat src(3, 1, CV_64F, param + NINTRINSIC + i*6 + 3); + Mat dst(3, 1, tvecs->depth(), tvecs->rows == 1 ? + tvecs->data + i*tvecs->elemSize1() : tvecs->ptr(i)); + src.convertTo(dst, dst.type()); + } } return std::sqrt(reprojErr/total); } - -/* finds intrinsic and extrinsic camera parameters - from a few views of known calibration pattern */ -static double cvCalibrateCamera2( const CvMat* objectPoints, - const CvMat* imagePoints, const CvMat* npoints, - CvSize imageSize, CvMat* cameraMatrix, CvMat* distCoeffs, - CvMat* rvecs, CvMat* tvecs, int flags, CvTermCriteria termCrit= - cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 30, DBL_EPSILON)) -{ - return cvCalibrateCamera2Internal(objectPoints, imagePoints, npoints, imageSize, -1, cameraMatrix, - distCoeffs, rvecs, tvecs, NULL, NULL, NULL, flags, termCrit); -} - -#if 0 -static double cvCalibrateCamera4( const CvMat* objectPoints, - const CvMat* imagePoints, const CvMat* npoints, - CvSize imageSize, int iFixedPoint, CvMat* cameraMatrix, CvMat* distCoeffs, - CvMat* rvecs, CvMat* tvecs, CvMat* newObjPoints, int flags, CvTermCriteria termCrit ) -{ - if( !CV_IS_MAT(npoints) ) - CV_Error( CV_StsBadArg, "npoints is not a valid matrix" ); - if( CV_MAT_TYPE(npoints->type) != CV_32SC1 || - (npoints->rows != 1 && npoints->cols != 1) ) - CV_Error( CV_StsUnsupportedFormat, - "the array of point counters must be 1-dimensional integer vector" ); - - bool releaseObject = iFixedPoint > 0 && iFixedPoint < npoints->data.i[0] - 1; - int nimages = npoints->rows * npoints->cols; - int npstep = npoints->rows == 1 ? 1 : npoints->step / CV_ELEM_SIZE(npoints->type); - int i, ni; - // check object points. If not qualified, report errors. - if( releaseObject ) - { - if( !CV_IS_MAT(objectPoints) ) - CV_Error( CV_StsBadArg, "objectPoints is not a valid matrix" ); - Mat matM; - if(CV_MAT_CN(objectPoints->type) == 3) { - matM = cvarrToMat(objectPoints); - } else { - convertPointsToHomogeneous(cvarrToMat(objectPoints), matM, CV_64F); - } - - matM = matM.reshape(3, 1); - ni = npoints->data.i[0]; - for( i = 1; i < nimages; i++ ) - { - if( npoints->data.i[i * npstep] != ni ) - { - CV_Error( CV_StsBadArg, "All objectPoints[i].size() should be equal when " - "object-releasing method is requested." ); - } - Mat ocmp = matM.colRange(ni * i, ni * i + ni) != matM.colRange(0, ni); - ocmp = ocmp.reshape(1); - if( countNonZero(ocmp) ) - { - CV_Error( CV_StsBadArg, "All objectPoints[i] should be identical when object-releasing" - " method is requested." ); - } - } - } - - return cvCalibrateCamera2Internal(objectPoints, imagePoints, npoints, imageSize, iFixedPoint, - cameraMatrix, distCoeffs, rvecs, tvecs, newObjPoints, NULL, - NULL, flags, termCrit); -} - -static void cvCalibrationMatrixValues( const CvMat *calibMatr, CvSize imgSize, - double apertureWidth, double apertureHeight, double *fovx, double *fovy, - double *focalLength, CvPoint2D64f *principalPoint, double *pasp ) -{ - /* Validate parameters. */ - if(calibMatr == 0) - CV_Error(CV_StsNullPtr, "Some of parameters is a NULL pointer!"); - - if(!CV_IS_MAT(calibMatr)) - CV_Error(CV_StsUnsupportedFormat, "Input parameters must be a matrices!"); - - double dummy = .0; - Point2d pp; - calibrationMatrixValues(cvarrToMat(calibMatr), imgSize, apertureWidth, apertureHeight, - fovx ? *fovx : dummy, - fovy ? *fovy : dummy, - focalLength ? *focalLength : dummy, - pp, - pasp ? *pasp : dummy); - - if(principalPoint) - *principalPoint = cvPoint2D64f(pp.x, pp.y); -} -#endif - //////////////////////////////// Stereo Calibration /////////////////////////////////// static int dbCmp( const void* _a, const void* _b ) @@ -1755,101 +662,96 @@ static int dbCmp( const void* _a, const void* _b ) return (a > b) - (a < b); } -static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _imagePoints1, - const CvMat* _imagePoints2, const CvMat* _npoints, - CvMat* _cameraMatrix1, CvMat* _distCoeffs1, - CvMat* _cameraMatrix2, CvMat* _distCoeffs2, - CvSize imageSize, CvMat* matR, CvMat* matT, - CvMat* matE, CvMat* matF, - CvMat* perViewErr, int flags, - CvTermCriteria termCrit ) +static double stereoCalibrateImpl( + const Mat& _objectPoints, const Mat& _imagePoints1, + const Mat& _imagePoints2, const Mat& _npoints, + Mat& _cameraMatrix1, Mat& _distCoeffs1, + Mat& _cameraMatrix2, Mat& _distCoeffs2, + Size imageSize, Mat* matR, Mat* matT, + Mat* matE, Mat* matF, + Mat* perViewErr, int flags, + TermCriteria termCrit ) { const int NINTRINSIC = 18; - Ptr npoints, imagePoints[2], objectPoints, RT0; double reprojErr = 0; - 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; + double dk[2][14]={{0}}; + Mat Dist[2]; + Matx33d A[2], R_LR; + int i, k, p, ni = 0, pos, pointsTotal = 0, maxPoints = 0, nparams; bool recomputeIntrinsics = false; double aspectRatio[2] = {0}; - CV_Assert( CV_IS_MAT(_imagePoints1) && CV_IS_MAT(_imagePoints2) && - CV_IS_MAT(_objectPoints) && CV_IS_MAT(_npoints) && - CV_IS_MAT(matR) && CV_IS_MAT(matT) ); + CV_Assert( _imagePoints1.type() == _imagePoints2.type() && + _imagePoints1.depth() == _objectPoints.depth() ); - CV_Assert( CV_ARE_TYPES_EQ(_imagePoints1, _imagePoints2) && - CV_ARE_DEPTHS_EQ(_imagePoints1, _objectPoints) ); + CV_Assert( (_npoints.cols == 1 || _npoints.rows == 1) && + _npoints.type() == CV_32S ); - CV_Assert( (_npoints->cols == 1 || _npoints->rows == 1) && - CV_MAT_TYPE(_npoints->type) == CV_32SC1 ); - - nimages = _npoints->cols + _npoints->rows - 1; - npoints.reset(cvCreateMat( _npoints->rows, _npoints->cols, _npoints->type )); - cvCopy( _npoints, npoints ); - - for( i = 0, pointsTotal = 0; i < nimages; i++ ) + int nimages = (int)_npoints.total(); + for( i = 0; i < nimages; i++ ) { - maxPoints = MAX(maxPoints, npoints->data.i[i]); - pointsTotal += npoints->data.i[i]; + ni = _npoints.at(i); + maxPoints = MAX(maxPoints, ni); + pointsTotal += ni; } - objectPoints.reset(cvCreateMat( _objectPoints->rows, _objectPoints->cols, - CV_64FC(CV_MAT_CN(_objectPoints->type)))); - cvConvert( _objectPoints, objectPoints ); - cvReshape( objectPoints, objectPoints, 3, 1 ); + Mat objectPoints, imagePoints[2]; + _objectPoints.convertTo(objectPoints, CV_64F); + objectPoints = objectPoints.reshape(3, 1); for( k = 0; k < 2; k++ ) { - const CvMat* points = k == 0 ? _imagePoints1 : _imagePoints2; - const CvMat* cameraMatrix = k == 0 ? _cameraMatrix1 : _cameraMatrix2; - const CvMat* distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2; + const Mat& points = k == 0 ? _imagePoints1 : _imagePoints2; + const Mat& cameraMatrix = k == 0 ? _cameraMatrix1 : _cameraMatrix2; + const Mat& distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2; - int cn = CV_MAT_CN(_imagePoints1->type); - CV_Assert( (CV_MAT_DEPTH(_imagePoints1->type) == CV_32F || - CV_MAT_DEPTH(_imagePoints1->type) == CV_64F) && - ((_imagePoints1->rows == pointsTotal && _imagePoints1->cols*cn == 2) || - (_imagePoints1->rows == 1 && _imagePoints1->cols == pointsTotal && cn == 2)) ); + int depth = points.depth(); + int cn = points.channels(); + CV_Assert( (depth == CV_32F || depth == CV_64F) && + ((points.rows == pointsTotal && points.cols*cn == 2) || + (points.rows == 1 && points.cols == pointsTotal && cn == 2))); - K[k] = cvMat(3,3,CV_64F,A[k]); - Dist[k] = cvMat(1,14,CV_64F,dk[k]); + A[k] = Matx33d(1, 0, 0, 0, 1, 0, 0, 0, 1); + Dist[k] = Mat(1,14,CV_64F,dk[k]); - imagePoints[k].reset(cvCreateMat( points->rows, points->cols, CV_64FC(CV_MAT_CN(points->type)))); - cvConvert( points, imagePoints[k] ); - cvReshape( imagePoints[k], imagePoints[k], 2, 1 ); + points.convertTo(imagePoints[k], CV_64F); + imagePoints[k] = imagePoints[k].reshape(2, 1); if( flags & (CALIB_FIX_INTRINSIC|CALIB_USE_INTRINSIC_GUESS| CALIB_FIX_ASPECT_RATIO|CALIB_FIX_FOCAL_LENGTH) ) - cvConvert( cameraMatrix, &K[k] ); + cameraMatrix.convertTo(A[k], CV_64F); if( flags & (CALIB_FIX_INTRINSIC|CALIB_USE_INTRINSIC_GUESS| CALIB_FIX_K1|CALIB_FIX_K2|CALIB_FIX_K3|CALIB_FIX_K4|CALIB_FIX_K5|CALIB_FIX_K6|CALIB_FIX_TANGENT_DIST) ) { - CvMat tdist = cvMat( distCoeffs->rows, distCoeffs->cols, - CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), Dist[k].data.db ); - cvConvert( distCoeffs, &tdist ); + Mat tdist( distCoeffs.size(), CV_MAKETYPE(CV_64F, distCoeffs.channels()), dk[k] ); + distCoeffs.convertTo(tdist, CV_64F); } if( !(flags & (CALIB_FIX_INTRINSIC|CALIB_USE_INTRINSIC_GUESS))) { - cvCalibrateCamera2( objectPoints, imagePoints[k], - npoints, imageSize, &K[k], &Dist[k], NULL, NULL, flags ); + Mat matA(A[k], false); + calibrateCameraInternal(objectPoints, imagePoints[k], + _npoints, imageSize, 0, matA, Dist[k], + 0, 0, 0, 0, 0, flags, termCrit); + //std::cout << "K(" << k << "): " << A[k] << "\n"; + //std::cout << "Dist(" << k << "): " << Dist[k] << "\n"; } } if( flags & CALIB_SAME_FOCAL_LENGTH ) { - static const int avg_idx[] = { 0, 4, 2, 5, -1 }; - for( k = 0; avg_idx[k] >= 0; k++ ) - A[0][avg_idx[k]] = A[1][avg_idx[k]] = (A[0][avg_idx[k]] + A[1][avg_idx[k]])*0.5; + A[0](0, 0) = A[1](0, 0) = (A[0](0, 0) + A[1](0, 0))*0.5; + A[0](0, 2) = A[1](0, 2) = (A[0](0, 2) + A[1](0, 2))*0.5; + A[0](1, 1) = A[1](1, 1) = (A[0](1, 1) + A[1](1, 1))*0.5; + A[0](1, 2) = A[1](1, 2) = (A[0](1, 2) + A[1](1, 2))*0.5; } if( flags & CALIB_FIX_ASPECT_RATIO ) { for( k = 0; k < 2; k++ ) - aspectRatio[k] = A[k][0]/A[k][4]; + aspectRatio[k] = A[k](0, 0)/A[k](1, 1); } recomputeIntrinsics = (flags & CALIB_FIX_INTRINSIC) == 0; @@ -1863,15 +765,12 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i // for intrinisic parameters of each camera ((fx,fy,cx,cy,k1,k2,p1,p2) ~ 8 parameters). nparams = 6*(nimages+1) + (recomputeIntrinsics ? NINTRINSIC*2 : 0); - CvLevMarq solver( nparams, 0, termCrit ); - - if(flags & CALIB_USE_LU) { - solver.solveMethod = DECOMP_LU; - } + std::vector mask(nparams, (uchar)0); + std::vector param(nparams, 0.); if( recomputeIntrinsics ) { - uchar* imask = solver.mask->data.ptr + nparams - NINTRINSIC*2; + uchar* imask = &mask[0] + nparams - NINTRINSIC*2; if( !(flags & CALIB_RATIONAL_MODEL) ) flags |= CALIB_FIX_K4 | CALIB_FIX_K5 | CALIB_FIX_K6; if( !(flags & CALIB_THIN_PRISM_MODEL) ) @@ -1913,7 +812,8 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i } // storage for initial [om(R){i}|t{i}] (in order to compute the median for each component) - RT0.reset(cvCreateMat( 6, nimages, CV_64F )); + Mat RT0(6, nimages, CV_64F); + double* RT0data = RT0.ptr(); /* Compute initial estimate of pose For each image, compute: @@ -1925,80 +825,77 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i om = median(om_ref_list) T = median(T_ref_list) */ - for( i = ofs = 0; i < nimages; ofs += ni, i++ ) + for( i = pos = 0; i < nimages; pos += ni, i++ ) { - ni = npoints->data.i[i]; - CvMat objpt_i; - double _om[2][3], r[2][9], t[2][3]; - CvMat om[2], R[2], T[2], imgpt_i[2]; - - objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3); + ni = _npoints.at(i); + Mat objpt_i(1, ni, CV_64FC3, objectPoints.ptr() + pos*3); + Matx33d R[2]; + Vec3d rv, T[2]; for( k = 0; k < 2; k++ ) { - imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2); - om[k] = cvMat(3, 1, CV_64F, _om[k]); - R[k] = cvMat(3, 3, CV_64F, r[k]); - T[k] = cvMat(3, 1, CV_64F, t[k]); + Mat imgpt_ik = Mat(1, ni, CV_64FC2, imagePoints[k].ptr() + pos*2); + solvePnP(objpt_i, imgpt_ik, A[k], Dist[k], rv, T[k], false, SOLVEPNP_ITERATIVE ); + Rodrigues(rv, R[k]); - cvFindExtrinsicCameraParams2( &objpt_i, &imgpt_i[k], &K[k], &Dist[k], &om[k], &T[k], 0 ); - cvRodrigues2( &om[k], &R[k] ); if( k == 0 ) { // save initial om_left and T_left - solver.param->data.db[(i+1)*6] = _om[0][0]; - solver.param->data.db[(i+1)*6 + 1] = _om[0][1]; - solver.param->data.db[(i+1)*6 + 2] = _om[0][2]; - solver.param->data.db[(i+1)*6 + 3] = t[0][0]; - solver.param->data.db[(i+1)*6 + 4] = t[0][1]; - solver.param->data.db[(i+1)*6 + 5] = t[0][2]; + param[(i+1)*6] = rv[0]; + param[(i+1)*6 + 1] = rv[1]; + param[(i+1)*6 + 2] = rv[2]; + param[(i+1)*6 + 3] = T[0][0]; + param[(i+1)*6 + 4] = T[0][1]; + param[(i+1)*6 + 5] = T[0][2]; } } - cvGEMM( &R[1], &R[0], 1, 0, 0, &R[0], CV_GEMM_B_T ); - cvGEMM( &R[0], &T[0], -1, &T[1], 1, &T[1] ); - cvRodrigues2( &R[0], &T[0] ); - RT0->data.db[i] = t[0][0]; - RT0->data.db[i + nimages] = t[0][1]; - RT0->data.db[i + nimages*2] = t[0][2]; - RT0->data.db[i + nimages*3] = t[1][0]; - RT0->data.db[i + nimages*4] = t[1][1]; - RT0->data.db[i + nimages*5] = t[1][2]; + R[0] = R[1]*R[0].t(); + T[1] -= R[0]*T[0]; + + Rodrigues(R[0], rv); + + RT0data[i] = rv[0]; + RT0data[i + nimages] = rv[1]; + RT0data[i + nimages*2] = rv[2]; + RT0data[i + nimages*3] = T[1][0]; + RT0data[i + nimages*4] = T[1][1]; + RT0data[i + nimages*5] = T[1][2]; } if(flags & CALIB_USE_EXTRINSIC_GUESS) { Vec3d R, T; - cvarrToMat(matT).convertTo(T, CV_64F); + matT->convertTo(T, CV_64F); if( matR->rows == 3 && matR->cols == 3 ) - Rodrigues(cvarrToMat(matR), R); + Rodrigues(*matR, R); else - cvarrToMat(matR).convertTo(R, CV_64F); + matR->convertTo(R, CV_64F); - solver.param->data.db[0] = R[0]; - solver.param->data.db[1] = R[1]; - solver.param->data.db[2] = R[2]; - solver.param->data.db[3] = T[0]; - solver.param->data.db[4] = T[1]; - solver.param->data.db[5] = T[2]; + param[0] = R[0]; + param[1] = R[1]; + param[2] = R[2]; + param[3] = T[0]; + param[4] = T[1]; + param[5] = T[2]; } else { // find the medians and save the first 6 parameters for( i = 0; i < 6; i++ ) { - qsort( RT0->data.db + i*nimages, nimages, CV_ELEM_SIZE(RT0->type), dbCmp ); - solver.param->data.db[i] = nimages % 2 != 0 ? RT0->data.db[i*nimages + nimages/2] : - (RT0->data.db[i*nimages + nimages/2 - 1] + RT0->data.db[i*nimages + nimages/2])*0.5; + double* rti = RT0data + i*nimages; + qsort( rti, nimages, sizeof(*rti), dbCmp ); + param[i] = nimages % 2 != 0 ? rti[nimages/2] : (rti[nimages/2 - 1] + rti[nimages/2])*0.5; } } if( recomputeIntrinsics ) for( k = 0; k < 2; k++ ) { - double* iparam = solver.param->data.db + (nimages+1)*6 + k*NINTRINSIC; + double* iparam = ¶m[(nimages+1)*6 + k*NINTRINSIC]; if( flags & CALIB_ZERO_TANGENT_DIST ) dk[k][2] = dk[k][3] = 0; - iparam[0] = A[k][0]; iparam[1] = A[k][4]; iparam[2] = A[k][2]; iparam[3] = A[k][5]; + iparam[0] = A[k](0, 0); iparam[1] = A[k](1, 1); iparam[2] = A[k](0, 2); iparam[3] = A[k](1, 2); iparam[4] = dk[k][0]; iparam[5] = dk[k][1]; iparam[6] = dk[k][2]; iparam[7] = dk[k][3]; iparam[8] = dk[k][4]; iparam[9] = dk[k][5]; iparam[10] = dk[k][6]; iparam[11] = dk[k][7]; @@ -2010,116 +907,93 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i iparam[17] = dk[k][13]; } - om_LR = cvMat(3, 1, CV_64F, solver.param->data.db); - T_LR = cvMat(3, 1, CV_64F, solver.param->data.db + 3); + A[0] = A[1] = Matx33d(1, 0, 0, 0, 1, 0, 0, 0, 1); - for(;;) + //std::cout << "param before LM: " << Mat(param, false).t() << "\n"; + + LMSolver::runAlt(param, mask, termCrit, DECOMP_SVD, false, + [&](Mat& _param, Mat* _JtErr, Mat* _JtJ, double* _errnorm) { - const CvMat* param = 0; - CvMat *JtJ = 0, *JtErr = 0; - double *_errNorm = 0; - double _omR[3], _tR[3]; - double _dr3dr1[9], _dr3dr2[9], /*_dt3dr1[9],*/ _dt3dr2[9], _dt3dt1[9], _dt3dt2[9]; - CvMat dr3dr1 = cvMat(3, 3, CV_64F, _dr3dr1); - CvMat dr3dr2 = cvMat(3, 3, CV_64F, _dr3dr2); - //CvMat dt3dr1 = cvMat(3, 3, CV_64F, _dt3dr1); - CvMat dt3dr2 = cvMat(3, 3, CV_64F, _dt3dr2); - CvMat dt3dt1 = cvMat(3, 3, CV_64F, _dt3dt1); - CvMat dt3dt2 = cvMat(3, 3, CV_64F, _dt3dt2); - CvMat om[2], T[2], imgpt_i[2]; + double* param_p = _param.ptr(); + Vec3d om_LR(param_p[0], param_p[1], param_p[2]); + Vec3d T_LR(param_p[3], param_p[4], param_p[5]); + Vec3d om[2], T[2]; + Matx33d dr3dr1, dr3dr2, dt3dr2, dt3dt1, dt3dt2; - if( !solver.updateAlt( param, JtJ, JtErr, _errNorm )) - break; reprojErr = 0; - - cvRodrigues2( &om_LR, &R_LR ); - om[1] = cvMat(3,1,CV_64F,_omR); - T[1] = cvMat(3,1,CV_64F,_tR); + Rodrigues(om_LR, R_LR); if( recomputeIntrinsics ) { - double* iparam = solver.param->data.db + (nimages+1)*6; - double* ipparam = solver.prevParam->data.db + (nimages+1)*6; + double* iparam = param_p + (nimages+1)*6; + //double* ipparam = solver.prevParam->data.db + (nimages+1)*6; if( flags & CALIB_SAME_FOCAL_LENGTH ) { iparam[NINTRINSIC] = iparam[0]; iparam[NINTRINSIC+1] = iparam[1]; - ipparam[NINTRINSIC] = ipparam[0]; - ipparam[NINTRINSIC+1] = ipparam[1]; + //ipparam[NINTRINSIC] = ipparam[0]; + //ipparam[NINTRINSIC+1] = ipparam[1]; } if( flags & CALIB_FIX_ASPECT_RATIO ) { iparam[0] = iparam[1]*aspectRatio[0]; iparam[NINTRINSIC] = iparam[NINTRINSIC+1]*aspectRatio[1]; - ipparam[0] = ipparam[1]*aspectRatio[0]; - ipparam[NINTRINSIC] = ipparam[NINTRINSIC+1]*aspectRatio[1]; + //ipparam[0] = ipparam[1]*aspectRatio[0]; + //ipparam[NINTRINSIC] = ipparam[NINTRINSIC+1]*aspectRatio[1]; } for( k = 0; k < 2; k++ ) { - A[k][0] = iparam[k*NINTRINSIC+0]; - A[k][4] = iparam[k*NINTRINSIC+1]; - A[k][2] = iparam[k*NINTRINSIC+2]; - A[k][5] = iparam[k*NINTRINSIC+3]; - dk[k][0] = iparam[k*NINTRINSIC+4]; - dk[k][1] = iparam[k*NINTRINSIC+5]; - dk[k][2] = iparam[k*NINTRINSIC+6]; - dk[k][3] = iparam[k*NINTRINSIC+7]; - dk[k][4] = iparam[k*NINTRINSIC+8]; - dk[k][5] = iparam[k*NINTRINSIC+9]; - dk[k][6] = iparam[k*NINTRINSIC+10]; - dk[k][7] = iparam[k*NINTRINSIC+11]; - dk[k][8] = iparam[k*NINTRINSIC+12]; - dk[k][9] = iparam[k*NINTRINSIC+13]; - dk[k][10] = iparam[k*NINTRINSIC+14]; - dk[k][11] = iparam[k*NINTRINSIC+15]; - dk[k][12] = iparam[k*NINTRINSIC+16]; - dk[k][13] = iparam[k*NINTRINSIC+17]; + A[k] = Matx33d(iparam[k*NINTRINSIC+0], 0, iparam[k*NINTRINSIC+2], + 0, iparam[k*NINTRINSIC+1], iparam[k*NINTRINSIC+3], + 0, 0, 1); + for(int j = 0; j < 14; j++) + dk[k][j] = iparam[k*NINTRINSIC+4+j]; } } - for( i = ofs = 0; i < nimages; ofs += ni, i++ ) + for( i = pos = 0; i < nimages; pos += ni, i++ ) { - ni = npoints->data.i[i]; - CvMat objpt_i; + ni = _npoints.at(i); - om[0] = cvMat(3,1,CV_64F,solver.param->data.db+(i+1)*6); - T[0] = cvMat(3,1,CV_64F,solver.param->data.db+(i+1)*6+3); + double* pi = param_p + (i+1)*6; + om[0] = Vec3d(pi[0], pi[1], pi[2]); + T[0] = Vec3d(pi[3], pi[4], pi[5]); - if( JtJ || JtErr ) - cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1], &dr3dr1, 0, - &dr3dr2, 0, 0, &dt3dt1, &dt3dr2, &dt3dt2 ); + if( _JtJ || _JtErr ) + composeRT( om[0], T[0], om_LR, T_LR, om[1], T[1], dr3dr1, noArray(), + dr3dr2, noArray(), noArray(), dt3dt1, dt3dr2, dt3dt2 ); else - cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1] ); + composeRT( om[0], T[0], om_LR, T_LR, om[1], T[1] ); - objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3); + Mat objpt_i(1, ni, CV_64FC3, objectPoints.ptr() + pos*3); err.resize(ni*2); Je.resize(ni*2); J_LR.resize(ni*2); Ji.resize(ni*2); - CvMat tmpimagePoints = cvMat(err.reshape(2, 1)); - CvMat dpdf = cvMat(Ji.colRange(0, 2)); - CvMat dpdc = cvMat(Ji.colRange(2, 4)); - CvMat dpdk = cvMat(Ji.colRange(4, NINTRINSIC)); - CvMat dpdrot = cvMat(Je.colRange(0, 3)); - CvMat dpdt = cvMat(Je.colRange(3, 6)); + Mat tmpImagePoints = err.reshape(2, 1); + Mat dpdf = Ji.colRange(0, 2); + Mat dpdc = Ji.colRange(2, 4); + Mat dpdk = Ji.colRange(4, NINTRINSIC); + Mat dpdrot = Je.colRange(0, 3); + Mat dpdt = Je.colRange(3, 6); for( k = 0; k < 2; k++ ) { - imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2); + Mat imgpt_ik(1, ni, CV_64FC2, imagePoints[k].ptr() + pos*2); - if( JtJ || JtErr ) - cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k], - &tmpimagePoints, &dpdrot, &dpdt, &dpdf, &dpdc, &dpdk, - (flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio[k] : 0); + if( _JtJ || _JtErr ) + projectPoints(objpt_i, om[k], T[k], A[k], Dist[k], + tmpImagePoints, dpdrot, dpdt, dpdf, dpdc, dpdk, noArray(), + (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 ); + projectPoints(objpt_i, om[k], T[k], A[k], Dist[k], tmpImagePoints); + subtract( tmpImagePoints, imgpt_ik, tmpImagePoints ); - if( solver.state == CvLevMarq::CALC_J ) + if( _JtJ ) { + Mat& JtErr = *_JtErr; + Mat& JtJ = *_JtJ; int iofs = (nimages+1)*6 + k*NINTRINSIC, eofs = (i+1)*6; - assert( JtJ && JtErr ); - - Mat _JtJ(cvarrToMat(JtJ)), _JtErr(cvarrToMat(JtErr)); + assert( _JtJ && _JtErr ); if( k == 1 ) { @@ -2127,340 +1001,101 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i // 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.ptr(p)); - CvMat de3dt3 = cvMat( 1, 3, CV_64F, de3dr3.data.db + 3 ); - CvMat de3dr2 = cvMat( 1, 3, CV_64F, J_LR.ptr(p) ); - CvMat de3dt2 = cvMat( 1, 3, CV_64F, de3dr2.data.db + 3 ); + Mat de3dr3( 1, 3, CV_64F, Je.ptr(p)); + Mat de3dt3( 1, 3, CV_64F, de3dr3.ptr() + 3 ); + Mat de3dr2( 1, 3, CV_64F, J_LR.ptr(p) ); + Mat de3dt2( 1, 3, CV_64F, de3dr2.ptr() + 3 ); double _de3dr1[3], _de3dt1[3]; - CvMat de3dr1 = cvMat( 1, 3, CV_64F, _de3dr1 ); - CvMat de3dt1 = cvMat( 1, 3, CV_64F, _de3dt1 ); + Mat de3dr1( 1, 3, CV_64F, _de3dr1 ); + Mat de3dt1( 1, 3, CV_64F, _de3dt1 ); - cvMatMul( &de3dr3, &dr3dr1, &de3dr1 ); - cvMatMul( &de3dt3, &dt3dt1, &de3dt1 ); + gemm(de3dr3, dr3dr1, 1, noArray(), 0, de3dr1); + gemm(de3dt3, dt3dt1, 1, noArray(), 0, de3dt1); - cvMatMul( &de3dr3, &dr3dr2, &de3dr2 ); - cvMatMulAdd( &de3dt3, &dt3dr2, &de3dr2, &de3dr2 ); + gemm(de3dr3, dr3dr2, 1, noArray(), 0, de3dr2); + gemm(de3dt3, dt3dr2, 1, de3dr2, 1, de3dr2); + gemm(de3dt3, dt3dt2, 1, noArray(), 0, de3dt2); - cvMatMul( &de3dt3, &dt3dt2, &de3dt2 ); - - cvCopy( &de3dr1, &de3dr3 ); - cvCopy( &de3dt1, &de3dt3 ); + de3dr1.copyTo(de3dr3); + de3dt1.copyTo(de3dt3); } - _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; + 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; } - _JtJ(Rect(eofs, eofs, 6, 6)) += Je.t()*Je; - _JtErr.rowRange(eofs, eofs + 6) += Je.t()*err; + JtJ(Rect(eofs, eofs, 6, 6)) += Je.t()*Je; + JtErr.rowRange(eofs, eofs + 6) += Je.t()*err; if( recomputeIntrinsics ) { - _JtJ(Rect(iofs, iofs, NINTRINSIC, NINTRINSIC)) += Ji.t()*Ji; - _JtJ(Rect(iofs, eofs, NINTRINSIC, 6)) += Je.t()*Ji; + JtJ(Rect(iofs, iofs, NINTRINSIC, NINTRINSIC)) += Ji.t()*Ji; + JtJ(Rect(iofs, eofs, NINTRINSIC, 6)) += Je.t()*Ji; if( k == 1 ) { - _JtJ(Rect(iofs, 0, NINTRINSIC, 6)) += J_LR.t()*Ji; + JtJ(Rect(iofs, 0, NINTRINSIC, 6)) += J_LR.t()*Ji; } - _JtErr.rowRange(iofs, iofs + NINTRINSIC) += Ji.t()*err; + JtErr.rowRange(iofs, iofs + NINTRINSIC) += Ji.t()*err; } } double viewErr = norm(err, NORM_L2SQR); - if(perViewErr) - perViewErr->data.db[i*2 + k] = std::sqrt(viewErr/ni); - + perViewErr->at(i, k) = std::sqrt(viewErr/ni); reprojErr += viewErr; } } - if(_errNorm) - *_errNorm = reprojErr; - } + if(_errnorm) + *_errnorm = reprojErr; + return true; + }); - cvRodrigues2( &om_LR, &R_LR ); + Vec3d om_LR(param[0], param[1], param[2]); + Vec3d T_LR(param[3], param[4], param[5]); + Rodrigues( om_LR, R_LR ); if( matR->rows == 1 || matR->cols == 1 ) - cvConvert( &om_LR, matR ); + om_LR.convertTo(*matR, matR->depth()); else - cvConvert( &R_LR, matR ); - cvConvert( &T_LR, matT ); + R_LR.convertTo(*matR, matR->depth()); + T_LR.convertTo(*matT, matT->depth()); + for( k = 0; k < 2; k++ ) + { + double* iparam = ¶m[(nimages+1)*6 + k*NINTRINSIC]; + A[k] = Matx33d(iparam[0], 0, iparam[2], 0, iparam[1], iparam[3], 0, 0, 1); + } if( recomputeIntrinsics ) { - cvConvert( &K[0], _cameraMatrix1 ); - cvConvert( &K[1], _cameraMatrix2 ); - for( k = 0; k < 2; k++ ) { - CvMat* distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2; - CvMat tdist = cvMat( distCoeffs->rows, distCoeffs->cols, - CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), Dist[k].data.db ); - cvConvert( &tdist, distCoeffs ); + Mat& cameraMatrix = k == 0 ? _cameraMatrix1 : _cameraMatrix2; + Mat& distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2; + A[k].convertTo(cameraMatrix, cameraMatrix.depth()); + Mat tdist( distCoeffs.size(), CV_MAKETYPE(CV_64F,distCoeffs.channels()), Dist[k].data ); + tdist.convertTo(distCoeffs, distCoeffs.depth()); } } if( matE || matF ) { - double* t = T_LR.data.db; - double tx[] = - { - 0, -t[2], t[1], - t[2], 0, -t[0], - -t[1], t[0], 0 - }; - CvMat Tx = cvMat(3, 3, CV_64F, tx); - double e[9], f[9]; - CvMat E = cvMat(3, 3, CV_64F, e); - CvMat F = cvMat(3, 3, CV_64F, f); - cvMatMul( &Tx, &R_LR, &E ); - if( matE ) - cvConvert( &E, matE ); + Matx33d Tx(0, -T_LR[2], T_LR[1], + T_LR[2], 0, -T_LR[0], + -T_LR[1], T_LR[0], 0); + Matx33d E = Tx*R_LR; + if (matE) + E.convertTo(*matE, matE->depth()); if( matF ) { - double ik[9]; - CvMat iK = cvMat(3, 3, CV_64F, ik); - cvInvert(&K[1], &iK); - cvGEMM( &iK, &E, 1, 0, 0, &E, CV_GEMM_A_T ); - cvInvert(&K[0], &iK); - cvMatMul(&E, &iK, &F); - cvConvertScale( &F, matF, fabs(f[8]) > 0 ? 1./f[8] : 1 ); + Matx33d iA0 = A[0].inv(), iA1 = A[1].inv(); + Matx33d F = iA1.t()*E*iA0; + F.convertTo(*matF, matF->depth(), fabs(F(2,2)) > 0 ? 1./F(2,2) : 1.); } } return std::sqrt(reprojErr/(pointsTotal*2)); } -#if 0 -static double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1, - const CvMat* _imagePoints2, const CvMat* _npoints, - CvMat* _cameraMatrix1, CvMat* _distCoeffs1, - CvMat* _cameraMatrix2, CvMat* _distCoeffs2, - CvSize imageSize, CvMat* matR, CvMat* matT, - CvMat* matE, CvMat* matF, - int flags, - CvTermCriteria termCrit ) -{ - return cvStereoCalibrateImpl(_objectPoints, _imagePoints1, _imagePoints2, _npoints, _cameraMatrix1, - _distCoeffs1, _cameraMatrix2, _distCoeffs2, imageSize, matR, matT, matE, - matF, NULL, flags, termCrit); -} - -static void -cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ, - CvMat *matrixQx, CvMat *matrixQy, CvMat *matrixQz, - CvPoint3D64f *eulerAngles) -{ - double matM[3][3], matR[3][3], matQ[3][3]; - CvMat M = cvMat(3, 3, CV_64F, matM); - CvMat R = cvMat(3, 3, CV_64F, matR); - CvMat Q = cvMat(3, 3, CV_64F, matQ); - double z, c, s; - - /* Validate parameters. */ - CV_Assert( CV_IS_MAT(matrixM) && CV_IS_MAT(matrixR) && CV_IS_MAT(matrixQ) && - matrixM->cols == 3 && matrixM->rows == 3 && - CV_ARE_SIZES_EQ(matrixM, matrixR) && CV_ARE_SIZES_EQ(matrixM, matrixQ)); - - cvConvert(matrixM, &M); - - /* Find Givens rotation Q_x for x axis (left multiplication). */ - /* - ( 1 0 0 ) - Qx = ( 0 c s ), c = m33/sqrt(m32^2 + m33^2), s = m32/sqrt(m32^2 + m33^2) - ( 0 -s c ) - */ - s = matM[2][1]; - c = matM[2][2]; - z = 1./std::sqrt(c * c + s * s + DBL_EPSILON); - c *= z; - s *= z; - - double _Qx[3][3] = { {1, 0, 0}, {0, c, s}, {0, -s, c} }; - CvMat Qx = cvMat(3, 3, CV_64F, _Qx); - - cvMatMul(&M, &Qx, &R); - assert(fabs(matR[2][1]) < FLT_EPSILON); - matR[2][1] = 0; - - /* Find Givens rotation for y axis. */ - /* - ( c 0 -s ) - Qy = ( 0 1 0 ), c = m33/sqrt(m31^2 + m33^2), s = -m31/sqrt(m31^2 + m33^2) - ( s 0 c ) - */ - s = -matR[2][0]; - c = matR[2][2]; - z = 1./std::sqrt(c * c + s * s + DBL_EPSILON); - c *= z; - s *= z; - - double _Qy[3][3] = { {c, 0, -s}, {0, 1, 0}, {s, 0, c} }; - CvMat Qy = cvMat(3, 3, CV_64F, _Qy); - cvMatMul(&R, &Qy, &M); - - assert(fabs(matM[2][0]) < FLT_EPSILON); - matM[2][0] = 0; - - /* Find Givens rotation for z axis. */ - /* - ( c s 0 ) - Qz = (-s c 0 ), c = m22/sqrt(m21^2 + m22^2), s = m21/sqrt(m21^2 + m22^2) - ( 0 0 1 ) - */ - - s = matM[1][0]; - c = matM[1][1]; - z = 1./std::sqrt(c * c + s * s + DBL_EPSILON); - c *= z; - s *= z; - - double _Qz[3][3] = { {c, s, 0}, {-s, c, 0}, {0, 0, 1} }; - CvMat Qz = cvMat(3, 3, CV_64F, _Qz); - - cvMatMul(&M, &Qz, &R); - assert(fabs(matR[1][0]) < FLT_EPSILON); - matR[1][0] = 0; - - // Solve the decomposition ambiguity. - // Diagonal entries of R, except the last one, shall be positive. - // Further rotate R by 180 degree if necessary - if( matR[0][0] < 0 ) - { - if( matR[1][1] < 0 ) - { - // rotate around z for 180 degree, i.e. a rotation matrix of - // [-1, 0, 0], - // [ 0, -1, 0], - // [ 0, 0, 1] - matR[0][0] *= -1; - matR[0][1] *= -1; - matR[1][1] *= -1; - - _Qz[0][0] *= -1; - _Qz[0][1] *= -1; - _Qz[1][0] *= -1; - _Qz[1][1] *= -1; - } - else - { - // rotate around y for 180 degree, i.e. a rotation matrix of - // [-1, 0, 0], - // [ 0, 1, 0], - // [ 0, 0, -1] - matR[0][0] *= -1; - matR[0][2] *= -1; - matR[1][2] *= -1; - matR[2][2] *= -1; - - cvTranspose( &Qz, &Qz ); - - _Qy[0][0] *= -1; - _Qy[0][2] *= -1; - _Qy[2][0] *= -1; - _Qy[2][2] *= -1; - } - } - else if( matR[1][1] < 0 ) - { - // ??? for some reason, we never get here ??? - - // rotate around x for 180 degree, i.e. a rotation matrix of - // [ 1, 0, 0], - // [ 0, -1, 0], - // [ 0, 0, -1] - matR[0][1] *= -1; - matR[0][2] *= -1; - matR[1][1] *= -1; - matR[1][2] *= -1; - matR[2][2] *= -1; - - cvTranspose( &Qz, &Qz ); - cvTranspose( &Qy, &Qy ); - - _Qx[1][1] *= -1; - _Qx[1][2] *= -1; - _Qx[2][1] *= -1; - _Qx[2][2] *= -1; - } - - // calculate the euler angle - if( eulerAngles ) - { - eulerAngles->x = acos(_Qx[1][1]) * (_Qx[1][2] >= 0 ? 1 : -1) * (180.0 / CV_PI); - eulerAngles->y = acos(_Qy[0][0]) * (_Qy[2][0] >= 0 ? 1 : -1) * (180.0 / CV_PI); - eulerAngles->z = acos(_Qz[0][0]) * (_Qz[0][1] >= 0 ? 1 : -1) * (180.0 / CV_PI); - } - - /* Calculate orthogonal matrix. */ - /* - Q = QzT * QyT * QxT - */ - cvGEMM( &Qz, &Qy, 1, 0, 0, &M, CV_GEMM_A_T + CV_GEMM_B_T ); - cvGEMM( &M, &Qx, 1, 0, 0, &Q, CV_GEMM_B_T ); - - /* Save R and Q matrices. */ - cvConvert( &R, matrixR ); - cvConvert( &Q, matrixQ ); - - if( matrixQx ) - cvConvert(&Qx, matrixQx); - if( matrixQy ) - cvConvert(&Qy, matrixQy); - if( matrixQz ) - cvConvert(&Qz, matrixQz); -} - -static void -cvDecomposeProjectionMatrix( const CvMat *projMatr, CvMat *calibMatr, - CvMat *rotMatr, CvMat *posVect, - CvMat *rotMatrX, CvMat *rotMatrY, - CvMat *rotMatrZ, CvPoint3D64f *eulerAngles) -{ - double tmpProjMatrData[16], tmpMatrixDData[16], tmpMatrixVData[16]; - CvMat tmpProjMatr = cvMat(4, 4, CV_64F, tmpProjMatrData); - CvMat tmpMatrixD = cvMat(4, 4, CV_64F, tmpMatrixDData); - CvMat tmpMatrixV = cvMat(4, 4, CV_64F, tmpMatrixVData); - CvMat tmpMatrixM; - - /* Validate parameters. */ - if(projMatr == 0 || calibMatr == 0 || rotMatr == 0 || posVect == 0) - CV_Error(CV_StsNullPtr, "Some of parameters is a NULL pointer!"); - - if(!CV_IS_MAT(projMatr) || !CV_IS_MAT(calibMatr) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(posVect)) - CV_Error(CV_StsUnsupportedFormat, "Input parameters must be a matrices!"); - - if(projMatr->cols != 4 || projMatr->rows != 3) - CV_Error(CV_StsUnmatchedSizes, "Size of projection matrix must be 3x4!"); - - if(calibMatr->cols != 3 || calibMatr->rows != 3 || rotMatr->cols != 3 || rotMatr->rows != 3) - CV_Error(CV_StsUnmatchedSizes, "Size of calibration and rotation matrices must be 3x3!"); - - if(posVect->cols != 1 || posVect->rows != 4) - CV_Error(CV_StsUnmatchedSizes, "Size of position vector must be 4x1!"); - - /* Compute position vector. */ - cvSetZero(&tmpProjMatr); // Add zero row to make matrix square. - int i, k; - for(i = 0; i < 3; i++) - for(k = 0; k < 4; k++) - cvmSet(&tmpProjMatr, i, k, cvmGet(projMatr, i, k)); - - cvSVD(&tmpProjMatr, &tmpMatrixD, NULL, &tmpMatrixV, CV_SVD_MODIFY_A + CV_SVD_V_T); - - /* Save position vector. */ - for(i = 0; i < 4; i++) - cvmSet(posVect, i, 0, cvmGet(&tmpMatrixV, 3, i)); // Solution is last row of V. - - /* Compute calibration and rotation matrices via RQ decomposition. */ - cvGetCols(projMatr, &tmpMatrixM, 0, 3); // M is first square matrix of P. - - CV_Assert(cvDet(&tmpMatrixM) != 0.0); // So far only finite cameras could be decomposed, so M has to be nonsingular [det(M) != 0]. - - cvRQDecomp3x3(&tmpMatrixM, calibMatr, rotMatr, rotMatrX, rotMatrY, rotMatrZ, eulerAngles); -} -#endif - static void collectCalibrationData( InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, @@ -2674,8 +1309,11 @@ double calibrateCameraRO(InputArrayOfArrays _objectPoints, Mat cameraMatrix = _cameraMatrix.getMat(); cameraMatrix = prepareCameraMatrix(cameraMatrix, rtype, flags); Mat distCoeffs = _distCoeffs.getMat(); - distCoeffs = (flags & CALIB_THIN_PRISM_MODEL) && !(flags & CALIB_TILTED_MODEL) ? prepareDistCoeffs(distCoeffs, rtype, 12) : - prepareDistCoeffs(distCoeffs, rtype); + distCoeffs = + (flags & CALIB_THIN_PRISM_MODEL) && + !(flags & CALIB_TILTED_MODEL) ? + prepareDistCoeffs(distCoeffs, rtype, 12) : + prepareDistCoeffs(distCoeffs, rtype); if( !(flags & CALIB_RATIONAL_MODEL) && (!(flags & CALIB_THIN_PRISM_MODEL)) && (!(flags & CALIB_TILTED_MODEL))) @@ -2742,19 +1380,14 @@ double calibrateCameraRO(InputArrayOfArrays _objectPoints, errorsM = _perViewErrors.getMat(); } - CvMat c_objPt = cvMat(objPt), c_imgPt = cvMat(imgPt), c_npoints = cvMat(npoints); - CvMat c_cameraMatrix = cvMat(cameraMatrix), c_distCoeffs = cvMat(distCoeffs); - CvMat c_rvecM = cvMat(rvecM), c_tvecM = cvMat(tvecM), c_stdDev = cvMat(stdDeviationsM), c_errors = cvMat(errorsM); - CvMat c_newObjPt = cvMat( newObjPt ); - - double reprojErr = cvCalibrateCamera2Internal(&c_objPt, &c_imgPt, &c_npoints, cvSize(imageSize), - iFixedPoint, - &c_cameraMatrix, &c_distCoeffs, - rvecs_needed ? &c_rvecM : NULL, - tvecs_needed ? &c_tvecM : NULL, - newobj_needed ? &c_newObjPt : NULL, - stddev_any_needed ? &c_stdDev : NULL, - errors_needed ? &c_errors : NULL, flags, cvTermCriteria(criteria)); + double reprojErr = calibrateCameraInternal( + objPt, imgPt, npoints, imageSize, iFixedPoint, + cameraMatrix, distCoeffs, + rvecs_needed ? &rvecM : NULL, + tvecs_needed ? &tvecM : NULL, + newobj_needed ? &newObjPt : NULL, + stddev_any_needed ? &stdDeviationsM : NULL, + errors_needed ? &errorsM : NULL, flags, cvTermCriteria(criteria)); if( newobj_needed ) newObjPt.copyTo(newObjPoints); @@ -2819,12 +1452,12 @@ void calibrationMatrixValues( InputArray _cameraMatrix, Size imageSize, if(_cameraMatrix.size() != Size(3, 3)) CV_Error(CV_StsUnmatchedSizes, "Size of cameraMatrix must be 3x3!"); - Matx33d K = _cameraMatrix.getMat(); - - CV_DbgAssert(imageSize.width != 0 && imageSize.height != 0 && K(0, 0) != 0.0 && K(1, 1) != 0.0); + Matx33d A; + _cameraMatrix.getMat().convertTo(A, CV_64F); + CV_DbgAssert(imageSize.width != 0 && imageSize.height != 0 && A(0, 0) != 0.0 && A(1, 1) != 0.0); /* Calculate pixel aspect ratio. */ - aspectRatio = K(1, 1) / K(0, 0); + aspectRatio = A(1, 1) / A(0, 0); /* Calculate number of pixel per realworld unit. */ double mx, my; @@ -2837,16 +1470,16 @@ void calibrationMatrixValues( InputArray _cameraMatrix, Size imageSize, } /* Calculate fovx and fovy. */ - fovx = atan2(K(0, 2), K(0, 0)) + atan2(imageSize.width - K(0, 2), K(0, 0)); - fovy = atan2(K(1, 2), K(1, 1)) + atan2(imageSize.height - K(1, 2), K(1, 1)); + fovx = atan2(A(0, 2), A(0, 0)) + atan2(imageSize.width - A(0, 2), A(0, 0)); + fovy = atan2(A(1, 2), A(1, 1)) + atan2(imageSize.height - A(1, 2), A(1, 1)); fovx *= 180.0 / CV_PI; fovy *= 180.0 / CV_PI; /* Calculate focal length. */ - focalLength = K(0, 0) / mx; + focalLength = A(0, 0) / mx; /* Calculate principle point. */ - principalPoint = Point2d(K(0, 2) / mx, K(1, 2) / my); + principalPoint = Point2d(A(0, 2) / mx, A(1, 2) / my); } double stereoCalibrate( InputArrayOfArrays _objectPoints, @@ -2908,41 +1541,33 @@ double stereoCalibrate( InputArrayOfArrays _objectPoints, collectCalibrationData( _objectPoints, _imagePoints1, _imagePoints2, objPt, imgPt, &imgPt2, npoints ); - CvMat c_objPt = cvMat(objPt), c_imgPt = cvMat(imgPt), c_imgPt2 = cvMat(imgPt2), c_npoints = cvMat(npoints); - CvMat c_cameraMatrix1 = cvMat(cameraMatrix1), c_distCoeffs1 = cvMat(distCoeffs1); - CvMat c_cameraMatrix2 = cvMat(cameraMatrix2), c_distCoeffs2 = cvMat(distCoeffs2); - Mat matR_ = _Rmat.getMat(), matT_ = _Tmat.getMat(); - CvMat c_matR = cvMat(matR_), c_matT = cvMat(matT_), c_matE, c_matF, c_matErr; + Mat matR = _Rmat.getMat(), matT = _Tmat.getMat(); bool E_needed = _Emat.needed(), F_needed = _Fmat.needed(), errors_needed = _perViewErrors.needed(); - Mat matE_, matF_, matErr_; + Mat matE, matF, matErr; if( E_needed ) { _Emat.create(3, 3, rtype); - matE_ = _Emat.getMat(); - c_matE = cvMat(matE_); + matE = _Emat.getMat(); } if( F_needed ) { _Fmat.create(3, 3, rtype); - matF_ = _Fmat.getMat(); - c_matF = cvMat(matF_); + matF = _Fmat.getMat(); } if( errors_needed ) { int nimages = int(_objectPoints.total()); _perViewErrors.create(nimages, 2, CV_64F); - matErr_ = _perViewErrors.getMat(); - c_matErr = cvMat(matErr_); + matErr = _perViewErrors.getMat(); } - double err = cvStereoCalibrateImpl(&c_objPt, &c_imgPt, &c_imgPt2, &c_npoints, &c_cameraMatrix1, - &c_distCoeffs1, &c_cameraMatrix2, &c_distCoeffs2, cvSize(imageSize), &c_matR, - &c_matT, E_needed ? &c_matE : NULL, F_needed ? &c_matF : NULL, - errors_needed ? &c_matErr : NULL, flags, cvTermCriteria(criteria)); - + double err = stereoCalibrateImpl(objPt, imgPt, imgPt2, npoints, cameraMatrix1, + distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, + &matR, &matT, E_needed ? &matE : NULL, F_needed ? &matF : NULL, + errors_needed ? &matErr : NULL, flags, criteria); cameraMatrix1.copyTo(_cameraMatrix1); cameraMatrix2.copyTo(_cameraMatrix2); distCoeffs1.copyTo(_distCoeffs1); diff --git a/modules/calib/test/test_cameracalibration.cpp b/modules/calib/test/test_cameracalibration.cpp index 59c2891df4..f1d0f873ed 100644 --- a/modules/calib/test/test_cameracalibration.cpp +++ b/modules/calib/test/test_cameracalibration.cpp @@ -1436,6 +1436,7 @@ void CV_StereoCalibrationTest::run( int ) + CALIB_FIX_K3 + CALIB_FIX_K4 + CALIB_FIX_K5 //+ CV_CALIB_FIX_K6 ); + err /= nframes*npoints; if( err > maxReprojErr ) { @@ -1945,7 +1946,8 @@ TEST(Calib3d_StereoCalibrate_CPP, extended) double res1 = stereoCalibrate( objpt, imgpt1, imgpt2, K1, c1, K2, c2, imageSize, R, T, E, F, err, flags); - EXPECT_LE(res1, res0); + printf("res0 = %g, res1 = %g\n", res0, res1); + EXPECT_LE(res1, res0*1.1); EXPECT_TRUE(err.total() == 2); } diff --git a/modules/calib/test/test_cameracalibration_badarg.cpp b/modules/calib/test/test_cameracalibration_badarg.cpp index 2a40077bb7..e38e788edb 100644 --- a/modules/calib/test/test_cameracalibration_badarg.cpp +++ b/modules/calib/test/test_cameracalibration_badarg.cpp @@ -232,8 +232,6 @@ protected: void run(int /* start_from */ ) { - Mat zeros(1, sizeof(CvMat), CV_8U, Scalar(0)); - Mat src_cpp(3, 1, CV_32F); Mat dst_cpp(3, 3, CV_32F); diff --git a/modules/core/include/opencv2/core/mat.hpp b/modules/core/include/opencv2/core/mat.hpp index 84df297bf9..44d123b416 100644 --- a/modules/core/include/opencv2/core/mat.hpp +++ b/modules/core/include/opencv2/core/mat.hpp @@ -370,6 +370,7 @@ public: void release() const; void clear() const; void setTo(const _InputArray& value, const _InputArray & mask = _InputArray()) const; + void setZero() const; void assign(const UMat& u) const; void assign(const Mat& m) const; @@ -1259,6 +1260,10 @@ public: */ Mat& setTo(InputArray value, InputArray mask=noArray()); + /** @brief Sets all the array elements to 0. + */ + Mat& setZero(); + /** @brief Changes the shape and/or the number of channels of a 2D matrix without copying the data. The method makes a new matrix header for \*this elements. The new matrix may have a different size diff --git a/modules/core/include/opencv2/core/matx.hpp b/modules/core/include/opencv2/core/matx.hpp index 45d8a97f34..ae2fce8423 100644 --- a/modules/core/include/opencv2/core/matx.hpp +++ b/modules/core/include/opencv2/core/matx.hpp @@ -58,6 +58,8 @@ namespace cv { +class CV_EXPORTS _OutputArray; + //! @addtogroup core_basic //! @{ @@ -215,6 +217,10 @@ public: template Matx(const Matx<_Tp, m, l>& a, const Matx<_Tp, l, n>& b, Matx_MatMulOp); Matx(const Matx<_Tp, n, m>& a, Matx_TOp); + //! copy & convert + void copyTo(const _OutputArray& dst) const; + void convertTo(const _OutputArray& dst, int type, double scale=1., double shift=0.) const; + _Tp val[m*n]; //< matrix elements }; @@ -970,8 +976,6 @@ Matx<_Tp, m, n> MatxCommaInitializer<_Tp, m, n>::operator *() const return *dst; } - - /////////////////////////////////// Vec Implementation /////////////////////////////////// template inline diff --git a/modules/core/include/opencv2/core/operations.hpp b/modules/core/include/opencv2/core/operations.hpp index bde28c49b2..b2ddf77daf 100644 --- a/modules/core/include/opencv2/core/operations.hpp +++ b/modules/core/include/opencv2/core/operations.hpp @@ -247,7 +247,17 @@ Matx<_Tp, n, l> Matx<_Tp, m, n>::solve(const Matx<_Tp, m, l>& rhs, int method) c return ok ? x : Matx<_Tp, n, l>::zeros(); } +template inline +void Matx<_Tp, m, n>::copyTo(OutputArray dst) const +{ + Mat(*this, false).copyTo(dst); +} +template inline +void Matx<_Tp, m, n>::convertTo(OutputArray dst, int type, double scale, double shift) const +{ + Mat(*this, false).convertTo(dst, type, scale, shift); +} ////////////////////////// Augmenting algebraic & logical operations ////////////////////////// diff --git a/modules/core/include/opencv2/core/types.hpp b/modules/core/include/opencv2/core/types.hpp index 819fd52817..08dd70d3ef 100644 --- a/modules/core/include/opencv2/core/types.hpp +++ b/modules/core/include/opencv2/core/types.hpp @@ -870,6 +870,7 @@ public: @param epsilon The desired accuracy or change in parameters at which the iterative algorithm stops. */ TermCriteria(int type, int maxCount, double epsilon); + TermCriteria(int maxCount, double epsilon); inline bool isValid() const { @@ -2469,6 +2470,26 @@ inline TermCriteria::TermCriteria(int _type, int _maxCount, double _epsilon) : type(_type), maxCount(_maxCount), epsilon(_epsilon) {} +inline TermCriteria::TermCriteria(int _maxCount, double _epsilon) +{ + type = 0; + if (_maxCount > 0) + { + maxCount = _maxCount; + type = COUNT; + } + else + maxCount = INT_MAX-1; + + if (_epsilon > 0) + { + epsilon = _epsilon; + type |= EPS; + } + else + epsilon = DBL_EPSILON; +} + //! @endcond } // cv diff --git a/modules/core/src/copy.cpp b/modules/core/src/copy.cpp index a6f06a5c7d..3a6a1a7ac6 100644 --- a/modules/core/src/copy.cpp +++ b/modules/core/src/copy.cpp @@ -661,6 +661,25 @@ Mat& Mat::setTo(InputArray _value, InputArray _mask) } +Mat& Mat::setZero() +{ + CV_INSTRUMENT_REGION(); + + if( empty() ) + return *this; + + size_t esz = elemSize(); + + const Mat* arrays[] = { this, 0 }; + uchar* ptrs[]={0}; + NAryMatIterator it(arrays, ptrs); + + for( size_t i = 0; i < it.nplanes; i++, ++it ) + memset(ptrs[0], 0, esz*it.size); + return *this; +} + + #if defined HAVE_OPENCL && !defined __APPLE__ static bool ocl_repeat(InputArray _src, int ny, int nx, OutputArray _dst) diff --git a/modules/core/src/matrix_wrap.cpp b/modules/core/src/matrix_wrap.cpp index bb61ce2de1..869729ca35 100644 --- a/modules/core/src/matrix_wrap.cpp +++ b/modules/core/src/matrix_wrap.cpp @@ -1863,6 +1863,22 @@ void _OutputArray::setTo(const _InputArray& arr, const _InputArray & mask) const CV_Error(Error::StsNotImplemented, ""); } +void _OutputArray::setZero() const +{ + _InputArray::KindFlag k = kind(); + + if( k == NONE ) + ; + else if (k == MAT || k == MATX || k == STD_VECTOR) + { + Mat m = getMat(); + m.setZero(); + } + else + { + setTo(Scalar::all(0), noArray()); + } +} void _OutputArray::assign(const UMat& u) const { diff --git a/modules/features2d/test/test_keypoints.cpp b/modules/features2d/test/test_keypoints.cpp index c169c97163..696936af8e 100644 --- a/modules/features2d/test/test_keypoints.cpp +++ b/modules/features2d/test/test_keypoints.cpp @@ -40,7 +40,6 @@ //M*/ #include "test_precomp.hpp" -#include "opencv2/core/core_c.h" namespace opencv_test { namespace { diff --git a/modules/stitching/src/motion_estimators.cpp b/modules/stitching/src/motion_estimators.cpp index 69fdc8473a..b4ae36a3d0 100644 --- a/modules/stitching/src/motion_estimators.cpp +++ b/modules/stitching/src/motion_estimators.cpp @@ -41,7 +41,6 @@ //M*/ #include "precomp.hpp" -#include "opencv2/core/core_c.h" #include "opencv2/core/cvdef.h" using namespace cv; @@ -253,44 +252,17 @@ bool BundleAdjusterBase::estimate(const std::vector &features, total_num_matches_ += static_cast(pairwise_matches[edges_[i].first * num_images_ + edges_[i].second].num_inliers); - CvLevMarq solver(num_images_ * num_params_per_cam_, - total_num_matches_ * num_errs_per_measurement_, - cvTermCriteria(term_criteria_)); - - Mat err, jac; - CvMat matParams = cvMat(cam_params_); - cvCopy(&matParams, solver.param); - - int iter = 0; - for(;;) - { - const CvMat* _param = 0; - CvMat* _jac = 0; - CvMat* _err = 0; - - bool proceed = solver.update(_param, _jac, _err); - - cvCopy(_param, &matParams); - - if (!proceed || !_err) - break; - - if (_jac) + int nerrs = total_num_matches_ * num_errs_per_measurement_; + LMSolver::run(cam_params_, Mat(), nerrs, term_criteria_, DECOMP_SVD, + [&](Mat& param, Mat* err, Mat* jac) { - calcJacobian(jac); - CvMat tmp = cvMat(jac); - cvCopy(&tmp, _jac); - } - - if (_err) - { - calcError(err); - LOG_CHAT("."); - iter++; - CvMat tmp = cvMat(err); - cvCopy(&tmp, _err); - } - } + param.copyTo(cam_params_); + if (jac) + calcJacobian(*jac); + if (err) + calcError(*err); + return true; + }); LOGLN_CHAT(""); LOGLN_CHAT("Bundle adjustment, final RMS error: " << std::sqrt(err.dot(err) / total_num_matches_));