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/ISAM2Params.h>
#include <gtsam/nonlinear/ISAM2Result.h>
#include <gtsam/nonlinear/ISAM2Clique.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianBayesTree.h>

#include <vector>

namespace gtsam {

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(dellaert): 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;  

  explicit 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 Vector& delta = getDelta()[key];
    return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta);
  }


  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:
  void addVariables(const Values& newTheta);

  void removeVariables(const KeySet& unusedKeys);

  void expmapMasked(const KeySet& mask);

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

  virtual boost::shared_ptr<KeySet> recalculate(
      const KeySet& markedKeys, const KeySet& relinKeys,
      const KeyVector& 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> {};

}  // namespace gtsam

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