ClusterTree.h

 
#pragma once
 
#include <gtsam/base/Testable.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Ordering.h>
 
namespace gtsam {
 
template <class GRAPH>
class ClusterTree {
 public:
  typedef GRAPH FactorGraphType;               
  typedef ClusterTree<GRAPH> This;             
  typedef boost::shared_ptr<This> shared_ptr;  
 
  typedef typename GRAPH::FactorType FactorType;       
  typedef boost::shared_ptr<FactorType> sharedFactor;  
 
  // TODO(frank): re-factor JunctionTree so we can make members private
  struct Cluster {
    typedef FastVector<boost::shared_ptr<Cluster> > Children;
    Children children;  
 
    typedef Ordering Keys;
    Keys orderedFrontalKeys;  
 
    FactorGraphType factors;  
 
    int problemSize_;
 
    Cluster() : problemSize_(0) {}
 
    virtual ~Cluster() {}
 
    const Cluster& operator[](size_t i) const {
      return *(children[i]);
    }
 
    template <class CONTAINER>
    Cluster(Key key, const CONTAINER& factorsToAdd)
        : problemSize_(0) {
      addFactors(key, factorsToAdd);
    }
 
    template <class CONTAINER>
    void addFactors(Key key, const CONTAINER& factorsToAdd) {
      orderedFrontalKeys.push_back(key);
      factors.push_back(factorsToAdd);
      problemSize_ += factors.size();
    }
 
    void addChild(const boost::shared_ptr<Cluster>& cluster) {
      children.push_back(cluster);
      problemSize_ = std::max(problemSize_, cluster->problemSize_);
    }
 
    size_t nrChildren() const {
      return children.size();
    }
 
    size_t nrFactors() const {
      return factors.size();
    }
 
    size_t nrFrontals() const {
      return orderedFrontalKeys.size();
    }
 
    int problemSize() const {
      return problemSize_;
    }
 
    virtual void print(const std::string& s = "",
                       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
    std::vector<size_t> nrFrontalsOfChildren() const;
 
    void merge(const boost::shared_ptr<Cluster>& cluster);
 
    void mergeChildren(const std::vector<bool>& merge);
  };
 
  typedef boost::shared_ptr<Cluster> sharedCluster;  
 
  // Define Node=Cluster for compatibility with tree traversal functions
  typedef Cluster Node;
  typedef sharedCluster sharedNode;
 
  GTSAM_CONCEPT_TESTABLE_TYPE(FactorType)
 
 protected:
  FastVector<sharedNode> roots_;
 
 
  ClusterTree(const This& other) {
    *this = other;
  }
 
 
 public:
 
  ClusterTree() {}
 
 
  void print(const std::string& s = "",
             const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
 
 
  void addRoot(const boost::shared_ptr<Cluster>& cluster) {
    roots_.push_back(cluster);
  }
 
  void addChildrenAsRoots(const boost::shared_ptr<Cluster>& cluster) {
    for (auto child : cluster->children)
      this->addRoot(child);
  }
 
  size_t nrRoots() const {
    return roots_.size();
  }
 
  const FastVector<sharedNode>& roots() const {
    return roots_;
  }
 
  const Cluster& operator[](size_t i) const {
    return *(roots_[i]);
  }
 
 
 protected:
 
  This& operator=(const This& other);
 
};
 
template <class BAYESTREE, class GRAPH>
class EliminatableClusterTree : public ClusterTree<GRAPH> {
 public:
  typedef BAYESTREE BayesTreeType;  
  typedef GRAPH FactorGraphType;    
  typedef EliminatableClusterTree<BAYESTREE, GRAPH> This;  
  typedef boost::shared_ptr<This> shared_ptr;              
 
  typedef typename BAYESTREE::ConditionalType ConditionalType;  
  typedef boost::shared_ptr<ConditionalType>
      sharedConditional;  
 
  typedef typename GRAPH::Eliminate Eliminate;         
  typedef typename GRAPH::FactorType FactorType;       
  typedef boost::shared_ptr<FactorType> sharedFactor;  
 
 protected:
  FastVector<sharedFactor> remainingFactors_;
 
 
  EliminatableClusterTree(const This& other) : ClusterTree<GRAPH>(other) {
    *this = other;
  }
 
 
 public:
 
  std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > eliminate(
      const Eliminate& function) const;
 
 
 
  const FastVector<sharedFactor>& remainingFactors() const {
    return remainingFactors_;
  }
 
 
 protected:
 
  This& operator=(const This& other);
 
  EliminatableClusterTree() {}
 
};
}
 
#include <gtsam/inference/ClusterTree-inst.h>