mirror of https://github.com/opencv/opencv.git
* extended C++ version of Levenberg-Marquardt (LM) solver to accommodate all features of the C counterpart. * removed C version of LM solver * made a few other little changes to make the code compile and run smoothlypull/20225/head
Vadim Pisarevsky
4 years ago
parent
c105402dfc
commit
eff6d32337
20 changed files with 1562 additions and 3587 deletions
File diff suppressed because it is too large
Load Diff
@ -1,322 +0,0 @@ |
||||
/*M///////////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
|
||||
//
|
||||
// By downloading, copying, installing or using the software you agree to this license.
|
||||
// If you do not agree to this license, do not download, install,
|
||||
// copy or use the software.
|
||||
//
|
||||
//
|
||||
// License Agreement
|
||||
// For Open Source Computer Vision Library
|
||||
//
|
||||
// Copyright (C) 2000, Intel Corporation, all rights reserved.
|
||||
// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
|
||||
// Third party copyrights are property of their respective owners.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without modification,
|
||||
// are permitted provided that the following conditions are met:
|
||||
//
|
||||
// * Redistribution's of source code must retain the above copyright notice,
|
||||
// this list of conditions and the following disclaimer.
|
||||
//
|
||||
// * Redistribution's in binary form must reproduce the above copyright notice,
|
||||
// this list of conditions and the following disclaimer in the documentation
|
||||
// and/or other materials provided with the distribution.
|
||||
//
|
||||
// * The name of the copyright holders may not be used to endorse or promote products
|
||||
// derived from this software without specific prior written permission.
|
||||
//
|
||||
// This software is provided by the copyright holders and contributors "as is" and
|
||||
// any express or implied warranties, including, but not limited to, the implied
|
||||
// warranties of merchantability and fitness for a particular purpose are disclaimed.
|
||||
// In no event shall the Intel Corporation or contributors be liable for any direct,
|
||||
// indirect, incidental, special, exemplary, or consequential damages
|
||||
// (including, but not limited to, procurement of substitute goods or services;
|
||||
// loss of use, data, or profits; or business interruption) however caused
|
||||
// and on any theory of liability, whether in contract, strict liability,
|
||||
// or tort (including negligence or otherwise) arising in any way out of
|
||||
// the use of this software, even if advised of the possibility of such damage.
|
||||
//
|
||||
//M*/
|
||||
|
||||
#include "precomp.hpp" |
||||
#include "opencv2/core/core_c.h" |
||||
|
||||
/************************************************************************************\
|
||||
Some backward compatibility stuff, to be moved to legacy or compat module |
||||
\************************************************************************************/ |
||||
|
||||
namespace cv { |
||||
|
||||
////////////////// Levenberg-Marquardt engine (the old variant) ////////////////////////
|
||||
|
||||
CvLevMarq::CvLevMarq() |
||||
{ |
||||
lambdaLg10 = 0; state = DONE; |
||||
criteria = cvTermCriteria(0,0,0); |
||||
iters = 0; |
||||
completeSymmFlag = false; |
||||
errNorm = prevErrNorm = DBL_MAX; |
||||
solveMethod = cv::DECOMP_SVD; |
||||
} |
||||
|
||||
CvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag ) |
||||
{ |
||||
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.reset(cvCreateMat( nparams, 1, CV_8U )); |
||||
cvSet(mask, cvScalarAll(1)); |
||||
prevParam.reset(cvCreateMat( nparams, 1, CV_64F )); |
||||
param.reset(cvCreateMat( nparams, 1, CV_64F )); |
||||
JtJ.reset(cvCreateMat( nparams, nparams, CV_64F )); |
||||
JtErr.reset(cvCreateMat( nparams, 1, CV_64F )); |
||||
if( nerrs > 0 ) |
||||
{ |
||||
J.reset(cvCreateMat( nerrs, nparams, CV_64F )); |
||||
err.reset(cvCreateMat( nerrs, 1, CV_64F )); |
||||
} |
||||
errNorm = 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; |
||||
solveMethod = cv::DECOMP_SVD; |
||||
} |
||||
|
||||
bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err ) |
||||
{ |
||||
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 || |
||||
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 ) |
||||
{ |
||||
CV_Assert( !err ); |
||||
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 || |
||||
cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon ) |
||||
{ |
||||
_param = param; |
||||
_JtJ = JtJ; |
||||
_JtErr = JtErr; |
||||
state = DONE; |
||||
return false; |
||||
} |
||||
|
||||
prevErrNorm = errNorm; |
||||
cvZero( JtJ ); |
||||
cvZero( JtErr ); |
||||
_param = param; |
||||
_JtJ = JtJ; |
||||
_JtErr = JtErr; |
||||
state = CALC_J; |
||||
return true; |
||||
} |
||||
|
||||
static void subMatrix(const Mat& src, Mat& dst, |
||||
const std::vector<uchar>& cols, |
||||
const std::vector<uchar>& rows) |
||||
{ |
||||
int nonzeros_cols = countNonZero(cols); |
||||
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; |
||||
const double LOG10 = log(10.); |
||||
double lambda = exp(lambdaLg10*LOG10); |
||||
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 _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( _JtJN, completeSymmFlag ); |
||||
|
||||
_JtJN.diag() *= 1. + lambda; |
||||
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] ? nonzero_param(j++) : 0); |
||||
} |
||||
|
||||
} |
||||
File diff suppressed because it is too large
Load Diff
Loading…
Reference in new issue