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
parent 286ba8cffd b5ddaae427
commit 21b415f6be
5 changed files with 108 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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);
}