12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add new interface to levenbergMarquardt, now can take a threshold for sum of error

release/4.3a0
Yong-Dian Jian 2010-10-01 14:47:28 +00:00
parent 9a805d68bf
commit a48bf9499a
2 changed files with 84 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
nonlinear/NonlinearOptimizer-inl.h
View File

@ -21,9 +21,21 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
inline bool check_convergence(double relativeErrorTreshold,
double absoluteErrorTreshold, double currentError, double newError,
int verbosity) {
inline bool check_convergence(
double relativeErrorTreshold,
double absoluteErrorTreshold,
double errorThreshold,
double currentError, double newError, int verbosity) {
if ( verbosity >= 2 ) {
if ( newError <= errorThreshold )
cout << "errorThreshold: " << newError << " < " << errorThreshold << endl;
else
cout << "errorThreshold: " << newError << " > " << errorThreshold << endl;
}
if ( newError <= errorThreshold ) return true ;
// check if diverges
double absoluteDecrease = currentError - newError;
if (verbosity >= 2) {
@ -120,8 +132,13 @@ namespace gtsam {
writer.write(next.error_);
// check convergence
bool converged = gtsam::check_convergence(relativeThreshold,
absoluteThreshold, error_, next.error_, verbosity);
bool converged = gtsam::check_convergence(
relativeThreshold,
absoluteThreshold,
0.0,
error_,
next.error_,
verbosity);
// return converged state or iterate
if (converged)
@ -236,37 +253,53 @@ namespace gtsam {
double relativeThreshold, double absoluteThreshold,
verbosityLevel verbosity, int maxIterations, double lambdaFactor, LambdaMode lambdaMode) const {
if (maxIterations <= 0) return *this;
return levenbergMarquardt(NonLinearOptimizerPara (absoluteThreshold, relativeThreshold, absoluteThreshold,
maxIterations, lambdaFactor, verbosity, lambdaMode)) ;
}
template<class G, class C, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::
levenbergMarquardt(const NonLinearOptimizerPara &para) const {
if (para.maxIterations_ <= 0) return *this;
// check if we're already close enough
if (error_ < absoluteThreshold) {
if (verbosity >= ERROR) cout << "Exiting, as error = " << error_
<< " < absoluteThreshold (" << absoluteThreshold << ")" << endl;
if (error_ < para.sumError_) {
if (para.verbosity_ >= ERROR)
cout << "Exiting, as error = " << error_ << " < " << para.sumError_ << endl;
return *this;
}
// do one iteration of LM
NonlinearOptimizer next = iterateLM(verbosity, lambdaFactor, lambdaMode);
NonlinearOptimizer next = iterateLM(para.verbosity_, para.lambdaFactor_, para.lambdaMode_);
// check convergence
// TODO: move convergence checks here and incorporate in verbosity levels
// TODO: build into iterations somehow as an instance variable
bool converged = gtsam::check_convergence(relativeThreshold,
absoluteThreshold, error_, next.error_, verbosity);
bool converged = gtsam::check_convergence(
para.relDecrease_,
para.absDecrease_,
para.sumError_,
error_,
next.error_,
para.verbosity_);
// return converged state or iterate
if (converged || maxIterations <= 1) {
if (converged || para.maxIterations_ <= 1) {
// maybe show output
if (verbosity >= CONFIG)
if (para.verbosity_ >= CONFIG)
next.config_->print("final config");
if (verbosity >= ERROR)
if (para.verbosity_ >= ERROR)
cout << "final error: " << next.error_ << endl;
if (verbosity >= LAMBDA)
if (para.verbosity_ >= LAMBDA)
cout << "final lambda = " << next.lambda_ << endl;
return next;
} else
return next.levenbergMarquardt(relativeThreshold, absoluteThreshold,
verbosity, maxIterations-1, lambdaFactor, lambdaMode);
} else {
NonLinearOptimizerPara newPara = para ;
newPara.maxIterations_ = newPara.maxIterations_ - 1;
return next.levenbergMarquardt(newPara) ;
}
}
/* ************************************************************************* */

35
nonlinear/NonlinearOptimizer.h
View File

@ -68,6 +68,30 @@ namespace gtsam {
CAUTIOUS
} LambdaMode;
// a container for all related parameters
struct NonLinearOptimizerPara {
public:
double absDecrease_; /* threshold for the absolute decrease per iteration */
double relDecrease_; /* threshold for the relative decrease per iteration */
double sumError_; /* threshold for the sum of error */
int maxIterations_ ;
double lambdaFactor_ ;
verbosityLevel verbosity_;
LambdaMode lambdaMode_;
public:
NonLinearOptimizerPara(): absDecrease_(1), relDecrease_(1e-3), sumError_(0.0),
maxIterations_(100), lambdaFactor_(10.0), verbosity_(ERROR), lambdaMode_(BOUNDED){}
NonLinearOptimizerPara(double absDecrease, double relDecrease, double sumError,
int iIters = 100, double lambdaFactor = 10, verbosityLevel v = ERROR, LambdaMode lambdaMode = BOUNDED)
:absDecrease_(absDecrease), relDecrease_(relDecrease), sumError_(sumError),
maxIterations_(iIters), lambdaFactor_(lambdaFactor), verbosity_(v), lambdaMode_(lambdaMode){}
};
private:
// keep a reference to const version of the graph
@ -180,6 +204,9 @@ namespace gtsam {
double lambdaFactor = 10, LambdaMode lambdaMode = BOUNDED) const;
NonlinearOptimizer
levenbergMarquardt(const NonLinearOptimizerPara &para) const;
/**
* Static interface to LM optimization using default ordering and thresholds
* @param graph Nonlinear factor graph to optimize
@ -247,10 +274,12 @@ namespace gtsam {
/**
* Check convergence
*/
bool check_convergence (double relativeErrorTreshold,
bool check_convergence (
double relativeErrorTreshold,
double absoluteErrorTreshold,
double currentError, double newError,
int verbosity);
double errorThreshold,
double currentError, double newError, int verbosity);
} // gtsam