Implementation of the Levenberg Algorithm. More...

#include <optimization_algorithm_levenberg.h>

Inheritance diagram for g2o::OptimizationAlgorithmLevenberg:

[legend]

Collaboration diagram for g2o::OptimizationAlgorithmLevenberg:

[legend]

Public Member Functions
	OptimizationAlgorithmLevenberg (std::unique_ptr< 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
void	setMaxTrialsAfterFailure (int max_trials)
int	maxTrialsAfterFailure () const
	get the number of inner iterations for Levenberg-Marquardt
double	userLambdaInit ()
void	setUserLambdaInit (double lambda)
int	levenbergIteration ()
	return the number of levenberg iterations performed in the last round
Public Member Functions inherited from g2o::OptimizationAlgorithmWithHessian
	OptimizationAlgorithmWithHessian (Solver &solver)
virtual	~OptimizationAlgorithmWithHessian ()
virtual bool	init (bool online=false)
virtual bool	computeMarginals (SparseBlockMatrix< MatrixX > &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
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
SparseOptimizer *	optimizer ()
void	setOptimizer (SparseOptimizer *optimizer)
const PropertyMap &	properties () const
	return the properties of the solver
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
double	_goodStepUpperScale
double	_ni
int	_levenbergIterations
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
PropertyMap	_properties

Private Attributes
std::unique_ptr< Solver >	m_solver

Detailed Description

Implementation of the Levenberg Algorithm.

Definition at line 40 of file optimization_algorithm_levenberg.h.

Constructor & Destructor Documentation

◆ OptimizationAlgorithmLevenberg()

g2o::OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg ( std::unique_ptr< Solver > solver )

explicit

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

Definition at line 41 of file optimization_algorithm_levenberg.cpp.

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

References _currentLambda, _goodStepLowerScale, _goodStepUpperScale, _levenbergIterations, _maxTrialsAfterFailure, _ni, g2o::OptimizationAlgorithm::_properties, _tau, _userLambdaInit, g2o::cst(), m_solver, g2o::OptimizationAlgorithmWithHessian::OptimizationAlgorithmWithHessian(), and g2o::OptimizationAlgorithmWithHessian::solver().

◆ ~OptimizationAlgorithmLevenberg()

g2o::OptimizationAlgorithmLevenberg::~OptimizationAlgorithmLevenberg ( )

virtual

Definition at line 57 of file optimization_algorithm_levenberg.cpp.

57{}

Member Function Documentation

◆ computeLambdaInit()

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 154 of file optimization_algorithm_levenberg.cpp.

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

References g2o::OptimizationAlgorithm::_optimizer, _tau, _userLambdaInit, g2o::OptimizableGraph::Vertex::dimension(), and g2o::OptimizableGraph::Vertex::hessian().

Referenced by solve().

◆ computeScale()

double g2o::OptimizationAlgorithmLevenberg::computeScale ( ) const

protected

Definition at line 168 of file optimization_algorithm_levenberg.cpp.

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

References _currentLambda, and g2o::OptimizationAlgorithmWithHessian::_solver.

Referenced by solve().

◆ currentLambda()

double g2o::OptimizationAlgorithmLevenberg::currentLambda ( ) const

inline

return the currently used damping factor

Definition at line 55 of file optimization_algorithm_levenberg.h.

55{ return _currentLambda; }

References _currentLambda.

◆ levenbergIteration()

int g2o::OptimizationAlgorithmLevenberg::levenbergIteration ( )

inline

return the number of levenberg iterations performed in the last round

Definition at line 72 of file optimization_algorithm_levenberg.h.

72{ return _levenbergIterations; }

References _levenbergIterations.

◆ maxTrialsAfterFailure()

int g2o::OptimizationAlgorithmLevenberg::maxTrialsAfterFailure ( ) const

inline

get the number of inner iterations for Levenberg-Marquardt

Definition at line 62 of file optimization_algorithm_levenberg.h.

62{ return _maxTrialsAfterFailure->value(); }

References _maxTrialsAfterFailure.

◆ printVerbose()

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 185 of file optimization_algorithm_levenberg.cpp.

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

References _currentLambda, _levenbergIterations, and g2o::OptimizationAlgorithmWithHessian::_solver.

◆ setMaxTrialsAfterFailure()

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 177 of file optimization_algorithm_levenberg.cpp.

                                                                            {
  _maxTrialsAfterFailure->setValue(max_trials);
}

References _maxTrialsAfterFailure.

◆ setUserLambdaInit()

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 181 of file optimization_algorithm_levenberg.cpp.

                                                                    {
  _userLambdaInit->setValue(lambda);
}

References _userLambdaInit.

◆ solve()

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 59 of file optimization_algorithm_levenberg.cpp.

                                {
  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) {
      G2O_WARN("{}: Failure while building CCS structure", __PRETTY_FUNCTION__);
      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();
 
  _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();
    double tempChi = _optimizer->activeRobustChi2();
 
    if (!ok2) tempChi = std::numeric_limits<double>::max();
 
    rho = (currentChi - tempChi);
    double scale =
        ok2 ? computeScale() + cst(1e-3) : 1;  // make sure it's non-zero :)
    rho /= scale;
 
    if (rho > 0 && g2o_isfinite(tempChi) && ok2) {  // 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
      if (!g2o_isfinite(_currentLambda)) break;
    }
    qmax++;
  } while (rho < 0 && qmax < _maxTrialsAfterFailure->value() &&
           !_optimizer->terminate());
 
  if (qmax == _maxTrialsAfterFailure->value() || rho == 0 ||
      !g2o_isfinite(_currentLambda))
    return Terminate;
  return OK;
}