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 _maxTrialsAfterFailure, g2o::OptimizationAlgorithm::_properties, _userLambdaInit, and g2o::PropertyMap::makeProperty().

◆ ~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(), g2o::OptimizableGraph::Vertex::hessian(), g2o::SparseOptimizer::indexMapping(), and g2o::Property< T >::value().

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, g2o::OptimizationAlgorithmWithHessian::_solver, g2o::Solver::b(), g2o::Solver::vectorSize(), and g2o::Solver::x().

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

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

◆ 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(); }

◆ 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, g2o::OptimizationAlgorithmWithHessian::_solver, and g2o::Solver::schur().

◆ 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, and g2o::Property< T >::setValue().

◆ 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, and g2o::Property< T >::setValue().

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