diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 14eb6f5fb..8071f7011 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -30,7 +30,7 @@ namespace gtsam { // Forward declarations template class FactorGraph; - template class JunctionTree; + template class ClusterTree; /* ************************************************************************* */ /** clique statistics */ @@ -244,7 +244,7 @@ namespace gtsam { void fillNodesIndex(const sharedClique& subtree); // Friend JunctionTree because it directly fills roots and nodes index. - template friend class JunctionTree; + template friend class ClusterTree; private: /** Serialization function */ diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h new file mode 100644 index 000000000..9ca9d897c --- /dev/null +++ b/gtsam/inference/ClusterTree-inst.h @@ -0,0 +1,183 @@ +/** + * @file ClusterTree-inst.h + * @date Oct 8, 2013 + * @author Kai Ni + * @author Richard Roberts + * @author Frank Dellaert + * @brief Collects factorgraph fragments defined on variable clusters, arranged in a tree + */ + +#include +#include +#include +#include +#include + +#include +#include + +namespace gtsam +{ + namespace + { + /* ************************************************************************* */ + // Elimination traversal data - stores a pointer to the parent data and collects the factors + // resulting from elimination of the children. Also sets up BayesTree cliques with parent and + // child pointers. + template + struct EliminationData { + EliminationData* const parentData; + size_t myIndexInParent; + FastVector childFactors; + boost::shared_ptr bayesTreeNode; + EliminationData(EliminationData* _parentData, size_t nChildren) : + parentData(_parentData), + bayesTreeNode(boost::make_shared()) + { + if(parentData) { + myIndexInParent = parentData->childFactors.size(); + parentData->childFactors.push_back(typename CLUSTERTREE::sharedFactor()); + } else { + myIndexInParent = 0; + } + // Set up BayesTree parent and child pointers + if(parentData) { + if(parentData->parentData) // If our parent is not the dummy node + bayesTreeNode->parent_ = parentData->bayesTreeNode; + parentData->bayesTreeNode->children.push_back(bayesTreeNode); + } + } + }; + + /* ************************************************************************* */ + // Elimination pre-order visitor - just creates the EliminationData structure for the visited + // node. + template + EliminationData eliminationPreOrderVisitor( + const typename CLUSTERTREE::sharedNode& node, EliminationData& parentData) + { + EliminationData myData(&parentData, node->children.size()); + myData.bayesTreeNode->problemSize_ = node->problemSize(); + return myData; + } + + /* ************************************************************************* */ + // Elimination post-order visitor - combine the child factors with our own factors, add the + // resulting conditional to the BayesTree, and add the remaining factor to the parent. + template + struct EliminationPostOrderVisitor + { + const typename CLUSTERTREE::Eliminate& eliminationFunction; + typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex; + EliminationPostOrderVisitor(const typename CLUSTERTREE::Eliminate& eliminationFunction, + typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) : + eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) {} + void operator()(const typename CLUSTERTREE::sharedNode& node, EliminationData& myData) + { + // Typedefs + typedef typename CLUSTERTREE::sharedFactor sharedFactor; + typedef typename CLUSTERTREE::FactorType FactorType; + typedef typename CLUSTERTREE::FactorGraphType FactorGraphType; + typedef typename CLUSTERTREE::ConditionalType ConditionalType; + typedef typename CLUSTERTREE::BayesTreeType::Node BTNode; + + // Gather factors + FactorGraphType gatheredFactors; + gatheredFactors.reserve(node->factors.size() + node->children.size()); + gatheredFactors += node->factors; + gatheredFactors += myData.childFactors; + + // Check for Bayes tree orphan subtrees, and add them to our children + BOOST_FOREACH(const sharedFactor& f, node->factors) + { + if(const BayesTreeOrphanWrapper* asSubtree = dynamic_cast*>(f.get())) + { + myData.bayesTreeNode->children.push_back(asSubtree->clique); + asSubtree->clique->parent_ = myData.bayesTreeNode; + } + } + + // Do dense elimination step + std::pair, boost::shared_ptr > eliminationResult = + eliminationFunction(gatheredFactors, Ordering(node->keys)); + + // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor + myData.bayesTreeNode->setEliminationResult(eliminationResult); + + // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid + // putting orphan subtrees in the index - they'll already be in the index of the ISAM2 + // object they're added to. + BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals()) + nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode)); + + // Store remaining factor in parent's gathered factors + if(!eliminationResult.second->empty()) + myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second; + } + }; + } + + /* ************************************************************************* */ + template + void ClusterTree::Cluster::print( + const std::string& s, const KeyFormatter& keyFormatter) const + { + std::cout << s; + BOOST_FOREACH(Key j, keys) + std::cout << j << " "; + std::cout << "problemSize = " << problemSize_ << std::endl; + } + + /* ************************************************************************* */ + template + void ClusterTree::print( + const std::string& s, const KeyFormatter& keyFormatter) const + { + treeTraversal::PrintForest(*this, s, keyFormatter); + } + + /* ************************************************************************* */ + template + ClusterTree& ClusterTree::operator=(const This& other) + { + // Start by duplicating the tree. + roots_ = treeTraversal::CloneForest(other); + + // Assign the remaining factors - these are pointers to factors in the original factor graph and + // we do not clone them. + remainingFactors_ = other.remainingFactors_; + + return *this; + } + + /* ************************************************************************* */ + template + std::pair, boost::shared_ptr > + ClusterTree::eliminate(const Eliminate& function) const + { + gttic(ClusterTree_eliminate); + // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node + // that contains all of the roots as its children. rootsContainer also stores the remaining + // uneliminated factors passed up from the roots. + boost::shared_ptr result = boost::make_shared(); + EliminationData rootsContainer(0, roots_.size()); + EliminationPostOrderVisitor visitorPost(function, result->nodes_); + treeTraversal::DepthFirstForestParallel(*this, rootsContainer, + eliminationPreOrderVisitor, visitorPost, 10); + + // Create BayesTree from roots stored in the dummy BayesTree node. + result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(), rootsContainer.bayesTreeNode->children.end()); + + // Add remaining factors that were not involved with eliminated variables + boost::shared_ptr allRemainingFactors = boost::make_shared(); + allRemainingFactors->reserve(remainingFactors_.size() + rootsContainer.childFactors.size()); + allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end()); + BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) + if(factor) + allRemainingFactors->push_back(factor); + + // Return result + return std::make_pair(result, allRemainingFactors); + } + +} diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h new file mode 100644 index 000000000..5a412a79e --- /dev/null +++ b/gtsam/inference/ClusterTree.h @@ -0,0 +1,125 @@ +/** + * @file ClusterTree.h + * @date Oct 8, 2013 + * @author Kai Ni + * @author Richard Roberts + * @author Frank Dellaert + * @brief Collects factorgraph fragments defined on variable clusters, arranged in a tree + */ + +#pragma once + +#include +#include +#include + +namespace gtsam +{ + + /** + * A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: + * each node k represents a subset \f$ C_k \sub X \f$, and the tree is family preserving, in that + * each factor \f$ f_i \f$ is associated with a single cluster and \f$ scope(f_i) \sub C_k \f$. + * \nosubgrouping + */ + template + class ClusterTree + { + public: + typedef GRAPH FactorGraphType; ///< The factor graph type + typedef typename GRAPH::FactorType FactorType; ///< The type of factors + typedef ClusterTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor + typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination + typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals + typedef boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional + typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine + + struct Cluster { + typedef FastVector Keys; + typedef FastVector Factors; + typedef FastVector > Children; + + Keys keys; ///< Frontal keys of this node + Factors factors; ///< Factors associated with this node + Children children; ///< sub-trees + int problemSize_; + + int problemSize() const { return problemSize_; } + + /** print this node */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + }; + + typedef boost::shared_ptr sharedCluster; ///< Shared pointer to Cluster + typedef Cluster Node; ///< Define Node=Cluster for compatibility with tree traversal functions + typedef sharedCluster sharedNode; ///< Define Node=Cluster for compatibility with tree traversal functions + + /** concept check */ + GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); + + protected: + FastVector roots_; + FastVector remainingFactors_; + + /// @name Standard Constructors + /// @{ + + /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are + * copied, factors are not cloned. */ + ClusterTree(const This& other) { *this = other; } + + /// @} + + public: + /// @name Testable + /// @{ + + /** Print the cluster tree */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// @} + + /// @name Standard Interface + /// @{ + + /** Eliminate the factors to a Bayes tree and remaining factor graph + * @param function The function to use to eliminate, see the namespace functions + * in GaussianFactorGraph.h + * @return The Bayes tree and factor graph resulting from elimination + */ + std::pair, boost::shared_ptr > + eliminate(const Eliminate& function) const; + + /// @} + + /// @name Advanced Interface + /// @{ + + /** Return the set of roots (one for a tree, multiple for a forest) */ + const FastVector& roots() const { return roots_; } + + /** Return the remaining factors that are not pulled into elimination */ + const FastVector& remainingFactors() const { return remainingFactors_; } + + /// @} + + protected: + /// @name Details + + /// Assignment operator - makes a deep copy of the tree structure, but only pointers to factors + /// are copied, factors are not cloned. + This& operator=(const This& other); + + /// Default constructor to be used in derived classes + ClusterTree() {} + + /// @} + + }; + + +} + + diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index eb670642d..a6cb2d82e 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -20,16 +20,11 @@ #pragma once -#include -#include -#include #include +#include #include #include -#include -#include - namespace gtsam { namespace { @@ -119,121 +114,6 @@ namespace gtsam { } myData.myJTNode->problemSize_ = combinedProblemSize; } - - /* ************************************************************************* */ - // Elimination traversal data - stores a pointer to the parent data and collects the factors - // resulting from elimination of the children. Also sets up BayesTree cliques with parent and - // child pointers. - template - struct EliminationData { - EliminationData* const parentData; - size_t myIndexInParent; - FastVector childFactors; - boost::shared_ptr bayesTreeNode; - EliminationData(EliminationData* _parentData, size_t nChildren) : - parentData(_parentData), - bayesTreeNode(boost::make_shared()) - { - if(parentData) { - myIndexInParent = parentData->childFactors.size(); - parentData->childFactors.push_back(typename JUNCTIONTREE::sharedFactor()); - } else { - myIndexInParent = 0; - } - // Set up BayesTree parent and child pointers - if(parentData) { - if(parentData->parentData) // If our parent is not the dummy node - bayesTreeNode->parent_ = parentData->bayesTreeNode; - parentData->bayesTreeNode->children.push_back(bayesTreeNode); - } - } - }; - - /* ************************************************************************* */ - // Elimination pre-order visitor - just creates the EliminationData structure for the visited - // node. - template - EliminationData eliminationPreOrderVisitor( - const typename JUNCTIONTREE::sharedNode& node, EliminationData& parentData) - { - EliminationData myData(&parentData, node->children.size()); - myData.bayesTreeNode->problemSize_ = node->problemSize(); - return myData; - } - - /* ************************************************************************* */ - // Elimination post-order visitor - combine the child factors with our own factors, add the - // resulting conditional to the BayesTree, and add the remaining factor to the parent. - template - struct EliminationPostOrderVisitor - { - const typename JUNCTIONTREE::Eliminate& eliminationFunction; - typename JUNCTIONTREE::BayesTreeType::Nodes& nodesIndex; - EliminationPostOrderVisitor(const typename JUNCTIONTREE::Eliminate& eliminationFunction, - typename JUNCTIONTREE::BayesTreeType::Nodes& nodesIndex) : - eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) {} - void operator()(const typename JUNCTIONTREE::sharedNode& node, EliminationData& myData) - { - // Typedefs - typedef typename JUNCTIONTREE::sharedFactor sharedFactor; - typedef typename JUNCTIONTREE::FactorType FactorType; - typedef typename JUNCTIONTREE::FactorGraphType FactorGraphType; - typedef typename JUNCTIONTREE::ConditionalType ConditionalType; - typedef typename JUNCTIONTREE::BayesTreeType::Node BTNode; - - // Gather factors - FactorGraphType gatheredFactors; - gatheredFactors.reserve(node->factors.size() + node->children.size()); - gatheredFactors += node->factors; - gatheredFactors += myData.childFactors; - - // Check for Bayes tree orphan subtrees, and add them to our children - BOOST_FOREACH(const sharedFactor& f, node->factors) - { - if(const BayesTreeOrphanWrapper* asSubtree = dynamic_cast*>(f.get())) - { - myData.bayesTreeNode->children.push_back(asSubtree->clique); - asSubtree->clique->parent_ = myData.bayesTreeNode; - } - } - - // Do dense elimination step - std::pair, boost::shared_ptr > eliminationResult = - eliminationFunction(gatheredFactors, Ordering(node->keys)); - - // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor - myData.bayesTreeNode->setEliminationResult(eliminationResult); - - // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid - // putting orphan subtrees in the index - they'll already be in the index of the ISAM2 - // object they're added to. - BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals()) - nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode)); - - // Store remaining factor in parent's gathered factors - if(!eliminationResult.second->empty()) - myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second; - } - }; - } - - /* ************************************************************************* */ - template - void JunctionTree::Node::print( - const std::string& s, const KeyFormatter& keyFormatter) const - { - std::cout << s; - BOOST_FOREACH(Key j, keys) - std::cout << j << " "; - std::cout << "problemSize = " << problemSize_ << std::endl; - } - - /* ************************************************************************* */ - template - void JunctionTree::print( - const std::string& s, const KeyFormatter& keyFormatter) const - { - treeTraversal::PrintForest(*this, s, keyFormatter); } /* ************************************************************************* */ @@ -252,59 +132,15 @@ namespace gtsam { // the created junction tree roots in a dummy Node. typedef typename ETREE::Node ETreeNode; ConstructorTraversalData rootData(0); - rootData.myJTNode = boost::make_shared(); // Make a dummy node to gather the junction tree roots + rootData.myJTNode = boost::make_shared(); // Make a dummy node to gather the junction tree roots treeTraversal::DepthFirstForest(eliminationTree, rootData, ConstructorTraversalVisitorPre, ConstructorTraversalVisitorPost); // Assign roots from the dummy node - roots_ = rootData.myJTNode->children; + Base::roots_ = rootData.myJTNode->children; // Transfer remaining factors from elimination tree - remainingFactors_ = eliminationTree.remainingFactors(); - } - - /* ************************************************************************* */ - template - JunctionTree& JunctionTree::operator=(const This& other) - { - // Start by duplicating the tree. - roots_ = treeTraversal::CloneForest(other); - - // Assign the remaining factors - these are pointers to factors in the original factor graph and - // we do not clone them. - remainingFactors_ = other.remainingFactors_; - - return *this; - } - - /* ************************************************************************* */ - template - std::pair, boost::shared_ptr > - JunctionTree::eliminate(const Eliminate& function) const - { - gttic(JunctionTree_eliminate); - // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node - // that contains all of the roots as its children. rootsContainer also stores the remaining - // uneliminated factors passed up from the roots. - boost::shared_ptr result = boost::make_shared(); - EliminationData rootsContainer(0, roots_.size()); - EliminationPostOrderVisitor visitorPost(function, result->nodes_); - treeTraversal::DepthFirstForestParallel(*this, rootsContainer, - eliminationPreOrderVisitor, visitorPost, 10); - - // Create BayesTree from roots stored in the dummy BayesTree node. - result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(), rootsContainer.bayesTreeNode->children.end()); - - // Add remaining factors that were not involved with eliminated variables - boost::shared_ptr allRemainingFactors = boost::make_shared(); - allRemainingFactors->reserve(remainingFactors_.size() + rootsContainer.childFactors.size()); - allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end()); - BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) - if(factor) - allRemainingFactors->push_back(factor); - - // Return result - return std::make_pair(result, allRemainingFactors); + Base::remainingFactors_ = eliminationTree.remainingFactors(); } } //namespace gtsam diff --git a/gtsam/inference/JunctionTree.h b/gtsam/inference/JunctionTree.h index 987bfa205..b470ae95f 100644 --- a/gtsam/inference/JunctionTree.h +++ b/gtsam/inference/JunctionTree.h @@ -20,15 +20,14 @@ #pragma once -#include -#include -#include +#include namespace gtsam { /** - * A ClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with - * the additional property that it represents the clique tree associated with a Bayes net. + * A JunctionTree is a ClusterTree, i.e., a set of variable clusters with factors, arranged + * in a tree, with the additional property that it represents the clique tree associated + * with a Bayes net. * * In GTSAM a junction tree is an intermediate data structure in multifrontal * variable elimination. Each node is a cluster of factors, along with a @@ -47,45 +46,13 @@ namespace gtsam { * \nosubgrouping */ template - class JunctionTree { + class JunctionTree : public ClusterTree { public: - typedef GRAPH FactorGraphType; ///< The factor graph type - typedef typename GRAPH::FactorType FactorType; ///< The type of factors typedef JunctionTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor - typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination - typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals - typedef boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional - typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine - - struct Node { - typedef FastVector Keys; - typedef FastVector Factors; - typedef FastVector > Children; - - Keys keys; ///< Frontal keys of this node - Factors factors; ///< Factors associated with this node - Children children; ///< sub-trees - int problemSize_; - - int problemSize() const { return problemSize_; } - - /** print this node */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - }; - - typedef boost::shared_ptr sharedNode; ///< Shared pointer to Node - - private: - - /** concept check */ - GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); - - FastVector roots_; - FastVector remainingFactors_; + typedef ClusterTree Base; ///< Our base class protected: @@ -99,43 +66,6 @@ namespace gtsam { /** Build the junction tree from an elimination tree. */ template JunctionTree(const ETREE& eliminationTree); - - /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are - * copied, factors are not cloned. */ - JunctionTree(const This& other) { *this = other; } - - /** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors - * are copied, factors are not cloned. */ - This& operator=(const This& other); - - /// @} - - public: - - /// @name Standard Interface - /// @{ - - /** Eliminate the factors to a Bayes net and remaining factor graph - * @param function The function to use to eliminate, see the namespace functions - * in GaussianFactorGraph.h - * @return The Bayes net and factor graph resulting from elimination - */ - std::pair, boost::shared_ptr > - eliminate(const Eliminate& function) const; - - /** Print the junction tree */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - /// @} - - /// @name Advanced Interface - /// @{ - - /** Return the set of roots (one for a tree, multiple for a forest) */ - const FastVector& roots() const { return roots_; } - - /** Return the remaining factors that are not pulled into elimination */ - const FastVector& remainingFactors() const { return remainingFactors_; } /// @} @@ -146,4 +76,4 @@ namespace gtsam { }; -} \ No newline at end of file +} diff --git a/gtsam/linear/GaussianJunctionTree.cpp b/gtsam/linear/GaussianJunctionTree.cpp index 17549d381..982c17bd3 100644 --- a/gtsam/linear/GaussianJunctionTree.cpp +++ b/gtsam/linear/GaussianJunctionTree.cpp @@ -22,7 +22,8 @@ namespace gtsam { - // Instantiate base class + // Instantiate base classes + template class ClusterTree; template class JunctionTree; /* ************************************************************************* */ diff --git a/gtsam/symbolic/SymbolicJunctionTree.cpp b/gtsam/symbolic/SymbolicJunctionTree.cpp index 7d1af49f7..00a52497b 100644 --- a/gtsam/symbolic/SymbolicJunctionTree.cpp +++ b/gtsam/symbolic/SymbolicJunctionTree.cpp @@ -23,6 +23,7 @@ namespace gtsam { // Instantiate base class + template class ClusterTree; template class JunctionTree; /* ************************************************************************* */