BayesTree.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/Key.h>
#include <gtsam/base/FastList.h>
#include <gtsam/base/ConcurrentMap.h>
#include <gtsam/base/FastVector.h>
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/version.hpp>
 
#include <stdexcept>
#include <unordered_map>
#include <vector>
#endif
 
#include <memory>
#include <string>
 
namespace gtsam {
 
  // Forward declarations
  template<class FACTOR> class FactorGraph;
  template<class BAYESTREE, class GRAPH> class EliminatableClusterTree;
 
  /* ************************************************************************* */
  struct GTSAM_EXPORT BayesTreeCliqueStats {
    double avgConditionalSize;
    std::size_t maxConditionalSize;
    double avgSeparatorSize;
    std::size_t maxSeparatorSize;
    void print(const std::string& s = "") const ;
  };

  struct GTSAM_EXPORT BayesTreeCliqueData {
    FastVector<std::size_t> conditionalSizes;
    FastVector<std::size_t> separatorSizes;
    BayesTreeCliqueStats getStats() const;
  };
 
  /* ************************************************************************* */
  template<class CLIQUE>
  class BayesTree
  {
  protected:
    typedef BayesTree<CLIQUE> This;
    typedef std::shared_ptr<This> shared_ptr;
 
  public:
    typedef CLIQUE Clique; 
    typedef std::shared_ptr<Clique> sharedClique; 
    typedef Clique Node; 
    typedef sharedClique sharedNode; 
    typedef typename CLIQUE::ConditionalType ConditionalType;
    typedef std::shared_ptr<ConditionalType> sharedConditional;
    typedef typename CLIQUE::BayesNetType BayesNetType;
    typedef std::shared_ptr<BayesNetType> sharedBayesNet;
    typedef typename CLIQUE::FactorType FactorType;
    typedef std::shared_ptr<FactorType> sharedFactor;
    typedef typename CLIQUE::FactorGraphType FactorGraphType;
    typedef std::shared_ptr<FactorGraphType> sharedFactorGraph;
    typedef typename FactorGraphType::Eliminate Eliminate;
    typedef typename CLIQUE::EliminationTraitsType EliminationTraitsType;

    typedef FastList<sharedClique> Cliques;

    typedef ConcurrentMap<Key, sharedClique> Nodes;

    typedef FastVector<sharedClique> Roots;
 
  protected:

    Nodes nodes_;

    Roots roots_;


    BayesTree() {}

    BayesTree(const This& other);


    ~BayesTree(); 

    This& operator=(const This& other);
 
  public:
  

    bool equals(const This& other, double tol = 1e-9) const;

    void print(const std::string& s = "",
        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;


    size_t size() const;

    inline bool empty() const {
      return nodes_.empty();
    }

    const Nodes& nodes() const { return nodes_; }

    sharedClique operator[](Key j) const { return nodes_.at(j); }

    const Roots& roots() const { return roots_;  }

    const sharedClique& clique(Key j) const {
      typename Nodes::const_iterator c = nodes_.find(j);
      if(c == nodes_.end())
        throw std::out_of_range("Requested the BayesTree clique for a key that is not in the BayesTree");
      else
        return c->second;
    }

    BayesTreeCliqueData getCliqueData() const;

    size_t numCachedSeparatorMarginals() const;

    sharedConditional marginalFactor(Key j, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;

    sharedFactorGraph joint(Key j1, Key j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;

    sharedFactorGraph joint(
        const KeyVector& keys,
        const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;

    sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;

    sharedBayesNet jointBayesNet(
        const KeyVector& keys,
        const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;


    void dot(std::ostream& os, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;

    std::string dot(
        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;

    void saveGraph(const std::string& filename,
                  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
  

    template<class CONTAINER>
    Key findParentClique(const CONTAINER& parents) const;

    void clear();

    void deleteCachedShortcuts();

    void removePath(sharedClique clique, BayesNetType* bn, Cliques* orphans);

    void removeTop(const KeyVector& keys, BayesNetType* bn, Cliques* orphans);

    Cliques removeSubtree(const sharedClique& subtree);

    void insertRoot(const sharedClique& subtree);

    void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());

    void addFactorsToGraph(FactorGraph<FactorType>* graph) const;

    gtsam::KeySet collectAffectedKeys(const gtsam::KeyVector& keys) const;
 
   protected:

    void dot(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter,
             size_t parentnum = 0) const;

    void getCliqueData(sharedClique clique, BayesTreeCliqueData* stats) const;

    void removeClique(sharedClique clique);

    void fillNodesIndex(const sharedClique& subtree);

    void collectAffectedPathKeys(gtsam::KeySet& traversedKeys,
                                 const sharedClique& clique) const;
 
    // Friend JunctionTree because it directly fills roots and nodes index.
    template<class BAYESTREE, class GRAPH> friend class EliminatableClusterTree;
 
   private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
    friend class boost::serialization::access;
    template<class ARCHIVE>
    void save(ARCHIVE& ar, const unsigned int version) const {
      if (version == 0) {
        ar & BOOST_SERIALIZATION_NVP(nodes_);
        ar & BOOST_SERIALIZATION_NVP(roots_);
        return;
      }
 
      std::vector<sharedClique> cliques, detached;
      std::unordered_map<const Clique*, size_t> indices;
      for (const sharedClique& root : roots_) {
        if (!indices.emplace(root.get(), cliques.size()).second)
          throw std::invalid_argument("BayesTree contains duplicate cliques");
        cliques.push_back(root);
      }
 
      std::vector<std::vector<size_t>> children;
      for (size_t i = 0; i < cliques.size(); ++i) {
        sharedClique copy = std::make_shared<Clique>(*cliques[i]);
        copy->parent_.reset();
        copy->children.clear();
        copy->is_root = true;
        detached.push_back(copy);
        children.emplace_back();
        for (const sharedClique& child : cliques[i]->children) {
          if (!indices.emplace(child.get(), cliques.size()).second)
            throw std::invalid_argument("BayesTree contains duplicate cliques");
          children.back().push_back(cliques.size());
          cliques.push_back(child);
        }
      }
 
      const size_t rootCount = roots_.size();
      ar & BOOST_SERIALIZATION_NVP(rootCount);
      ar & boost::serialization::make_nvp("cliques", detached);
      ar & BOOST_SERIALIZATION_NVP(children);
    }

    template<class ARCHIVE>
    void load(ARCHIVE& ar, const unsigned int version) {
      if (version == 0) {
        ar & BOOST_SERIALIZATION_NVP(nodes_);
        ar & BOOST_SERIALIZATION_NVP(roots_);
        return;
      }
 
      size_t rootCount;
      std::vector<sharedClique> cliques;
      std::vector<std::vector<size_t>> children;
      ar & BOOST_SERIALIZATION_NVP(rootCount);
      ar & BOOST_SERIALIZATION_NVP(cliques);
      ar & BOOST_SERIALIZATION_NVP(children);
      if (rootCount > cliques.size() || children.size() != cliques.size())
        throw std::invalid_argument("Invalid serialized BayesTree structure");
 
      nodes_.clear();
      roots_.assign(cliques.begin(), cliques.begin() + rootCount);
      for (size_t i = 0; i < cliques.size(); ++i) {
        sharedClique& clique = cliques[i];
        clique->parent_.reset();
        clique->children.clear();
        clique->is_root = i < rootCount;
      }
      for (size_t i = 0; i < cliques.size(); ++i) {
        sharedClique& clique = cliques[i];
        for (size_t childIndex : children[i]) {
          if (childIndex >= cliques.size())
            throw std::invalid_argument("Invalid serialized BayesTree child");
          sharedClique& child = cliques[childIndex];
          if (!child->parent_.expired())
            throw std::invalid_argument("Serialized BayesTree child has multiple parents");
          clique->children.push_back(child);
          child->parent_ = clique;
          child->is_root = false;
        }
      }
 
      for (size_t i = 0; i < cliques.size(); ++i) {
        if ((i < rootCount) != cliques[i]->isRoot())
          throw std::invalid_argument("Invalid serialized BayesTree roots");
        for (Key key : cliques[i]->conditional()->frontals()) {
          if (!nodes_.insert({key, cliques[i]}).second)
            throw std::invalid_argument("Serialized BayesTree contains duplicate keys");
        }
      }
    }
 
    BOOST_SERIALIZATION_SPLIT_MEMBER()
#endif

 
  }; // BayesTree
 
  /* ************************************************************************* */
  template <class CLIQUE, typename = void>
  class BayesTreeOrphanWrapper : public CLIQUE::ConditionalType {
   public:
    typedef CLIQUE CliqueType;
    typedef typename CLIQUE::ConditionalType Base;
 
    std::shared_ptr<CliqueType> clique;

    BayesTreeOrphanWrapper(const std::shared_ptr<CliqueType>& clique)
        : clique(clique) {
      this->keys_.assign(clique->conditional()->beginParents(),
                         clique->conditional()->endParents());
    }
 
    void print(
        const std::string& s = "",
        const KeyFormatter& formatter = DefaultKeyFormatter) const override {
      clique->print(s + "stored clique", formatter);
    }
  };
 
} 
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
namespace boost {
namespace serialization {

template <class CLIQUE>
struct version<gtsam::BayesTree<CLIQUE>> {
  typedef mpl::int_<1> type;
  typedef mpl::integral_c_tag tag;
  BOOST_STATIC_CONSTANT(int, value = type::value);
};
 
}  // namespace serialization
}  // namespace boost
#endif