BayesTreeCliqueBase.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
 
 * -------------------------------------------------------------------------- */
 
#pragma once
 
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h>
 
#include <string>
#include <mutex>
#include <optional>
 
namespace gtsam {
 
  // Forward declarations
  template<class CLIQUE> class BayesTree;
  template<class GRAPH> struct EliminationTraits;
 
  template<class DERIVED, class FACTORGRAPH>
  class BayesTreeCliqueBase
  {
  private:
    typedef BayesTreeCliqueBase<DERIVED, FACTORGRAPH> This;
    typedef DERIVED DerivedType;
    typedef EliminationTraits<FACTORGRAPH> EliminationTraitsType;
    typedef std::shared_ptr<This> shared_ptr;
    typedef std::weak_ptr<This> weak_ptr;
    typedef std::shared_ptr<DerivedType> derived_ptr;
    typedef std::weak_ptr<DerivedType> derived_weak_ptr;
 
  public:
    typedef FACTORGRAPH FactorGraphType;
    typedef typename EliminationTraitsType::BayesNetType BayesNetType;
    typedef typename BayesNetType::ConditionalType ConditionalType;
    typedef std::shared_ptr<ConditionalType> sharedConditional;
    typedef typename FactorGraphType::FactorType FactorType;
    typedef typename FactorGraphType::Eliminate Eliminate;
 
  protected:
 
 
    BayesTreeCliqueBase() : problemSize_(1) {}
 
    BayesTreeCliqueBase(const sharedConditional& conditional)
        : conditional_(conditional), problemSize_(1) {}
 
    BayesTreeCliqueBase(const BayesTreeCliqueBase& c)
        : conditional_(c.conditional_),
          parent_(c.parent_),
          children(c.children),
          problemSize_(c.problemSize_),
          is_root(c.is_root) {}
 
    BayesTreeCliqueBase& operator=(const BayesTreeCliqueBase& c) {
      conditional_ = c.conditional_;
      parent_ = c.parent_;
      children = c.children;
      problemSize_ = c.problemSize_;
      is_root = c.is_root;
      return *this;
    }
 
    // Virtual destructor.
    virtual ~BayesTreeCliqueBase() {}
 
 
    mutable std::optional<FactorGraphType> cachedSeparatorMarginal_;
    mutable std::mutex cachedSeparatorMarginalMutex_;
 
  public:
    sharedConditional conditional_;
    derived_weak_ptr parent_;
    FastVector<derived_ptr> children;
    int problemSize_;
 
    bool is_root = false;
 
    void setEliminationResult(const typename FactorGraphType::EliminationResult& eliminationResult);
 
 
    bool equals(const DERIVED& other, double tol = 1e-9) const;
 
    virtual void print(
        const std::string& s = "",
        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
 
    const sharedConditional& conditional() const { return conditional_; }
 
    inline bool isRoot() const { return parent_.expired(); }
 
    size_t nrChildren() const { return children.size(); }
 
    const derived_ptr operator[](size_t i) const { return children.at(i); }
 
    size_t treeSize() const;
 
    size_t numCachedSeparatorMarginals() const;
 
    derived_ptr parent() const { return parent_.lock(); }
 
    int problemSize() const { return problemSize_; }
 
 
    BayesNetType shortcut(const derived_ptr& root, Eliminate function = EliminationTraitsType::DefaultEliminate) const;
 
    FactorGraphType separatorMarginal(Eliminate function = EliminationTraitsType::DefaultEliminate) const;
 
    FactorGraphType marginal2(Eliminate function = EliminationTraitsType::DefaultEliminate) const;
 
    void deleteCachedShortcuts();
 
    const std::optional<FactorGraphType>& cachedSeparatorMarginal() const {
      std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
      return cachedSeparatorMarginal_; 
    }
 
    friend class BayesTree<DerivedType>;
 
  protected:
 
    KeyVector separator_setminus_B(const derived_ptr& B) const;
 
    KeyVector shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const;
 
    void deleteCachedShortcutsNonRecursive() { 
      std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
      cachedSeparatorMarginal_ = {}; 
    }
 
  private:
 
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 
    friend class boost::serialization::access;
    template<class ARCHIVE>
    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      if(!parent_.lock()) {
        is_root = true;
      }
      ar & BOOST_SERIALIZATION_NVP(is_root);
      ar & BOOST_SERIALIZATION_NVP(conditional_);
      if (!is_root) { // TODO(fan): Workaround for boost/serialization #119
        ar & BOOST_SERIALIZATION_NVP(parent_);
      }
      ar & BOOST_SERIALIZATION_NVP(children);
    }
#endif
 
 
  };
 
}