diff --git a/modules/calib3d/doc/calib3d.bib b/modules/calib3d/doc/calib3d.bib new file mode 100644 index 000000000..57989b34f --- /dev/null +++ b/modules/calib3d/doc/calib3d.bib @@ -0,0 +1,41 @@ +@article{lepetit2009epnp, + title={Epnp: An accurate o (n) solution to the pnp problem}, + author={Lepetit, Vincent and Moreno-Noguer, Francesc and Fua, Pascal}, + journal={International journal of computer vision}, + volume={81}, + number={2}, + pages={155--166}, + year={2009}, + publisher={Springer} +} + +@article{gao2003complete, + title={Complete solution classification for the perspective-three-point problem}, + author={Gao, Xiao-Shan and Hou, Xiao-Rong and Tang, Jianliang and Cheng, Hang-Fei}, + journal={Pattern Analysis and Machine Intelligence, IEEE Transactions on}, + volume={25}, + number={8}, + pages={930--943}, + year={2003}, + publisher={IEEE} +} + +@inproceedings{hesch2011direct, + title={A direct least-squares (DLS) method for PnP}, + author={Hesch, Joel and Roumeliotis, Stergios and others}, + booktitle={Computer Vision (ICCV), 2011 IEEE International Conference on}, + pages={383--390}, + year={2011}, + organization={IEEE} +} + +@article{penate2013exhaustive, + title={Exhaustive linearization for robust camera pose and focal length estimation}, + author={Penate-Sanchez, Adrian and Andrade-Cetto, Juan and Moreno-Noguer, Francesc}, + journal={Pattern Analysis and Machine Intelligence, IEEE Transactions on}, + volume={35}, + number={10}, + pages={2387--2400}, + year={2013}, + publisher={IEEE} +} diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index f33b9ee14..e75e22ef9 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -189,10 +189,10 @@ enum { LMEDS = 4, //!< least-median algorithm }; enum { SOLVEPNP_ITERATIVE = 0, - SOLVEPNP_EPNP = 1, // F.Moreno-Noguer, V.Lepetit and P.Fua "EPnP: Efficient Perspective-n-Point Camera Pose Estimation" - SOLVEPNP_P3P = 2, // X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem" - SOLVEPNP_DLS = 3, // Joel A. Hesch and Stergios I. Roumeliotis. "A Direct Least-Squares (DLS) Method for PnP" - SOLVEPNP_UPNP = 4 // A.Penate-Sanchez, J.Andrade-Cetto, F.Moreno-Noguer. "Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation" + SOLVEPNP_EPNP = 1, //!< EPnP: Efficient Perspective-n-Point Camera Pose Estimation @cite lepetit2009epnp + SOLVEPNP_P3P = 2, //!< Complete Solution Classification for the Perspective-Three-Point Problem @cite gao2003complete + SOLVEPNP_DLS = 3, //!< A Direct Least-Squares (DLS) Method for PnP @cite hesch2011direct + SOLVEPNP_UPNP = 4 //!< Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation @cite penate2013exhaustive }; @@ -225,7 +225,8 @@ enum { CALIB_USE_INTRINSIC_GUESS = 0x00001, CALIB_FIX_INTRINSIC = 0x00100, CALIB_SAME_FOCAL_LENGTH = 0x00200, // for stereo rectification - CALIB_ZERO_DISPARITY = 0x00400 + CALIB_ZERO_DISPARITY = 0x00400, + CALIB_USE_LU = (1 << 17), //!< use LU instead of SVD decomposition for solving. much faster but potentially less precise }; //! the algorithm for finding fundamental matrix diff --git a/modules/calib3d/include/opencv2/calib3d/calib3d_c.h b/modules/calib3d/include/opencv2/calib3d/calib3d_c.h index 239269238..90942dfee 100644 --- a/modules/calib3d/include/opencv2/calib3d/calib3d_c.h +++ b/modules/calib3d/include/opencv2/calib3d/calib3d_c.h @@ -415,6 +415,7 @@ public: int state; int iters; bool completeSymmFlag; + int solveMethod; }; #endif diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index fc0a267be..5d570ba97 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -1232,7 +1232,6 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, CvMat* rvecs, CvMat* tvecs, int flags, CvTermCriteria termCrit ) { const int NINTRINSIC = 16; - CvLevMarq solver; double reprojErr = 0; Matx33d A; @@ -1388,7 +1387,11 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, cvInitIntrinsicParams2D( &_matM, &m, npoints, imageSize, &matA, aspectRatio ); } - solver.init( nparams, 0, termCrit ); + CvLevMarq solver( nparams, 0, termCrit ); + + if(flags & CALIB_USE_LU) { + solver.solveMethod = DECOMP_LU; + } { double* param = solver.param->data.db; @@ -1635,7 +1638,6 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1 { const int NINTRINSIC = 16; Ptr npoints, err, J_LR, Je, Ji, imagePoints[2], objectPoints, RT0; - CvLevMarq solver; double reprojErr = 0; double A[2][9], dk[2][12]={{0,0,0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0,0,0}}, rlr[9]; @@ -1737,7 +1739,12 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1 // storage for initial [om(R){i}|t{i}] (in order to compute the median for each component) RT0.reset(cvCreateMat( 6, nimages, CV_64F )); - solver.init( nparams, 0, termCrit ); + CvLevMarq solver( nparams, 0, termCrit ); + + if(flags & CALIB_USE_LU) { + solver.solveMethod = DECOMP_LU; + } + if( recomputeIntrinsics ) { uchar* imask = solver.mask->data.ptr + nparams - NINTRINSIC*2; diff --git a/modules/calib3d/src/compat_ptsetreg.cpp b/modules/calib3d/src/compat_ptsetreg.cpp index d2ab252d3..e0f387da3 100644 --- a/modules/calib3d/src/compat_ptsetreg.cpp +++ b/modules/calib3d/src/compat_ptsetreg.cpp @@ -58,6 +58,7 @@ CvLevMarq::CvLevMarq() iters = 0; completeSymmFlag = false; errNorm = prevErrNorm = DBL_MAX; + solveMethod = cv::DECOMP_SVD; } CvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag ) @@ -93,9 +94,6 @@ void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _co prevParam.reset(cvCreateMat( nparams, 1, CV_64F )); param.reset(cvCreateMat( nparams, 1, CV_64F )); JtJ.reset(cvCreateMat( nparams, nparams, CV_64F )); - JtJN.reset(cvCreateMat( nparams, nparams, CV_64F )); - JtJV.reset(cvCreateMat( nparams, nparams, CV_64F )); - JtJW.reset(cvCreateMat( nparams, 1, CV_64F )); JtErr.reset(cvCreateMat( nparams, 1, CV_64F )); if( nerrs > 0 ) { @@ -116,6 +114,7 @@ void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _co state = STARTED; iters = 0; completeSymmFlag = _completeSymmFlag; + solveMethod = cv::DECOMP_SVD; } bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err ) @@ -256,6 +255,34 @@ bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, d return true; } +namespace { +static void subMatrix(const cv::Mat& src, cv::Mat& dst, const std::vector& cols, + const std::vector& rows) { + int nonzeros_cols = cv::countNonZero(cols); + cv::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; @@ -264,33 +291,36 @@ void CvLevMarq::step() int nparams = param->rows; Mat _JtJ = cvarrToMat(JtJ); - Mat _JtJN = cvarrToMat(JtJN); - Mat _JtJW = cvarrToMat(JtJW); - Mat _JtJV = cvarrToMat(JtJV); + Mat _mask = cvarrToMat(mask); - for( int i = 0; i < nparams; i++ ) - if( mask->data.ptr[i] == 0 ) - { - _JtJ.row(i) = 0; - _JtJ.col(i) = 0; - JtErr->data.db[i] = 0; - } + 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( _JtJ, completeSymmFlag ); + completeSymm( _JtJN, completeSymmFlag ); - _JtJ.copyTo(_JtJN); #if 1 _JtJN.diag() *= 1. + lambda; #else _JtJN.diag() += lambda; #endif - // solve(JtJN, JtErr, param, DECOMP_SVD); - SVD::compute(_JtJN, _JtJW, noArray(), _JtJV, SVD::MODIFY_A); - SVD::backSubst(_JtJW, _JtJV.t(), _JtJV, cvarrToMat(JtErr), cvarrToMat(param)); + 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] ? param->data.db[i] : 0); + param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? nonzero_param(j++) : 0); }