optimization_algorithm_dogleg.h

// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
#ifndef G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H
#define G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H
 
#include "optimization_algorithm_with_hessian.h"
 
namespace NDEVR 
{
 
  class BlockSolverBase;

  template<class t_solver>
  class  OptimizationAlgorithmDogleg : public OptimizationAlgorithmWithHessian<t_solver>
  {
    public:
      enum {
        STEP_UNDEFINED,
        STEP_SD, STEP_GN, STEP_DL
      };
 
    public:
      explicit OptimizationAlgorithmDogleg()
       {
          _userDeltaInit = 1e4;
          _maxTrialsAfterFailure = 100;
          _initialLambda = 1e-7;
          _lamdbaFactor = 10.;
          _delta = _userDeltaInit;
          _lastStep = STEP_UNDEFINED;
          _wasPDInAllIterations = true;
      }
      virtual ~OptimizationAlgorithmDogleg() {};

      virtual OptimizationAlgorithm::SolverResult solve(int iteration, bool online, int* blockPoseIndices, int* blockLandmarkIndices)
      {
          lib_assert(OptimizationAlgorithmWithHessian<t_solver>::_optimizer, "_optimizer not set");
          lib_assert(OptimizationAlgorithmWithHessian<t_solver>::solver.optimizer() == OptimizationAlgorithmWithHessian<t_solver>::_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 = OptimizationAlgorithmWithHessian<t_solver>::template solver.buildStructure(false, blockPoseIndices, blockLandmarkIndices);
              if (!ok)
                  return OptimizationAlgorithm::Fail;
 
              // init some members to the current size of the problem
              _hsd.resize(OptimizationAlgorithmWithHessian<t_solver>::template solver.vectorSize());
              _hdl.resize(OptimizationAlgorithmWithHessian<t_solver>::template solver.vectorSize());
              _auxVector.resize(OptimizationAlgorithmWithHessian<t_solver>::template solver.vectorSize());
              _delta = _userDeltaInit;
              _currentLambda = _initialLambda;
              _wasPDInAllIterations = true;
          }
          OptimizationAlgorithmWithHessian::template solver.optimizer.computeActiveErrors();
 
          g_type currentChi = OptimizationAlgorithmWithHessian::template optimizer.activeRobustChi2();
 
          OptimizationAlgorithmWithHessian::template solver.buildSystem();
 
          Eigen::VectorX<g_type>::ConstMapType b(OptimizationAlgorithmWithHessian::template solver.b(), OptimizationAlgorithmWithHessian::template solver.vectorSize());
 
          // compute alpha
          _auxVector.setZero();
          OptimizationAlgorithmWithHessian::template solver.multiplyHessian(_auxVector.data(), OptimizationAlgorithmWithHessian::template solver.b());
          g_type bNormSquared = b.squaredNorm();
          g_type alpha = bNormSquared / _auxVector.dot(b);
 
          _hsd = alpha * b;
          g_type hsdNorm = _hsd.norm();
          g_type hgnNorm = -1.;
 
          bool solvedGaussNewton = false;
          bool goodStep = false;
          int& numTries = _lastNumTries;
          numTries = 0;
          do {
              ++numTries;
 
              if (!solvedGaussNewton) {
                  const g_type minLambda = 1e-12;
                  const g_type 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)
                          OptimizationAlgorithmWithHessian::template solver.setLambda(_currentLambda, true);   // add _currentLambda to the diagonal
                      solverOk = OptimizationAlgorithmWithHessian::template solver.solve();
                      if (!_wasPDInAllIterations)
                          OptimizationAlgorithmWithHessian::template solver.restoreDiagonal();
                      _wasPDInAllIterations = _wasPDInAllIterations && solverOk;
                      if (!_wasPDInAllIterations) {
                          // simple strategy to control the damping factor
                          if (solverOk) {
                              _currentLambda = getMax(minLambda, _currentLambda / (0.5 * _lamdbaFactor));
                          }
                          else {
                              _currentLambda *= _lamdbaFactor;
                              if (_currentLambda > maxLambda) {
                                  _currentLambda = maxLambda;
                                  return OptimizationAlgorithm::Fail;
                              }
                          }
                      }
                  }
                  if (!solverOk) {
                      return OptimizationAlgorithm::Fail;
                  }
                  hgnNorm = Eigen::VectorX<g_type>::ConstMapType(OptimizationAlgorithmWithHessian::template solver.x(), OptimizationAlgorithmWithHessian::template solver.vectorSize()).norm();
              }
 
              Eigen::VectorX<g_type>::ConstMapType hgn(OptimizationAlgorithmWithHessian::template solver.x(), OptimizationAlgorithmWithHessian::template solver.vectorSize());
              lib_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
                  g_type c = _hsd.dot(_auxVector);
                  g_type bmaSquaredNorm = _auxVector.squaredNorm();
                  g_type beta;
                  if (c <= 0.)
                      beta = (-c + sqrt(c * c + bmaSquaredNorm * (_delta * _delta - _hsd.squaredNorm()))) / bmaSquaredNorm;
                  else {
                      g_type 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();
              OptimizationAlgorithmWithHessian::template solver.multiplyHessian(_auxVector.data(), _hdl.data());
              g_type linearGain = -1 * (_auxVector.dot(_hdl)) + 2 * (b.dot(_hdl));
 
              // apply the update and see what happens
              OptimizationAlgorithmWithHessian::template solver.optimizer.push();
              OptimizationAlgorithmWithHessian::template solver.optimizer.update(_hdl.data());
              OptimizationAlgorithmWithHessian::template solver.optimizer.computeActiveErrors();
              g_type newChi = OptimizationAlgorithmWithHessian::template optimizer.activeRobustChi2();
              g_type nonLinearGain = currentChi - newChi;
              if (fabs(linearGain) < 1e-12)
                  linearGain = 1e-12;
              g_type rho = nonLinearGain / linearGain;
              //cerr << PVAR(nonLinearGain) << " " << PVAR(linearGain) << " " << PVAR(rho) << endl;
              if (rho > 0) { // step is good and will be accepted
                  OptimizationAlgorithmWithHessian::template solver.optimizer.discardTop();
                  goodStep = true;
              }
              else { // recover previous state
                  OptimizationAlgorithmWithHessian::template solver.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);
          if (numTries == _maxTrialsAfterFailure || !goodStep)
              return OptimizationAlgorithm::Terminate;
          return OptimizationAlgorithm::OK;
      }

      virtual void printVerbose(std::ostream& os) const
      {
          os
              << "\t Delta= " << _delta
              << "\t step= " << stepType2Str(_lastStep)
              << "\t tries= " << _lastNumTries;
          if (!_wasPDInAllIterations)
              os << "\t lambda= " << _currentLambda;
      }

      int lastStep() const { return _lastStep;}
      g_type trustRegion() const { return _delta;}

      static const char* stepType2Str(int stepType)
      {
          switch (stepType) {
          case STEP_SD: return "Descent";
          case STEP_GN: return "GN";
          case STEP_DL: return "Dogleg";
          default: return "Undefined";
          }
      }
 
    protected:
      int _maxTrialsAfterFailure;       
      g_type _userDeltaInit;            
      g_type _initialLambda;            
      g_type _lamdbaFactor;             
 
      Eigen::VectorX<g_type> _hsd;         
      Eigen::VectorX<g_type> _hdl;         
      Eigen::VectorX<g_type> _auxVector;   
 
      g_type _currentLambda;        
      g_type _delta;                
      int _lastStep;                
      bool _wasPDInAllIterations;   
      int _lastNumTries;            
  };
 
} // end namespace
 
#endif