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<BayesTree<ISAM2Clique> >(*this);
      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