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

rewritten several functions from calib3d: findhomography, findfundamentalmat, findessentialmat, estimateaffine3d, computecorrespondepilines, convert points{to/from}homogeneous to C++.

Browse Source
This commit is contained in:
Vadim Pisarevsky
2013-02-28 18:25:05 +04:00
parent 891d7da6ee
commit 374e3a0890
15 changed files with 2623 additions and 2413 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -55,247 +55,6 @@
using namespace cv;
CvLevMarq::CvLevMarq()
{
mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = Ptr<CvMat>();
lambdaLg10 = 0; state = DONE;
criteria = cvTermCriteria(0,0,0);
iters = 0;
completeSymmFlag = false;
}
CvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
{
mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = Ptr<CvMat>();
init(nparams, nerrs, criteria0, _completeSymmFlag);
}
void CvLevMarq::clear()
{
mask.release();
prevParam.release();
param.release();
J.release();
err.release();
JtJ.release();
JtJN.release();
JtErr.release();
JtJV.release();
JtJW.release();
}
CvLevMarq::~CvLevMarq()
{
clear();
}
void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
{
if( !param || param->rows != nparams || nerrs != (err ? err->rows : 0) )
clear();
mask = cvCreateMat( nparams, 1, CV_8U );
cvSet(mask, cvScalarAll(1));
prevParam = cvCreateMat( nparams, 1, CV_64F );
param = cvCreateMat( nparams, 1, CV_64F );
JtJ = cvCreateMat( nparams, nparams, CV_64F );
JtJN = cvCreateMat( nparams, nparams, CV_64F );
JtJV = cvCreateMat( nparams, nparams, CV_64F );
JtJW = cvCreateMat( nparams, 1, CV_64F );
JtErr = cvCreateMat( nparams, 1, CV_64F );
if( nerrs > 0 )
{
J = cvCreateMat( nerrs, nparams, CV_64F );
err = cvCreateMat( nerrs, 1, CV_64F );
}
prevErrNorm = DBL_MAX;
lambdaLg10 = -3;
criteria = criteria0;
if( criteria.type & CV_TERMCRIT_ITER )
criteria.max_iter = MIN(MAX(criteria.max_iter,1),1000);
else
criteria.max_iter = 30;
if( criteria.type & CV_TERMCRIT_EPS )
criteria.epsilon = MAX(criteria.epsilon, 0);
else
criteria.epsilon = DBL_EPSILON;
state = STARTED;
iters = 0;
completeSymmFlag = _completeSymmFlag;
}
bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err )
{
double change;
matJ = _err = 0;
assert( !err.empty() );
if( state == DONE )
{
_param = param;
return false;
}
if( state == STARTED )
{
_param = param;
cvZero( J );
cvZero( err );
matJ = J;
_err = err;
state = CALC_J;
return true;
}
if( state == CALC_J )
{
cvMulTransposed( J, JtJ, 1 );
cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T );
cvCopy( param, prevParam );
step();
if( iters == 0 )
prevErrNorm = cvNorm(err, 0, CV_L2);
_param = param;
cvZero( err );
_err = err;
state = CHECK_ERR;
return true;
}
assert( state == CHECK_ERR );
errNorm = cvNorm( err, 0, CV_L2 );
if( errNorm > prevErrNorm )
{
if( ++lambdaLg10 <= 16 )
{
step();
_param = param;
cvZero( err );
_err = err;
state = CHECK_ERR;
return true;
}
}
lambdaLg10 = MAX(lambdaLg10-1, -16);
if( ++iters >= criteria.max_iter ||
(change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
{
_param = param;
state = DONE;
return true;
}
prevErrNorm = errNorm;
_param = param;
cvZero(J);
matJ = J;
_err = err;
state = CALC_J;
return true;
}
bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm )
{
double change;
CV_Assert( err.empty() );
if( state == DONE )
{
_param = param;
return false;
}
if( state == STARTED )
{
_param = param;
cvZero( JtJ );
cvZero( JtErr );
errNorm = 0;
_JtJ = JtJ;
_JtErr = JtErr;
_errNorm = &errNorm;
state = CALC_J;
return true;
}
if( state == CALC_J )
{
cvCopy( param, prevParam );
step();
_param = param;
prevErrNorm = errNorm;
errNorm = 0;
_errNorm = &errNorm;
state = CHECK_ERR;
return true;
}
assert( state == CHECK_ERR );
if( errNorm > prevErrNorm )
{
if( ++lambdaLg10 <= 16 )
{
step();
_param = param;
errNorm = 0;
_errNorm = &errNorm;
state = CHECK_ERR;
return true;
}
}
lambdaLg10 = MAX(lambdaLg10-1, -16);
if( ++iters >= criteria.max_iter ||
(change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
{
_param = param;
state = DONE;
return false;
}
prevErrNorm = errNorm;
cvZero( JtJ );
cvZero( JtErr );
_param = param;
_JtJ = JtJ;
_JtErr = JtErr;
state = CALC_J;
return true;
}
void CvLevMarq::step()
{
const double LOG10 = log(10.);
double lambda = exp(lambdaLg10*LOG10);
int i, j, nparams = param->rows;
for( i = 0; i < nparams; i++ )
if( mask->data.ptr[i] == 0 )
{
double *row = JtJ->data.db + i*nparams, *col = JtJ->data.db + i;
for( j = 0; j < nparams; j++ )
row[j] = col[j*nparams] = 0;
JtErr->data.db[i] = 0;
}
if( !err )
cvCompleteSymm( JtJ, completeSymmFlag );
#if 1
cvCopy( JtJ, JtJN );
for( i = 0; i < nparams; i++ )
JtJN->data.db[(nparams+1)*i] *= 1. + lambda;
#else
cvSetIdentity(JtJN, cvRealScalar(lambda));
cvAdd( JtJ, JtJN, JtJN );
#endif
cvSVD( JtJN, JtJW, 0, JtJV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
cvSVBkSb( JtJW, JtJV, JtJV, JtErr, param, CV_SVD_U_T + CV_SVD_V_T );
for( i = 0; i < nparams; i++ )
param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? param->data.db[i] : 0);
}
// reimplementation of dAB.m
CV_IMPL void cvCalcMatMulDeriv( const CvMat* A, const CvMat* B, CvMat* dABdA, CvMat* dABdB )
{