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

namespace gtsam {

class DoglegOptimizer;

class GTSAM_EXPORT DoglegParams : public NonlinearOptimizerParams {
public:
  enum VerbosityDL {
    SILENT,
    VERBOSE
  };

  double deltaInitial; 
  VerbosityDL verbosityDL; 

  DoglegParams() :
    deltaInitial(1.0), verbosityDL(SILENT) {}

  virtual ~DoglegParams() {}

  virtual void print(const std::string& str = "") const {
    NonlinearOptimizerParams::print(str);
    std::cout << "               deltaInitial: " << deltaInitial << "\n";
    std::cout.flush();
  }

  double getDeltaInitial() const { return deltaInitial; }
  std::string getVerbosityDL() const { return verbosityDLTranslator(verbosityDL); }

  void setDeltaInitial(double deltaInitial) { this->deltaInitial = deltaInitial; }
  void setVerbosityDL(const std::string& verbosityDL) { this->verbosityDL = verbosityDLTranslator(verbosityDL); }

private:
  VerbosityDL verbosityDLTranslator(const std::string& verbosityDL) const;
  std::string verbosityDLTranslator(VerbosityDL verbosityDL) const;
};

class GTSAM_EXPORT DoglegState : public NonlinearOptimizerState {
public:
  double Delta;

  DoglegState() {}

  virtual ~DoglegState() {}

protected:
  DoglegState(const NonlinearFactorGraph& graph, const Values& values, const DoglegParams& params, unsigned int iterations = 0) :
    NonlinearOptimizerState(graph, values, iterations), Delta(params.deltaInitial) {}

  friend class DoglegOptimizer;
};

class GTSAM_EXPORT DoglegOptimizer : public NonlinearOptimizer {

protected:
  DoglegParams params_;
  DoglegState state_;

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


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

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



  virtual ~DoglegOptimizer() {}

  virtual void iterate();

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

  DoglegParams& params() { return params_; }

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

  DoglegState& state() { return state_; }

  double getDelta() const { return state_.Delta; }


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

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

  DoglegParams ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const;
};

}