generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #5691 from paroj:levmarqLU

Browse Source
This commit is contained in:
Vadim Pisarevsky 2015-12-11 08:39:05 +00:00
commit 21b415f6be
5 changed files with 108 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
modules/calib3d/doc/calib3d.bib Normal file
View File

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

11
modules/calib3d/include/opencv2/calib3d.hpp
View File

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

1
modules/calib3d/include/opencv2/calib3d/calib3d_c.h
View File

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

15
modules/calib3d/src/calibration.cpp
View File

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

68
modules/calib3d/src/compat_ptsetreg.cpp
View File

@ -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 _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_<double> nonzero_param = cvarrToMat(JtJW);
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);
}