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>