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

#include <optimization_algorithm_dogleg.h>

Inheritance diagram for g2o::OptimizationAlgorithmDogleg:

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 (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 More...

double	trustRegion () const
	return the diameter of the trust region 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

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

Protected Attributes
Property< int > *	_maxTrialsAfterFailure

Property< double > *	_userDeltaInit

Property< double > *	_initialLambda

Property< double > *	_lamdbaFactor

VectorXD	_hsd
	steepest decent step More...

VectorXD	_hdl
	final dogleg step More...

VectorXD	_auxVector
	auxilary vector used to perform multiplications or other stuff More...

double	_currentLambda
	the damping factor to force positive definite matrix More...

double	_delta
	trust region More...

int	_lastStep
	type of the step taken by the algorithm More...

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

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 More...

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

Detailed Description

Implementation of Powell's Dogleg Algorithm.

Definition at line 40 of file optimization_algorithm_dogleg.h.

Member Enumeration Documentation

anonymous enum

type of the step to take

Enumerator
STEP_UNDEFINED
STEP_SD
STEP_GN
STEP_DL

Definition at line 44 of file optimization_algorithm_dogleg.h.

            {
         STEP_UNDEFINED,
         STEP_SD, STEP_GN, STEP_DL
       };

Constructor & Destructor Documentation

g2o::OptimizationAlgorithmDogleg::OptimizationAlgorithmDogleg ( BlockSolverBase * solver )

explicit

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

Definition at line 41 of file optimization_algorithm_dogleg.cpp.

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

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

g2o::OptimizationAlgorithmDogleg::~OptimizationAlgorithmDogleg ( )

virtual

Definition at line 53 of file optimization_algorithm_dogleg.cpp.

54 {

55 }

Member Function Documentation

int g2o::OptimizationAlgorithmDogleg::lastStep ( ) const

inline

return the type of the last step taken by the algorithm

Definition at line 62 of file optimization_algorithm_dogleg.h.

62 { return _lastStep;}

g2o::OptimizationAlgorithmDogleg::_lastStep

int _lastStep

type of the step taken by the algorithm

Definition: optimization_algorithm_dogleg.h:83

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 209 of file optimization_algorithm_dogleg.cpp.

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

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

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 57 of file optimization_algorithm_dogleg.cpp.

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::SparseOptimizer::discardTop(), 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::Property< T >::value(), g2o::Solver::vectorSize(), and g2o::Solver::x().

   {
     assert(_optimizer && "_optimizer not set");
     assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph");
     assert(dynamic_cast<BlockSolverBase*>(_solver) && "underlying linear solver is not a block solver");
 
     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) {
         cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
         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;
     }
 
     VectorXD::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 = 1e-12;
         const double maxLambda = 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 / (0.5 * _lamdbaFactor->value()));
             } else {
               _currentLambda *= _lamdbaFactor->value();
               if (_currentLambda > maxLambda) {
                 _currentLambda = maxLambda;
                 return Fail;
               }
             }
           }
         }
         if (!solverOk) {
           return Fail;
         }
         hgnNorm = VectorXD::ConstMapType(_solver->x(), _solver->vectorSize()).norm();
       }
 
       VectorXD::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 = 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(_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;
   }

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

static

convert the type into an integer

Definition at line 219 of file optimization_algorithm_dogleg.cpp.

References STEP_DL, STEP_GN, and STEP_SD.

Referenced by printVerbose().

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