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

#include <optimization_algorithm_dogleg.h>

Inheritance diagram for g2o::OptimizationAlgorithmDogleg:

[legend]

Collaboration diagram for g2o::OptimizationAlgorithmDogleg:

[legend]

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.

46{ STEP_UNDEFINED, STEP_SD, STEP_GN, STEP_DL };

g2o::OptimizationAlgorithmDogleg::STEP_SD

@ STEP_SD

Definition optimization_algorithm_dogleg.h:46

g2o::OptimizationAlgorithmDogleg::STEP_DL

@ STEP_DL

Definition optimization_algorithm_dogleg.h:46

g2o::OptimizationAlgorithmDogleg::STEP_GN

@ STEP_GN

Definition optimization_algorithm_dogleg.h:46

g2o::OptimizationAlgorithmDogleg::STEP_UNDEFINED

@ STEP_UNDEFINED

Definition optimization_algorithm_dogleg.h:46

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, m_solver, g2o::OptimizationAlgorithmWithHessian::OptimizationAlgorithmWithHessian(), g2o::OptimizationAlgorithmWithHessian::solver(), and STEP_UNDEFINED.

◆ ~OptimizationAlgorithmDogleg()

g2o::OptimizationAlgorithmDogleg::~OptimizationAlgorithmDogleg ( )

virtual

Definition at line 61 of file optimization_algorithm_dogleg.cpp.

61{}

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.

61{ return _lastStep; }

References _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::cst(), G2O_WARN, g2o::get_monotonic_time(), g2o::G2OBatchStatistics::globalStats(), g2o::BlockSolverBase::multiplyHessian(), OK, STEP_DL, STEP_GN, STEP_SD, Terminate, g2o::G2OBatchStatistics::timeQuadraticForm, and g2o::G2OBatchStatistics::timeResiduals.

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