LevenbergMarquardtOptimizer.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

#pragma once

#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/date_time/posix_time/posix_time.hpp>

class NonlinearOptimizerMoreOptimizationTest;

namespace gtsam {

class LevenbergMarquardtOptimizer;

class GTSAM_EXPORT LevenbergMarquardtParams: public NonlinearOptimizerParams {

public:
  enum VerbosityLM {
    SILENT = 0, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA
  };

  static VerbosityLM verbosityLMTranslator(const std::string &s);
  static std::string verbosityLMTranslator(VerbosityLM value);

public:

  double lambdaInitial; 
  double lambdaFactor; 
  double lambdaUpperBound; 
  double lambdaLowerBound; 
  VerbosityLM verbosityLM; 
  double minModelFidelity; 
  std::string logFile; 
  bool diagonalDamping; 
  bool reuse_diagonal_; 
  bool useFixedLambdaFactor_; 
  double min_diagonal_; 
  double max_diagonal_; 

  LevenbergMarquardtParams() :
      lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound(
          0.0), verbosityLM(SILENT), minModelFidelity(1e-3),
          diagonalDamping(false), reuse_diagonal_(false), useFixedLambdaFactor_(true),
          min_diagonal_(1e-6), max_diagonal_(1e32) {
  }
  virtual ~LevenbergMarquardtParams() {
  }

  virtual void print(const std::string& str = "") const;

  inline double getlambdaInitial() const {
    return lambdaInitial;
  }
  inline double getlambdaFactor() const {
    return lambdaFactor;
  }
  inline double getlambdaUpperBound() const {
    return lambdaUpperBound;
  }
  inline double getlambdaLowerBound() const {
    return lambdaLowerBound;
  }
  inline std::string getVerbosityLM() const {
    return verbosityLMTranslator(verbosityLM);
  }
  inline std::string getLogFile() const {
    return logFile;
  }
  inline bool getDiagonalDamping() const {
    return diagonalDamping;
  }

  inline void setlambdaInitial(double value) {
    lambdaInitial = value;
  }
  inline void setlambdaFactor(double value) {
    lambdaFactor = value;
  }
  inline void setlambdaUpperBound(double value) {
    lambdaUpperBound = value;
  }
  inline void setlambdaLowerBound(double value) {
    lambdaLowerBound = value;
  }
  inline void setVerbosityLM(const std::string &s) {
    verbosityLM = verbosityLMTranslator(s);
  }
  inline void setLogFile(const std::string &s) {
    logFile = s;
  }
  inline void setDiagonalDamping(bool flag) {
    diagonalDamping = flag;
  }
  inline void setUseFixedLambdaFactor(bool flag) {
    useFixedLambdaFactor_ = flag;
  }
};

class GTSAM_EXPORT LevenbergMarquardtState: public NonlinearOptimizerState {

public:
  double lambda;
  int totalNumberInnerIterations; // The total number of inner iterations in the optimization (for each iteration, LM may try multiple iterations with different lambdas)
  boost::posix_time::ptime startTime;
  VectorValues hessianDiagonal; //only update hessianDiagonal when reuse_diagonal_ = false

  LevenbergMarquardtState() {
    initTime();
  }

  void initTime() {
    startTime = boost::posix_time::microsec_clock::universal_time();
  }

  virtual ~LevenbergMarquardtState() {
  }

protected:
  LevenbergMarquardtState(const NonlinearFactorGraph& graph,
      const Values& initialValues, const LevenbergMarquardtParams& params,
      unsigned int iterations = 0) :
      NonlinearOptimizerState(graph, initialValues, iterations), lambda(
          params.lambdaInitial), totalNumberInnerIterations(0) {
    initTime();
  }

  friend class LevenbergMarquardtOptimizer;
};

class GTSAM_EXPORT LevenbergMarquardtOptimizer: public NonlinearOptimizer {

protected:
  LevenbergMarquardtParams params_; 
  LevenbergMarquardtState state_; 

public:
  typedef boost::shared_ptr<LevenbergMarquardtOptimizer> shared_ptr;


  LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph,
      const Values& initialValues, const LevenbergMarquardtParams& params =
          LevenbergMarquardtParams()) :
      NonlinearOptimizer(graph), params_(ensureHasOrdering(params, graph)), state_(
          graph, initialValues, params_) {
  }

  LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph,
      const Values& initialValues, const Ordering& ordering) :
      NonlinearOptimizer(graph) {
    params_.ordering = ordering;
    state_ = LevenbergMarquardtState(graph, initialValues, params_);
  }

  double lambda() const {
    return state_.lambda;
  }

  // Apply policy to increase lambda if the current update was successful (stepQuality not used in the naive policy)
  void increaseLambda();

  // Apply policy to decrease lambda if the current update was NOT successful (stepQuality not used in the naive policy)
  void decreaseLambda(double stepQuality);

  int getInnerIterations() const {
    return state_.totalNumberInnerIterations;
  }

  virtual void print(const std::string& str = "") const {
    std::cout << str << "LevenbergMarquardtOptimizer" << std::endl;
    this->params_.print("  parameters:\n");
  }



  virtual ~LevenbergMarquardtOptimizer() {
  }

  virtual void iterate();

  const LevenbergMarquardtParams& params() const {
    return params_;
  }

  LevenbergMarquardtParams& params() {
    return params_;
  }

  const LevenbergMarquardtState& state() const {
    return state_;
  }

  LevenbergMarquardtState& state() {
    return state_;
  }

  GaussianFactorGraph::shared_ptr buildDampedSystem(const GaussianFactorGraph& linear);
  friend class ::NonlinearOptimizerMoreOptimizationTest;

  void writeLogFile(double currentError);


protected:

  virtual const NonlinearOptimizerParams& _params() const {
    return params_;
  }

  virtual const NonlinearOptimizerState& _state() const {
    return state_;
  }

  LevenbergMarquardtParams ensureHasOrdering(LevenbergMarquardtParams params,
      const NonlinearFactorGraph& graph) const;

  virtual GaussianFactorGraph::shared_ptr linearize() const;
};

}