Merge pull request #5691 from paroj:levmarqLU

modules/calib3d/doc/calib3d.bib

+@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}
+}

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

modules/calib3d/include/opencv2/calib3d/calib3d_c.h

@@ -415,6 +415,7 @@ public:
     int state;
     int iters;
     bool completeSymmFlag;
+    int solveMethod;
 };
 
 #endif

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<CvMat> 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;

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<uchar>& cols,
+                      const std::vector<uchar>& 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 _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 _JtJW = cvarrToMat(JtJW);
-    Mat _JtJV = cvarrToMat(JtJV);
+    Mat _JtErr = cvarrToMat(JtJV);
+    Mat_<double> nonzero_param = cvarrToMat(JtJW);
 
-    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;
-        }
+    subMatrix(cvarrToMat(JtErr), _JtErr, std::vector<uchar>(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);
 }