Implementation of the Levenberg Algorithm. More...

#include <optimization_algorithm_levenberg.h>

Inheritance diagram for g2o::OptimizationAlgorithmLevenberg:

Collaboration diagram for g2o::OptimizationAlgorithmLevenberg:

[legend]

Public Member Functions
	OptimizationAlgorithmLevenberg (Solver *solver)

virtual	~OptimizationAlgorithmLevenberg ()

virtual SolverResult	solve (int iteration, bool online=false)

virtual void	printVerbose (std::ostream &os) const

double	currentLambda () const
	return the currently used damping factor More...

void	setMaxTrialsAfterFailure (int max_trials)
	the number of internal iteration if an update step increases chi^2 within Levenberg-Marquardt More...

int	maxTrialsAfterFailure () const
	get the number of inner iterations for Levenberg-Marquardt More...

double	userLambdaInit ()
	return the lambda set by the user, if < 0 the SparseOptimizer will compute the initial lambda More...

void	setUserLambdaInit (double lambda)
	specify the initial lambda used for the first iteraion, if not given the SparseOptimizer tries to compute a suitable value More...

int	levenbergIteration ()
	return the number of levenberg iterations performed in the last round More...

Public Member Functions inherited from g2o::OptimizationAlgorithmWithHessian
	OptimizationAlgorithmWithHessian (Solver *solver)

virtual	~OptimizationAlgorithmWithHessian ()

virtual bool	init (bool online=false)

virtual bool	computeMarginals (SparseBlockMatrix< MatrixXD > &spinv, const std::vector< std::pair< int, int > > &blockIndices)

virtual bool	buildLinearStructure ()

virtual void	updateLinearSystem ()

virtual bool	updateStructure (const std::vector< HyperGraph::Vertex * > &vset, const HyperGraph::EdgeSet &edges)

Solver *	solver ()
	return the underlying solver used to solve the linear system More...

virtual void	setWriteDebug (bool writeDebug)

virtual bool	writeDebug () const

Public Member Functions inherited from g2o::OptimizationAlgorithm
	OptimizationAlgorithm ()

virtual	~OptimizationAlgorithm ()

const SparseOptimizer *	optimizer () const
	return the optimizer operating on More...

SparseOptimizer *	optimizer ()

void	setOptimizer (SparseOptimizer *optimizer)

const PropertyMap &	properties () const
	return the properties of the solver More...

bool	updatePropertiesFromString (const std::string &propString)

void	printProperties (std::ostream &os) const

Protected Member Functions
double	computeLambdaInit () const

double	computeScale () const

Protected Attributes
Property< int > *	_maxTrialsAfterFailure

Property< double > *	_userLambdaInit

double	_currentLambda

double	_tau

double	_goodStepLowerScale
	lower bound for lambda decrease if a good LM step More...

double	_goodStepUpperScale
	upper bound for lambda decrease if a good LM step More...

double	_ni

int	_levenbergIterations
	the numer of levenberg iterations performed to accept the last step More...

Protected Attributes inherited from g2o::OptimizationAlgorithmWithHessian
Solver *	_solver

Property< bool > *	_writeDebug

Protected Attributes inherited from g2o::OptimizationAlgorithm
SparseOptimizer *	_optimizer
	the optimizer the solver is working on More...

PropertyMap	_properties
	the properties of your solver, use this to store the parameters of your solver More...

Detailed Description

Implementation of the Levenberg Algorithm.

Definition at line 38 of file optimization_algorithm_levenberg.h.

Constructor & Destructor Documentation

g2o::OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg ( Solver * solver )

explicit

construct the Levenberg algorithm, which will use the given Solver for solving the linearized system.

Definition at line 40 of file optimization_algorithm_levenberg.cpp.

References _currentLambda, _goodStepLowerScale, _goodStepUpperScale, _levenbergIterations, _maxTrialsAfterFailure, _ni, g2o::OptimizationAlgorithm::_properties, _tau, _userLambdaInit, and g2o::PropertyMap::makeProperty().

                                                                                :
     OptimizationAlgorithmWithHessian(solver)
   {
     _currentLambda = -1.;
     _tau = 1e-5;
     _goodStepUpperScale = 2./3.;
     _goodStepLowerScale = 1./3.;
     _userLambdaInit = _properties.makeProperty<Property<double> >("initialLambda", 0.);
     _maxTrialsAfterFailure = _properties.makeProperty<Property<int> >("maxTrialsAfterFailure", 10);
     _ni=2.;
     _levenbergIterations = 0;
   }

g2o::OptimizationAlgorithmLevenberg::~OptimizationAlgorithmLevenberg ( )

virtual

Definition at line 53 of file optimization_algorithm_levenberg.cpp.

54 {

55 }

Member Function Documentation

double g2o::OptimizationAlgorithmLevenberg::computeLambdaInit ( ) const

protected

helper for Levenberg, this function computes the initial damping factor, if the user did not specify an own value, see setUserLambdaInit()

Definition at line 149 of file optimization_algorithm_levenberg.cpp.

References g2o::OptimizationAlgorithm::_optimizer, _tau, _userLambdaInit, g2o::OptimizableGraph::Vertex::dimension(), g2o::OptimizableGraph::Vertex::hessian(), g2o::SparseOptimizer::indexMapping(), and g2o::Property< T >::value().

Referenced by solve().

   {
     if (_userLambdaInit->value() > 0)
       return _userLambdaInit->value();
     double maxDiagonal=0.;
     for (size_t k = 0; k < _optimizer->indexMapping().size(); k++) {
       OptimizableGraph::Vertex* v = _optimizer->indexMapping()[k];
       assert(v);
       int dim = v->dimension();
       for (int j = 0; j < dim; ++j){
         maxDiagonal = std::max(fabs(v->hessian(j,j)),maxDiagonal);
       }
     }
     return _tau*maxDiagonal;
   }

double g2o::OptimizationAlgorithmLevenberg::computeScale ( ) const

protected

Definition at line 165 of file optimization_algorithm_levenberg.cpp.

References _currentLambda, g2o::OptimizationAlgorithmWithHessian::_solver, g2o::Solver::b(), g2o::Solver::vectorSize(), and g2o::Solver::x().

Referenced by solve().

   {
     double scale = 0.;
     for (size_t j=0; j < _solver->vectorSize(); j++){
       scale += _solver->x()[j] * (_currentLambda * _solver->x()[j] + _solver->b()[j]);
     }
     return scale;
   }

double g2o::OptimizationAlgorithmLevenberg::currentLambda ( ) const

inline

return the currently used damping factor

Definition at line 53 of file optimization_algorithm_levenberg.h.

53 { return _currentLambda;}

g2o::OptimizationAlgorithmLevenberg::_currentLambda

double _currentLambda

Definition: optimization_algorithm_levenberg.h:73

int g2o::OptimizationAlgorithmLevenberg::levenbergIteration ( )

inline

return the number of levenberg iterations performed in the last round

Definition at line 67 of file optimization_algorithm_levenberg.h.

67 { return _levenbergIterations;}

g2o::OptimizationAlgorithmLevenberg::_levenbergIterations

int _levenbergIterations

the numer of levenberg iterations performed to accept the last step

Definition: optimization_algorithm_levenberg.h:78

int g2o::OptimizationAlgorithmLevenberg::maxTrialsAfterFailure ( ) const

inline

get the number of inner iterations for Levenberg-Marquardt

Definition at line 59 of file optimization_algorithm_levenberg.h.

59 { return _maxTrialsAfterFailure->value();}

g2o::OptimizationAlgorithmLevenberg::_maxTrialsAfterFailure

Property< int > * _maxTrialsAfterFailure

Definition: optimization_algorithm_levenberg.h:71

g2o::Property::value

const T & value() const

Definition: property.h:57

void g2o::OptimizationAlgorithmLevenberg::printVerbose ( std::ostream & os ) const

virtual

called by the optimizer if verbose. re-implement, if you want to print something

Reimplemented from g2o::OptimizationAlgorithm.

Definition at line 184 of file optimization_algorithm_levenberg.cpp.

References _currentLambda, _levenbergIterations, g2o::OptimizationAlgorithmWithHessian::_solver, and g2o::Solver::schur().

   {
     os
       << "\t schur= " << _solver->schur()
       << "\t lambda= " << FIXED(_currentLambda)
       << "\t levenbergIter= " << _levenbergIterations;
   }

void g2o::OptimizationAlgorithmLevenberg::setMaxTrialsAfterFailure ( int max_trials )

the number of internal iteration if an update step increases chi^2 within Levenberg-Marquardt

Definition at line 174 of file optimization_algorithm_levenberg.cpp.

References _maxTrialsAfterFailure, and g2o::Property< T >::setValue().

   {
     _maxTrialsAfterFailure->setValue(max_trials);
   }

void g2o::OptimizationAlgorithmLevenberg::setUserLambdaInit ( double lambda )

specify the initial lambda used for the first iteraion, if not given the SparseOptimizer tries to compute a suitable value

Definition at line 179 of file optimization_algorithm_levenberg.cpp.

References _userLambdaInit, and g2o::Property< T >::setValue().

   {
     _userLambdaInit->setValue(lambda);
   }

OptimizationAlgorithm::SolverResult g2o::OptimizationAlgorithmLevenberg::solve	(	int	iteration,
		bool	online = `false`
	)

virtual

Solve one iteration. The SparseOptimizer running on-top will call this for the given number of iterations.

Parameters

iteration indicates the current iteration

Implements g2o::OptimizationAlgorithm.

Definition at line 57 of file optimization_algorithm_levenberg.cpp.

References __PRETTY_FUNCTION__, _currentLambda, _goodStepLowerScale, _goodStepUpperScale, _levenbergIterations, _maxTrialsAfterFailure, _ni, g2o::OptimizationAlgorithm::_optimizer, g2o::OptimizationAlgorithmWithHessian::_solver, g2o::SparseOptimizer::activeRobustChi2(), g2o::Solver::buildStructure(), g2o::Solver::buildSystem(), g2o::SparseOptimizer::computeActiveErrors(), computeLambdaInit(), computeScale(), g2o::SparseOptimizer::discardTop(), g2o_isfinite, g2o::get_monotonic_time(), g2o::G2OBatchStatistics::globalStats(), g2o::G2OBatchStatistics::levenbergIterations, OK, g2o::Solver::optimizer(), g2o::SparseOptimizer::pop(), g2o::SparseOptimizer::push(), g2o::Solver::restoreDiagonal(), g2o::Solver::setLambda(), g2o::Solver::solve(), Terminate, g2o::SparseOptimizer::terminate(), g2o::G2OBatchStatistics::timeLinearSolution, g2o::G2OBatchStatistics::timeQuadraticForm, g2o::G2OBatchStatistics::timeResiduals, g2o::G2OBatchStatistics::timeUpdate, g2o::Property< T >::value(), and g2o::Solver::x().

   {
     assert(_optimizer && "_optimizer not set");
     assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph");
 
     if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure
       bool ok = _solver->buildStructure();
       if (! ok) {
         cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
         return OptimizationAlgorithm::Fail;
       }
     }
 
     double t=get_monotonic_time();
     _optimizer->computeActiveErrors();
     G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
     if (globalStats) {
       globalStats->timeResiduals = get_monotonic_time()-t;
       t=get_monotonic_time();
     }
 
     double currentChi = _optimizer->activeRobustChi2();
     double tempChi=currentChi;
 
     _solver->buildSystem();
     if (globalStats) {
       globalStats->timeQuadraticForm = get_monotonic_time()-t;
     }
 
     // core part of the Levenbarg algorithm
     if (iteration == 0) {
       _currentLambda = computeLambdaInit();
       _ni = 2;
     }
 
     double rho=0;
     int& qmax = _levenbergIterations;
     qmax = 0;
     do {
       _optimizer->push();
       if (globalStats) {
         globalStats->levenbergIterations++;
         t=get_monotonic_time();
       }
       // update the diagonal of the system matrix
       _solver->setLambda(_currentLambda, true);
       bool ok2 = _solver->solve();
       if (globalStats) {
         globalStats->timeLinearSolution+=get_monotonic_time()-t;
         t=get_monotonic_time();
       }
       _optimizer->update(_solver->x());
       if (globalStats) {
         globalStats->timeUpdate = get_monotonic_time()-t;
       }
 
       // restore the diagonal
       _solver->restoreDiagonal();
 
       _optimizer->computeActiveErrors();
       tempChi = _optimizer->activeRobustChi2();
 
       if (! ok2)
         tempChi=std::numeric_limits<double>::max();
 
       rho = (currentChi-tempChi);
       double scale = computeScale();
       scale += 1e-3; // make sure it's non-zero :)
       rho /=  scale;
 
       if (rho>0 && g2o_isfinite(tempChi)){ // last step was good
         double alpha = 1.-pow((2*rho-1),3);
         // crop lambda between minimum and maximum factors
         alpha = (std::min)(alpha, _goodStepUpperScale);
         double scaleFactor = (std::max)(_goodStepLowerScale, alpha);
         _currentLambda *= scaleFactor;
         _ni = 2;
         currentChi=tempChi;
         _optimizer->discardTop();
       } else {
         _currentLambda*=_ni;
         _ni*=2;
         _optimizer->pop(); // restore the last state before trying to optimize
       }
       qmax++;
     } while (rho<0 && qmax < _maxTrialsAfterFailure->value() && ! _optimizer->terminate());
 
     if (qmax == _maxTrialsAfterFailure->value() || rho==0)
       return Terminate;
     return OK;
   }