ISAM2Clique.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/inference/BayesTreeCliqueBase.h>
#include <gtsam/inference/Key.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <string>
 
namespace gtsam {

class GTSAM_EXPORT ISAM2Clique
    : public BayesTreeCliqueBase<ISAM2Clique, GaussianFactorGraph> {
 public:
  typedef ISAM2Clique This;
  typedef BayesTreeCliqueBase<This, GaussianFactorGraph> Base;
  typedef std::shared_ptr<This> shared_ptr;
  typedef std::weak_ptr<This> weak_ptr;
  typedef GaussianConditional ConditionalType;
  typedef ConditionalType::shared_ptr sharedConditional;
 
  Base::FactorType::shared_ptr cachedFactor_;
  Vector gradientContribution_;
#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE
  mutable FastMap<Key, VectorValues::iterator> solnPointers_;
#endif

  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_; }

  void addGradientAtZero(VectorValues* g) const;
 
  bool equals(const This& other, double tol = 1e-9) const;

  void print(const std::string& s = "",
             const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
  void optimizeWildfire(const KeySet& replaced, double threshold,
                        KeySet* changed, VectorValues* delta,
                        size_t* count) const;
 
  bool optimizeWildfireNode(const KeySet& replaced, double threshold,
                            KeySet* changed, VectorValues* delta,
                            size_t* count) const;

  void nnz_internal(size_t* result) const;
  size_t calculate_nnz() const;

  void findAll(const KeySet& markedMask, KeySet* keys) const;
 
 private:
  friend class ISAM2;

  void addGradientAtZero(VectorValues* g,
                         bool* hasConstrainedConditional) const;

  bool isDirty(const KeySet& replaced, const KeySet& changed) const;

  void fastBackSubstitute(VectorValues* delta) const;
 
  /*
   * Check whether the values changed above a threshold, or always true if the
   * clique was replaced.
   */
  bool valuesChanged(const KeySet& replaced, const Vector& originalValues,
                     const VectorValues& delta, double threshold) const;

  void markFrontalsAsChanged(KeySet* changed) const;

  void restoreFromOriginals(const Vector& originalValues,
                            VectorValues* delta) const;
 
#if 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(cachedFactor_);
    ar& BOOST_SERIALIZATION_NVP(gradientContribution_);
  }
#endif
};  // \struct ISAM2Clique

size_t optimizeWildfire(const ISAM2Clique::shared_ptr& root, double threshold,
                        const KeySet& replaced, VectorValues* delta);
 
size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root,
                                    double threshold, const KeySet& replaced,
                                    VectorValues* delta);
 
}  // namespace gtsam