From 9d89c369f8862ecee506f5593cf50dd89f0a9768 Mon Sep 17 00:00:00 2001 From: Pavel Rojtberg Date: Thu, 10 Dec 2015 23:01:00 +0100 Subject: [PATCH 1/4] update SOLVEPNP documentation --- modules/calib3d/doc/calib3d.bib | 41 +++++++++++++++++++++ modules/calib3d/include/opencv2/calib3d.hpp | 8 ++-- 2 files changed, 45 insertions(+), 4 deletions(-) create mode 100644 modules/calib3d/doc/calib3d.bib 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 fd50afab0..2f36afd2f 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 }; From 22b64e2c28ad254f1093aad408ee0605fbbed36e Mon Sep 17 00:00:00 2001 From: Pavel Rojtberg Date: Sat, 21 Nov 2015 01:30:53 +0100 Subject: [PATCH 2/4] CvLevMarq: remove fixed rows instead of setting them to zero use the same approach like in fisheye calibration: instead of setting masked out rows to zero, remove them from the equation system. This way JtJ does not become singular and we can use the much faster LU decomposition instead of SVD. This results in a speedup of the Calibrate unit tests of 3x-10x. --- modules/calib3d/src/compat_ptsetreg.cpp | 66 ++++++++++++++++++------- 1 file changed, 47 insertions(+), 19 deletions(-) diff --git a/modules/calib3d/src/compat_ptsetreg.cpp b/modules/calib3d/src/compat_ptsetreg.cpp index d2ab252d3..6bc407ee6 100644 --- a/modules/calib3d/src/compat_ptsetreg.cpp +++ b/modules/calib3d/src/compat_ptsetreg.cpp @@ -93,9 +93,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 ) { @@ -256,6 +253,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 +289,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); + 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); } From 4c251bd2604d73ea71fd276c4058f9c6953c0a92 Mon Sep 17 00:00:00 2001 From: Pavel Rojtberg Date: Thu, 10 Dec 2015 23:01:55 +0100 Subject: [PATCH 3/4] CvLevMarq: add solve method to interface --- modules/calib3d/include/opencv2/calib3d/calib3d_c.h | 1 + modules/calib3d/src/compat_ptsetreg.cpp | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) 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/compat_ptsetreg.cpp b/modules/calib3d/src/compat_ptsetreg.cpp index 6bc407ee6..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 ) @@ -113,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 ) @@ -314,7 +316,7 @@ void CvLevMarq::step() #else _JtJN.diag() += lambda; #endif - solve(_JtJN, _JtErr, nonzero_param); + solve(_JtJN, _JtErr, nonzero_param, solveMethod); int j = 0; for( int i = 0; i < nparams; i++ ) From b5ddaae427a1dbcabc72a99c6345eb87fbcd1658 Mon Sep 17 00:00:00 2001 From: Pavel Rojtberg Date: Thu, 10 Dec 2015 23:26:37 +0100 Subject: [PATCH 4/4] calib3d: add CALIB_USE_LU to use LU decomposition in solver --- modules/calib3d/include/opencv2/calib3d.hpp | 3 ++- modules/calib3d/src/calibration.cpp | 15 +++++++++++---- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 2f36afd2f..a472bab11 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -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/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;