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/linear/GaussianBayesTree.h>
#include <gtsam/nonlinear/ISAM2Clique.h>
#include <gtsam/nonlinear/ISAM2Params.h>
#include <gtsam/nonlinear/ISAM2Result.h>
#include <gtsam/nonlinear/ISAM2UpdateParams.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.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 std::optional<double> doglegDelta_;
 
  KeySet fixedVariables_;
 
  int update_count_;  
 
 public:
  using This = ISAM2;                       
  using Base = BayesTree<ISAM2Clique>;      
  using Clique = Base::Clique;              
  using sharedClique = Base::sharedClique;  
  using Cliques = Base::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 std::optional<FastMap<Key, int> >& constrainedKeys = {},
      const std::optional<FastList<Key> >& noRelinKeys = {},
      const std::optional<FastList<Key> >& extraReelimKeys = {},
      bool force_relinearize = false);
 
  virtual ISAM2Result update(const NonlinearFactorGraph& newFactors,
                             const Values& newTheta,
                             const ISAM2UpdateParams& updateParams);
 
  void marginalizeLeaves(
      const FastList<Key>& leafKeys,
      FactorIndices* marginalFactorsIndices = nullptr,
      FactorIndices* deletedFactorsIndices = nullptr);
 
  template <class... OptArgs>
      void marginalizeLeaves(const FastList<Key>& leafKeys,
                             OptArgs&&... optArgs) {
          // dereference the optional arguments and pass
          // it to the pointer version
          marginalizeLeaves(leafKeys, (&optArgs)...);
      }
 
  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;
 
 
  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_; }
 
  const ISAM2Params& params() const { return params_; }
 
  void printStats() const { getCliqueData().getStats().print(); }
 
  VectorValues gradientAtZero() const;
 
 
 protected:
  void recalculate(const ISAM2UpdateParams& updateParams,
                   const KeySet& relinKeys, ISAM2Result* result);
 
  // Do a batch step - reorder and relinearize all variables
  void recalculateBatch(const ISAM2UpdateParams& updateParams,
                        KeySet* affectedKeysSet, ISAM2Result* result);
 
  // retrieve all factors that ONLY contain the affected variables
  // (note that the remaining stuff is summarized in the cached factors)
  GaussianFactorGraph relinearizeAffectedFactors(
      const ISAM2UpdateParams& updateParams, const FastList<Key>& affectedKeys,
      const KeySet& relinKeys);
 
  void recalculateIncremental(const ISAM2UpdateParams& updateParams,
                              const KeySet& relinKeys,
                              const FastList<Key>& affectedKeys,
                              KeySet* affectedKeysSet, Cliques* orphans,
                              ISAM2Result* result);
 
  void addVariables(const Values& newTheta,
                    ISAM2Result::DetailedResults* detail = 0);
 
  void removeVariables(const KeySet& unusedKeys);
 
  void updateDelta(bool forceFullSolve = false) const;
 
 private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 
  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(theta_);
      ar & BOOST_SERIALIZATION_NVP(variableIndex_);
      ar & BOOST_SERIALIZATION_NVP(delta_);
      ar & BOOST_SERIALIZATION_NVP(deltaNewton_);
      ar & BOOST_SERIALIZATION_NVP(RgProd_);
      ar & BOOST_SERIALIZATION_NVP(deltaReplacedMask_);
      ar & BOOST_SERIALIZATION_NVP(nonlinearFactors_);
      ar & BOOST_SERIALIZATION_NVP(linearFactors_);
      ar & BOOST_SERIALIZATION_NVP(doglegDelta_);
      ar & BOOST_SERIALIZATION_NVP(fixedVariables_);
      ar & BOOST_SERIALIZATION_NVP(update_count_);
  }
#endif
 
};  // ISAM2
 
template <>
struct traits<ISAM2> : public Testable<ISAM2> {};
 
}  // namespace gtsam