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

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

// \callgraph

#pragma once

#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
#include <gtsam/linear/GaussianBayesTree.h>

#include <boost/variant.hpp>

namespace gtsam {

struct GTSAM_EXPORT ISAM2GaussNewtonParams {
  double wildfireThreshold; 

  ISAM2GaussNewtonParams(
      double _wildfireThreshold = 0.001 
  ) : wildfireThreshold(_wildfireThreshold) {}

  void print(const std::string str = "") const {
    std::cout << str << "type:              ISAM2GaussNewtonParams\n";
    std::cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
    std::cout.flush();
  }

  double getWildfireThreshold() const { return wildfireThreshold; }
  void setWildfireThreshold(double wildfireThreshold) { this->wildfireThreshold = wildfireThreshold; }
};

struct GTSAM_EXPORT ISAM2DoglegParams {
  double initialDelta; 
  double wildfireThreshold; 
  DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationMode; 
  bool verbose; 

  ISAM2DoglegParams(
      double _initialDelta = 1.0, 
      double _wildfireThreshold = 1e-5, 
      DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = DoglegOptimizerImpl::SEARCH_EACH_ITERATION, 
      bool _verbose = false 
  ) : initialDelta(_initialDelta), wildfireThreshold(_wildfireThreshold),
  adaptationMode(_adaptationMode), verbose(_verbose) {}

  void print(const std::string str = "") const {
    std::cout << str << "type:              ISAM2DoglegParams\n";
    std::cout << str << "initialDelta:      " << initialDelta << "\n";
    std::cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
    std::cout << str << "adaptationMode:    " << adaptationModeTranslator(adaptationMode) << "\n";
    std::cout.flush();
  }

  double getInitialDelta() const { return initialDelta; }
  double getWildfireThreshold() const { return wildfireThreshold; }
  std::string getAdaptationMode() const { return adaptationModeTranslator(adaptationMode); };
  bool isVerbose() const { return verbose; };

  void setInitialDelta(double initialDelta) { this->initialDelta = initialDelta; }
  void setWildfireThreshold(double wildfireThreshold) { this->wildfireThreshold = wildfireThreshold; }
  void setAdaptationMode(const std::string& adaptationMode) { this->adaptationMode = adaptationModeTranslator(adaptationMode); }
  void setVerbose(bool verbose) { this->verbose = verbose; };

  std::string adaptationModeTranslator(const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const;
  DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator(const std::string& adaptationMode) const;
};

typedef FastMap<char,Vector> ISAM2ThresholdMap;
typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue;
struct GTSAM_EXPORT ISAM2Params {
  typedef boost::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams> OptimizationParams; 
  typedef boost::variant<double, FastMap<char,Vector> > RelinearizationThreshold; 

  OptimizationParams optimizationParams;

  RelinearizationThreshold relinearizeThreshold;

  int relinearizeSkip; 

  bool enableRelinearization; 

  bool evaluateNonlinearError; 

  enum Factorization { CHOLESKY, QR };
  Factorization factorization;

  bool cacheLinearizedFactors;

  KeyFormatter keyFormatter; 

  bool enableDetailedResults; 

  bool enablePartialRelinearizationCheck;

  bool findUnusedFactorSlots;

  ISAM2Params(
      OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), 
      RelinearizationThreshold _relinearizeThreshold = 0.1, 
      int _relinearizeSkip = 10, 
      bool _enableRelinearization = true, 
      bool _evaluateNonlinearError = false, 
      Factorization _factorization = ISAM2Params::CHOLESKY, 
      bool _cacheLinearizedFactors = true, 
      const KeyFormatter& _keyFormatter = DefaultKeyFormatter 
  ) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold),
      relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization),
      evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization),
      cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter),
      enableDetailedResults(false), enablePartialRelinearizationCheck(false),
      findUnusedFactorSlots(false) {}

  void print(const std::string& str = "") const {
    std::cout << str << "\n";
    if(optimizationParams.type() == typeid(ISAM2GaussNewtonParams))
      boost::get<ISAM2GaussNewtonParams>(optimizationParams).print("optimizationParams:                ");
    else if(optimizationParams.type() == typeid(ISAM2DoglegParams))
      boost::get<ISAM2DoglegParams>(optimizationParams).print("optimizationParams:                ");
    else
      std::cout << "optimizationParams:                " << "{unknown type}" << "\n";
    if(relinearizeThreshold.type() == typeid(double))
      std::cout << "relinearizeThreshold:              " << boost::get<double>(relinearizeThreshold) << "\n";
    else
    {
      std::cout << "relinearizeThreshold:              " << "{mapped}" << "\n";
      for(const ISAM2ThresholdMapValue& value: boost::get<ISAM2ThresholdMap>(relinearizeThreshold)) {
        std::cout << "                                   '" << value.first << "' -> [" << value.second.transpose() << " ]\n";
      }
    }
    std::cout << "relinearizeSkip:                   " << relinearizeSkip << "\n";
    std::cout << "enableRelinearization:             " << enableRelinearization << "\n";
    std::cout << "evaluateNonlinearError:            " << evaluateNonlinearError << "\n";
    std::cout << "factorization:                     " << factorizationTranslator(factorization) << "\n";
    std::cout << "cacheLinearizedFactors:            " << cacheLinearizedFactors << "\n";
    std::cout << "enableDetailedResults:             " << enableDetailedResults << "\n";
    std::cout << "enablePartialRelinearizationCheck: " << enablePartialRelinearizationCheck << "\n";
    std::cout << "findUnusedFactorSlots:             " << findUnusedFactorSlots << "\n";
    std::cout.flush();
  }


  OptimizationParams getOptimizationParams() const { return this->optimizationParams; }
  RelinearizationThreshold getRelinearizeThreshold() const { return relinearizeThreshold; }
  int getRelinearizeSkip() const { return relinearizeSkip; }
  bool isEnableRelinearization() const { return enableRelinearization; }
  bool isEvaluateNonlinearError() const { return evaluateNonlinearError; }
  std::string getFactorization() const { return factorizationTranslator(factorization); }
  bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; }
  KeyFormatter getKeyFormatter() const { return keyFormatter; }
  bool isEnableDetailedResults() const { return enableDetailedResults; }
  bool isEnablePartialRelinearizationCheck() const { return enablePartialRelinearizationCheck; }

  void setOptimizationParams(OptimizationParams optimizationParams) { this->optimizationParams = optimizationParams; }
  void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) { this->relinearizeThreshold = relinearizeThreshold; }
  void setRelinearizeSkip(int relinearizeSkip) { this->relinearizeSkip = relinearizeSkip; }
  void setEnableRelinearization(bool enableRelinearization) { this->enableRelinearization = enableRelinearization; }
  void setEvaluateNonlinearError(bool evaluateNonlinearError) { this->evaluateNonlinearError = evaluateNonlinearError; }
  void setFactorization(const std::string& factorization) { this->factorization = factorizationTranslator(factorization); }
  void setCacheLinearizedFactors(bool cacheLinearizedFactors) { this->cacheLinearizedFactors = cacheLinearizedFactors; }
  void setKeyFormatter(KeyFormatter keyFormatter) { this->keyFormatter = keyFormatter; }
  void setEnableDetailedResults(bool enableDetailedResults) { this->enableDetailedResults = enableDetailedResults; }
  void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck) { this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; }

  GaussianFactorGraph::Eliminate getEliminationFunction() const {
    return factorization == CHOLESKY
      ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky
      : (GaussianFactorGraph::Eliminate)EliminateQR;
  }



  static Factorization factorizationTranslator(const std::string& str);
  static std::string factorizationTranslator(const Factorization& value);

};

typedef FastVector<size_t> FactorIndices;

struct GTSAM_EXPORT ISAM2Result {
  boost::optional<double> errorBefore;

  boost::optional<double> errorAfter;

  size_t variablesRelinearized;

  size_t variablesReeliminated;

  size_t factorsRecalculated;

  size_t cliques;

  FactorIndices newFactorsIndices;

  struct DetailedResults {
    struct VariableStatus {
      bool isReeliminated;
      bool isAboveRelinThreshold; 
      bool isRelinearizeInvolved; 
      bool isRelinearized; 
      bool isObserved; 
      bool isNew; 
      bool inRootClique; 
      VariableStatus(): isReeliminated(false), isAboveRelinThreshold(false), isRelinearizeInvolved(false),
          isRelinearized(false), isObserved(false), isNew(false), inRootClique(false) {}
    };

    FastMap<Key, VariableStatus> variableStatus;
  };

  boost::optional<DetailedResults> detail;


  void print(const std::string str = "") const {
    std::cout << str << "  Reelimintated: " << variablesReeliminated << "  Relinearized: " << variablesRelinearized << "  Cliques: " << cliques << std::endl;
  }

  size_t getVariablesRelinearized() const { return variablesRelinearized; };
  size_t getVariablesReeliminated() const { return variablesReeliminated; };
  size_t getCliques() const { return cliques; };
};

class GTSAM_EXPORT ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique, GaussianFactorGraph>
{
public:
  typedef ISAM2Clique This;
  typedef BayesTreeCliqueBase<This, GaussianFactorGraph> Base;
  typedef boost::shared_ptr<This> shared_ptr;
  typedef boost::weak_ptr<This> weak_ptr;
  typedef GaussianConditional ConditionalType;
  typedef ConditionalType::shared_ptr sharedConditional;

  Base::FactorType::shared_ptr cachedFactor_;
  Vector gradientContribution_;
  FastMap<Key, VectorValues::iterator> solnPointers_;

  ISAM2Clique() : Base() {}

  ISAM2Clique(const ISAM2Clique& other) :
    Base(other), cachedFactor_(other.cachedFactor_), gradientContribution_(other.gradientContribution_) {}

  ISAM2Clique& operator=(const ISAM2Clique& other)
  {
    Base::operator=(other);
    cachedFactor_ = other.cachedFactor_;
    gradientContribution_ = other.gradientContribution_;
    return *this;
  }

  void setEliminationResult(const FactorGraphType::EliminationResult& eliminationResult);

  Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }

  const Vector& gradientContribution() const { return gradientContribution_; }

  bool equals(const This& other, double tol=1e-9) const;

  void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;

private:

  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
    ar & BOOST_SERIALIZATION_NVP(cachedFactor_);
    ar & BOOST_SERIALIZATION_NVP(gradientContribution_);
  }
}; // \struct ISAM2Clique

class GTSAM_EXPORT ISAM2: public BayesTree<ISAM2Clique> {

protected:

  Values theta_;

  VariableIndex variableIndex_;

  mutable VectorValues delta_;

  mutable VectorValues deltaNewton_; // Only used when using Dogleg - stores the Gauss-Newton update
  mutable VectorValues RgProd_; // Only used when using Dogleg - stores R*g and is updated incrementally

  mutable KeySet deltaReplacedMask_; // TODO: Make sure accessed in the right way

  NonlinearFactorGraph nonlinearFactors_;

  mutable GaussianFactorGraph linearFactors_;

  ISAM2Params params_;

  mutable boost::optional<double> doglegDelta_;

  KeySet fixedVariables_;

  int update_count_; 

public:

  typedef ISAM2 This; 
  typedef BayesTree<ISAM2Clique> Base; 
  typedef Base::Clique Clique; 
  typedef Base::sharedClique sharedClique; 
  typedef Base::Cliques Cliques; 

  ISAM2(const ISAM2Params& params);

  ISAM2();

  virtual ~ISAM2() {}

  virtual bool equals(const ISAM2& other, double tol = 1e-9) const;

  virtual ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
      const Values& newTheta = Values(),
      const FactorIndices& removeFactorIndices = FactorIndices(),
      const boost::optional<FastMap<Key,int> >& constrainedKeys = boost::none,
      const boost::optional<FastList<Key> >& noRelinKeys = boost::none,
      const boost::optional<FastList<Key> >& extraReelimKeys = boost::none,
      bool force_relinearize = false);

  void marginalizeLeaves(const FastList<Key>& leafKeys,
    boost::optional<FactorIndices&> marginalFactorsIndices = boost::none,
    boost::optional<FactorIndices&> deletedFactorsIndices = boost::none);

  const Values& getLinearizationPoint() const {
    return theta_;
  }

  bool valueExists(Key key) const {
    return theta_.exists(key);
  }

  Values calculateEstimate() const;

  template<class VALUE>
  VALUE calculateEstimate(Key key) const;

  const Value& calculateEstimate(Key key) const;

  Matrix marginalCovariance(Key key) const;


  struct Impl;

  Values calculateBestEstimate() const;

  const VectorValues& getDelta() const;

  double error(const VectorValues& x) const;

  const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; }

  const VariableIndex& getVariableIndex() const { return variableIndex_; }

  const KeySet& getFixedVariables() const { return fixedVariables_; }

  size_t lastAffectedVariableCount;
  size_t lastAffectedFactorCount;
  size_t lastAffectedCliqueCount;
  size_t lastAffectedMarkedCount;
  mutable size_t lastBacksubVariableCount;
  size_t lastNnzTop;

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

  void printStats() const { getCliqueData().getStats().print(); }
  
  VectorValues gradientAtZero() const;
  

protected:

  FastSet<Key> getAffectedFactors(const FastList<Key>& keys) const;
  GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySet& relinKeys) const;
  GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);

  virtual boost::shared_ptr<KeySet > recalculate(const KeySet& markedKeys, const KeySet& relinKeys,
      const std::vector<Key>& observedKeys, const KeySet& unusedIndices, const boost::optional<FastMap<Key,int> >& constrainKeys, ISAM2Result& result);
  void updateDelta(bool forceFullSolve = false) const;

}; // ISAM2

template<> struct traits<ISAM2> : public Testable<ISAM2> {};

template<class CLIQUE>
size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root,
    double threshold, const KeySet& replaced, VectorValues& delta);

template<class CLIQUE>
size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root,
    double threshold, const KeySet& replaced, VectorValues& delta);

template<class CLIQUE>
int calculate_nnz(const boost::shared_ptr<CLIQUE>& clique);

} 

#include <gtsam/nonlinear/ISAM2-inl.h>
#include <gtsam/nonlinear/ISAM2-impl.h>