g2o::OptimizationAlgorithmDogleg Class Reference

Implementation of Powell's Dogleg Algorithm. More...

#include <optimization_algorithm_dogleg.h>

Inheritance diagram for g2o::OptimizationAlgorithmDogleg:

Collaboration diagram for g2o::OptimizationAlgorithmDogleg:

Public Types
enum	{ STEP_UNDEFINED , STEP_SD , STEP_GN , STEP_DL }
	type of the step to take More...

Public Member Functions
	OptimizationAlgorithmDogleg (std::unique_ptr< BlockSolverBase > solver)
virtual	~OptimizationAlgorithmDogleg ()
virtual SolverResult	solve (int iteration, bool online=false)
virtual void	printVerbose (std::ostream &os) const
int	lastStep () const
	return the type of the last step taken by the algorithm
double	trustRegion () const
	return the diameter of the trust region
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

Static Public Member Functions
static const char *	stepType2Str (int stepType)
	convert the type into an integer

Protected Attributes
Property< int > *	_maxTrialsAfterFailure
Property< double > *	_userDeltaInit
Property< double > *	_initialLambda
Property< double > *	_lamdbaFactor
VectorX	_hsd
	steepest decent step
VectorX	_hdl
	final dogleg step
VectorX	_auxVector
double	_currentLambda
	the damping factor to force positive definite matrix
double	_delta
	trust region
int	_lastStep
	type of the step taken by the algorithm
bool	_wasPDInAllIterations
int	_lastNumTries
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< BlockSolverBase >	m_solver

Detailed Description

Implementation of Powell's Dogleg Algorithm.

Definition at line 42 of file optimization_algorithm_dogleg.h.

Member Enumeration Documentation

anonymous enum

anonymous enum

type of the step to take

Enumerator
STEP_UNDEFINED
STEP_SD
STEP_GN
STEP_DL

Definition at line 46 of file optimization_algorithm_dogleg.h.

{ STEP_UNDEFINED, STEP_SD, STEP_GN, STEP_DL };

Constructor & Destructor Documentation

OptimizationAlgorithmDogleg()

g2o::OptimizationAlgorithmDogleg::OptimizationAlgorithmDogleg ( std::unique_ptr< BlockSolverBase >  solver )

explicit

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

Definition at line 42 of file optimization_algorithm_dogleg.cpp.

    : OptimizationAlgorithmWithHessian(*solver.get()),
      m_solver{std::move(solver)} {
  _userDeltaInit =
      _properties.makeProperty<Property<double>>("initialDelta", (double)1e4);
  _maxTrialsAfterFailure =
      _properties.makeProperty<Property<int>>("maxTrialsAfterFailure", 100);
  _initialLambda =
      _properties.makeProperty<Property<double>>("initialLambda", (double)1e-7);
  _lamdbaFactor =
      _properties.makeProperty<Property<double>>("lambdaFactor", 10.);
  _delta = _userDeltaInit->value();
  _lastStep = STEP_UNDEFINED;
  _wasPDInAllIterations = true;
  _lastNumTries = 0;
  _currentLambda = 0.;
}

References _currentLambda, _delta, _initialLambda, _lamdbaFactor, _lastNumTries, _lastStep, _maxTrialsAfterFailure, g2o::OptimizationAlgorithm::_properties, _userDeltaInit, _wasPDInAllIterations, g2o::PropertyMap::makeProperty(), STEP_UNDEFINED, and g2o::Property< T >::value().

~OptimizationAlgorithmDogleg()

g2o::OptimizationAlgorithmDogleg::~OptimizationAlgorithmDogleg ( )

virtual

Definition at line 61 of file optimization_algorithm_dogleg.cpp.

{}

Member Function Documentation

lastStep()

int g2o::OptimizationAlgorithmDogleg::lastStep ( ) const

inline

return the type of the last step taken by the algorithm

Definition at line 61 of file optimization_algorithm_dogleg.h.

{ return _lastStep; }

printVerbose()

void g2o::OptimizationAlgorithmDogleg::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 219 of file optimization_algorithm_dogleg.cpp.

                                                                   {
  os << "\t Delta= " << _delta << "\t step= " << stepType2Str(_lastStep)
     << "\t tries= " << _lastNumTries;
  if (!_wasPDInAllIterations) os << "\t lambda= " << _currentLambda;
}

References _currentLambda, _delta, _lastNumTries, _lastStep, _wasPDInAllIterations, and stepType2Str().

solve()

OptimizationAlgorithm::SolverResult g2o::OptimizationAlgorithmDogleg::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 63 of file optimization_algorithm_dogleg.cpp.

                                {
  assert(_optimizer && "_optimizer not set");
  assert(_solver.optimizer() == _optimizer &&
         "underlying linear solver operates on different graph");
 
  BlockSolverBase& blockSolver = static_cast<BlockSolverBase&>(_solver);
 
  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;
    }
 
    // init some members to the current size of the problem
    _hsd.resize(_solver.vectorSize());
    _hdl.resize(_solver.vectorSize());
    _auxVector.resize(_solver.vectorSize());
    _delta = _userDeltaInit->value();
    _currentLambda = _initialLambda->value();
    _wasPDInAllIterations = true;
  }
 
  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;
  }
 
  VectorX::ConstMapType b(_solver.b(), _solver.vectorSize());
 
  // compute alpha
  _auxVector.setZero();
  blockSolver.multiplyHessian(_auxVector.data(), _solver.b());
  double bNormSquared = b.squaredNorm();
  double alpha = bNormSquared / _auxVector.dot(b);
 
  _hsd = alpha * b;
  double hsdNorm = _hsd.norm();
  double hgnNorm = -1.;
 
  bool solvedGaussNewton = false;
  bool goodStep = false;
  int& numTries = _lastNumTries;
  numTries = 0;
  do {
    ++numTries;
 
    if (!solvedGaussNewton) {
      const double minLambda = cst(1e-12);
      const double maxLambda = cst(1e3);
      solvedGaussNewton = true;
      // apply a damping factor to enforce positive definite Hessian, if the
      // matrix appeared to be not positive definite in at least one iteration
      // before. We apply a damping factor to obtain a PD matrix.
      bool solverOk = false;
      while (!solverOk) {
        if (!_wasPDInAllIterations)
          _solver.setLambda(_currentLambda,
                            true);  // add _currentLambda to the diagonal
        solverOk = _solver.solve();
        if (!_wasPDInAllIterations) _solver.restoreDiagonal();
        _wasPDInAllIterations = _wasPDInAllIterations && solverOk;
        if (!_wasPDInAllIterations) {
          // simple strategy to control the damping factor
          if (solverOk) {
            _currentLambda =
                std::max(minLambda,
                         _currentLambda / (cst(0.5) * _lamdbaFactor->value()));
          } else {
            _currentLambda *= _lamdbaFactor->value();
            if (_currentLambda > maxLambda) {
              _currentLambda = maxLambda;
              return Fail;
            }
          }
        }
      }
      hgnNorm = VectorX::ConstMapType(_solver.x(), _solver.vectorSize()).norm();
    }
 
    VectorX::ConstMapType hgn(_solver.x(), _solver.vectorSize());
    assert(hgnNorm >= 0. && "Norm of the GN step is not computed");
 
    if (hgnNorm < _delta) {
      _hdl = hgn;
      _lastStep = STEP_GN;
    } else if (hsdNorm > _delta) {
      _hdl = _delta / hsdNorm * _hsd;
      _lastStep = STEP_SD;
    } else {
      _auxVector = hgn - _hsd;  // b - a
      double c = _hsd.dot(_auxVector);
      double bmaSquaredNorm = _auxVector.squaredNorm();
      double beta;
      if (c <= 0.)
        beta = (-c + sqrt(c * c + bmaSquaredNorm *
                                      (_delta * _delta - _hsd.squaredNorm()))) /
               bmaSquaredNorm;
      else {
        double hsdSqrNorm = _hsd.squaredNorm();
        beta =
            (_delta * _delta - hsdSqrNorm) /
            (c + sqrt(c * c + bmaSquaredNorm * (_delta * _delta - hsdSqrNorm)));
      }
      assert(beta > 0. && beta < 1 && "Error while computing beta");
      _hdl = _hsd + beta * (hgn - _hsd);
      _lastStep = STEP_DL;
      assert(_hdl.norm() < _delta + 1e-5 &&
             "Computed step does not correspond to the trust region");
    }
 
    // compute the linear gain
    _auxVector.setZero();
    blockSolver.multiplyHessian(_auxVector.data(), _hdl.data());
    double linearGain = -1 * (_auxVector.dot(_hdl)) + 2 * (b.dot(_hdl));
 
    // apply the update and see what happens
    _optimizer->push();
    _optimizer->update(_hdl.data());
    _optimizer->computeActiveErrors();
    double newChi = _optimizer->activeRobustChi2();
    double nonLinearGain = currentChi - newChi;
    if (fabs(linearGain) < 1e-12) linearGain = cst(1e-12);
    double rho = nonLinearGain / linearGain;
    // cerr << PVAR(nonLinearGain) << " " << PVAR(linearGain) << " " <<
    // PVAR(rho) << endl;
    if (rho > 0) {  // step is good and will be accepted
      _optimizer->discardTop();
      goodStep = true;
    } else {  // recover previous state
      _optimizer->pop();
    }
 
    // update trust region based on the step quality
    if (rho > 0.75)
      _delta = std::max<double>(_delta, 3 * _hdl.norm());
    else if (rho < 0.25)
      _delta *= 0.5;
  } while (!goodStep && numTries < _maxTrialsAfterFailure->value());
  if (numTries == _maxTrialsAfterFailure->value() || !goodStep)
    return Terminate;
  return OK;
}

References __PRETTY_FUNCTION__, _auxVector, _currentLambda, _delta, _hdl, _hsd, _initialLambda, _lamdbaFactor, _lastNumTries, _lastStep, _maxTrialsAfterFailure, g2o::OptimizationAlgorithm::_optimizer, g2o::OptimizationAlgorithmWithHessian::_solver, _userDeltaInit, _wasPDInAllIterations, g2o::SparseOptimizer::activeRobustChi2(), g2o::Solver::b(), g2o::Solver::buildStructure(), g2o::Solver::buildSystem(), g2o::SparseOptimizer::computeActiveErrors(), g2o::cst(), g2o::SparseOptimizer::discardTop(), G2O_WARN, g2o::get_monotonic_time(), g2o::G2OBatchStatistics::globalStats(), g2o::BlockSolverBase::multiplyHessian(), OK, g2o::Solver::optimizer(), g2o::SparseOptimizer::pop(), g2o::SparseOptimizer::push(), g2o::Solver::restoreDiagonal(), g2o::Solver::setLambda(), g2o::Solver::solve(), STEP_DL, STEP_GN, STEP_SD, Terminate, g2o::G2OBatchStatistics::timeQuadraticForm, g2o::G2OBatchStatistics::timeResiduals, g2o::SparseOptimizer::update(), g2o::Property< T >::value(), g2o::Solver::vectorSize(), and g2o::Solver::x().

stepType2Str()

const char * g2o::OptimizationAlgorithmDogleg::stepType2Str ( int  stepType )

static

convert the type into an integer

Definition at line 225 of file optimization_algorithm_dogleg.cpp.

                                                                  {
  switch (stepType) {
    case STEP_SD:
      return "Descent";
    case STEP_GN:
      return "GN";
    case STEP_DL:
      return "Dogleg";
    default:
      return "Undefined";
  }
}

References STEP_DL, STEP_GN, and STEP_SD.

Referenced by printVerbose().

trustRegion()

double g2o::OptimizationAlgorithmDogleg::trustRegion ( ) const

inline

return the diameter of the trust region

Definition at line 63 of file optimization_algorithm_dogleg.h.

{ return _delta; }

Member Data Documentation

_auxVector

VectorX g2o::OptimizationAlgorithmDogleg::_auxVector

protected

auxiliary vector used to perform multiplications or other stuff

Definition at line 78 of file optimization_algorithm_dogleg.h.

Referenced by solve().

_currentLambda

double g2o::OptimizationAlgorithmDogleg::_currentLambda

protected

the damping factor to force positive definite matrix

Definition at line 82 of file optimization_algorithm_dogleg.h.

Referenced by OptimizationAlgorithmDogleg(), printVerbose(), and solve().

_delta

double g2o::OptimizationAlgorithmDogleg::_delta

protected

trust region

Definition at line 83 of file optimization_algorithm_dogleg.h.

Referenced by OptimizationAlgorithmDogleg(), printVerbose(), and solve().

_hdl

VectorX g2o::OptimizationAlgorithmDogleg::_hdl

protected

final dogleg step

Definition at line 77 of file optimization_algorithm_dogleg.h.

Referenced by solve().

_hsd

VectorX g2o::OptimizationAlgorithmDogleg::_hsd

protected

steepest decent step

Definition at line 76 of file optimization_algorithm_dogleg.h.

Referenced by solve().

_initialLambda

Property<double>* g2o::OptimizationAlgorithmDogleg::_initialLambda

protected

Definition at line 73 of file optimization_algorithm_dogleg.h.

Referenced by OptimizationAlgorithmDogleg(), and solve().

_lamdbaFactor

Property<double>* g2o::OptimizationAlgorithmDogleg::_lamdbaFactor

protected

Definition at line 74 of file optimization_algorithm_dogleg.h.

Referenced by OptimizationAlgorithmDogleg(), and solve().

_lastNumTries

int g2o::OptimizationAlgorithmDogleg::_lastNumTries

protected

Definition at line 87 of file optimization_algorithm_dogleg.h.

Referenced by OptimizationAlgorithmDogleg(), printVerbose(), and solve().

_lastStep

int g2o::OptimizationAlgorithmDogleg::_lastStep

protected

type of the step taken by the algorithm

Definition at line 84 of file optimization_algorithm_dogleg.h.

Referenced by OptimizationAlgorithmDogleg(), printVerbose(), and solve().

_maxTrialsAfterFailure

Property<int>* g2o::OptimizationAlgorithmDogleg::_maxTrialsAfterFailure

protected

Definition at line 70 of file optimization_algorithm_dogleg.h.

Referenced by OptimizationAlgorithmDogleg(), and solve().

_userDeltaInit

Property<double>* g2o::OptimizationAlgorithmDogleg::_userDeltaInit

protected

Definition at line 71 of file optimization_algorithm_dogleg.h.

Referenced by OptimizationAlgorithmDogleg(), and solve().

_wasPDInAllIterations

bool g2o::OptimizationAlgorithmDogleg::_wasPDInAllIterations

protected

the matrix we solve was positive definite in all iterations -> if not apply damping

Definition at line 85 of file optimization_algorithm_dogleg.h.

Referenced by OptimizationAlgorithmDogleg(), printVerbose(), and solve().

m_solver

std::unique_ptr<BlockSolverBase> g2o::OptimizationAlgorithmDogleg::m_solver

private

Definition at line 90 of file optimization_algorithm_dogleg.h.

---

The documentation for this class was generated from the following files:

• /build/reproducible-path/g2o-0~20230806/g2o/core/optimization_algorithm_dogleg.h
• /build/reproducible-path/g2o-0~20230806/g2o/core/optimization_algorithm_dogleg.cpp