From 2dd83fd92c12d7a01bacaa8bc11e05ebf64b210a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 11:44:17 -0700 Subject: [PATCH 01/11] Count then merge --- gtsam/inference/JunctionTree-inst.h | 125 +++++++++++++++++----------- 1 file changed, 77 insertions(+), 48 deletions(-) diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index d7ff0281a..5735a3bd2 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -27,7 +27,7 @@ namespace gtsam { -template +template struct ConstructorTraversalData { typedef typename JunctionTree::Node Node; typedef typename JunctionTree::sharedNode sharedNode; @@ -37,8 +37,13 @@ struct ConstructorTraversalData { FastVector childSymbolicConditionals; FastVector childSymbolicFactors; - ConstructorTraversalData(ConstructorTraversalData* _parentData) - : parentData(_parentData) {} + // Small inner class to store symbolic factors + class SymbolicFactors: public FactorGraph { + }; + + ConstructorTraversalData(ConstructorTraversalData* _parentData) : + parentData(_parentData) { + } // Pre-order visitor function static ConstructorTraversalData ConstructorTraversalVisitorPre( @@ -64,13 +69,11 @@ struct ConstructorTraversalData { // our number of symbolic elimination parents is exactly 1 less than // our child's symbolic elimination parents - this condition indicates that // eliminating the current node did not introduce any parents beyond those - // already in the child. + // already in the child-> // Do symbolic elimination for this node - class : public FactorGraph {} - symbolicFactors; - symbolicFactors.reserve(ETreeNode->factors.size() + - myData.childSymbolicFactors.size()); + SymbolicFactors symbolicFactors; + symbolicFactors.reserve(ETreeNode->factors.size() + myData.childSymbolicFactors.size()); // Add ETree node factors symbolicFactors += ETreeNode->factors; // Add symbolic factors passed up from children @@ -78,60 +81,87 @@ struct ConstructorTraversalData { Ordering keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key); - std::pair - symbolicElimResult = - internal::EliminateSymbolic(symbolicFactors, keyAsOrdering); + SymbolicConditional::shared_ptr myConditional; + SymbolicFactor::shared_ptr mySeparatorFactor; + boost::tie(myConditional, mySeparatorFactor) = internal::EliminateSymbolic( + symbolicFactors, keyAsOrdering); // Store symbolic elimination results in the parent - myData.parentData->childSymbolicConditionals.push_back( - symbolicElimResult.first); - myData.parentData->childSymbolicFactors.push_back( - symbolicElimResult.second); + myData.parentData->childSymbolicConditionals.push_back(myConditional); + myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor); + sharedNode node = myData.myJTNode; + const FastVector& childConditionals = + myData.childSymbolicConditionals; // Merge our children if they are in our clique - if our conditional has // exactly one fewer parent than our child's conditional. size_t myNrFrontals = 1; - const size_t myNrParents = symbolicElimResult.first->nrParents(); - size_t nrMergedChildren = 0; - assert(node->children.size() == myData.childSymbolicConditionals.size()); - // Loop over children - int combinedProblemSize = - (int)(symbolicElimResult.first->size() * symbolicFactors.size()); + const size_t myNrParents = myConditional->nrParents(); + assert(node->newChildren.size() == childConditionals.size()); + gttic(merge_children); - for (size_t i = 0; i < myData.childSymbolicConditionals.size(); ++i) { + // First count how many keys, factors and children we'll end up with + size_t nrKeys = node->orderedFrontalKeys.size(); + size_t nrFactors = node->factors.size(); + size_t nrChildren = 0; + // Loop over children + for (size_t i = 0; i < childConditionals.size(); ++i) { // Check if we should merge the i^th child - if (myNrParents + myNrFrontals == - myData.childSymbolicConditionals[i]->nrParents()) { + if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { // Get a reference to the i, adjusting the index to account for children // previously merged and removed from the i list. - const Node& child = *node->children[i - nrMergedChildren]; - // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. - node->orderedFrontalKeys.insert(node->orderedFrontalKeys.end(), - child.orderedFrontalKeys.rbegin(), - child.orderedFrontalKeys.rend()); - // Merge keys, factors, and children. - node->factors.insert(node->factors.end(), child.factors.begin(), child.factors.end()); - node->children.insert(node->children.end(), child.children.begin(), child.children.end()); - // Increment problem size - combinedProblemSize = std::max(combinedProblemSize, child.problemSize_); + sharedNode child = node->children[i]; + nrKeys += child->orderedFrontalKeys.size(); + nrFactors += child->factors.size(); + nrChildren += child->children.size(); // Increment number of frontal variables - myNrFrontals += child.orderedFrontalKeys.size(); - // Remove i from list. - node->children.erase(node->children.begin() + (i - nrMergedChildren)); - // Increment number of merged children - ++nrMergedChildren; + myNrFrontals += child->orderedFrontalKeys.size(); + } else { + nrChildren += 1; // we keep the child } } + + // now reserve space, and really merge + node->orderedFrontalKeys.reserve(nrKeys); + node->factors.reserve(nrFactors); + typename Node::Children newChildren; + newChildren.reserve(nrChildren); + myNrFrontals = 1; + int combinedProblemSize = (int) (myConditional->size() * symbolicFactors.size()); + // Loop over newChildren + for (size_t i = 0; i < childConditionals.size(); ++i) { + // Check if we should merge the i^th child + sharedNode child = node->children[i]; + if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { + // Get a reference to the i, adjusting the index to account for newChildren + // previously merged and removed from the i list. + // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. + node->orderedFrontalKeys.insert(node->orderedFrontalKeys.end(), + child->orderedFrontalKeys.rbegin(), + child->orderedFrontalKeys.rend()); + // Merge keys, factors, and children. + node->factors.insert(node->factors.end(), child->factors.begin(), child->factors.end()); + newChildren.insert(newChildren.end(), child->children.begin(), child->children.end()); + // Increment problem size + combinedProblemSize = std::max(combinedProblemSize, child->problemSize_); + // Increment number of frontal variables + myNrFrontals += child->orderedFrontalKeys.size(); + } else { + newChildren.push_back(child); // we keep the child + } + } + node->children = newChildren; std::reverse(node->orderedFrontalKeys.begin(), node->orderedFrontalKeys.end()); gttoc(merge_children); node->problemSize_ = combinedProblemSize; } -}; +} +; /* ************************************************************************* */ -template -template +template +template JunctionTree::JunctionTree( const EliminationTree& eliminationTree) { gttic(JunctionTree_FromEliminationTree); @@ -147,12 +177,11 @@ JunctionTree::JunctionTree( typedef typename EliminationTree::Node ETreeNode; typedef ConstructorTraversalData Data; Data 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, - Data::ConstructorTraversalVisitorPre, - Data::ConstructorTraversalVisitorPostAlg2); + Data::ConstructorTraversalVisitorPre, + Data::ConstructorTraversalVisitorPostAlg2); // Assign roots from the dummy node Base::roots_ = rootData.myJTNode->children; @@ -161,4 +190,4 @@ JunctionTree::JunctionTree( Base::remainingFactors_ = eliminationTree.remainingFactors(); } -} // namespace gtsam +} // namespace gtsam From 67013cba054e2e5bb81f7fbd8257a3ffb710b401 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 12:49:33 -0700 Subject: [PATCH 02/11] Separated merge decision from actual merging --- gtsam/inference/JunctionTree-inst.h | 61 ++++++++++++++++++----------- 1 file changed, 39 insertions(+), 22 deletions(-) diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 5735a3bd2..232246d5e 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -73,7 +73,8 @@ struct ConstructorTraversalData { // Do symbolic elimination for this node SymbolicFactors symbolicFactors; - symbolicFactors.reserve(ETreeNode->factors.size() + myData.childSymbolicFactors.size()); + symbolicFactors.reserve( + ETreeNode->factors.size() + myData.childSymbolicFactors.size()); // Add ETree node factors symbolicFactors += ETreeNode->factors; // Add symbolic factors passed up from children @@ -96,29 +97,42 @@ struct ConstructorTraversalData { // Merge our children if they are in our clique - if our conditional has // exactly one fewer parent than our child's conditional. - size_t myNrFrontals = 1; const size_t myNrParents = myConditional->nrParents(); - assert(node->newChildren.size() == childConditionals.size()); + const size_t nrChildren = node->children.size(); + assert(childConditionals.size() == nrChildren); gttic(merge_children); - // First count how many keys, factors and children we'll end up with + + // decide which children to merge, as index into children + std::vector merge(nrChildren, false); + { + size_t myNrFrontals = 1; + for (size_t i = 0; i < nrChildren; ++i) { + // Check if we should merge the i^th child + if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { + sharedNode child = node->children[i]; + // Increment number of frontal variables + myNrFrontals += child->orderedFrontalKeys.size(); + merge[i] = true; + } + } + } + + // Count how many keys, factors and children we'll end up with size_t nrKeys = node->orderedFrontalKeys.size(); size_t nrFactors = node->factors.size(); - size_t nrChildren = 0; + size_t nrNewChildren = 0; // Loop over children - for (size_t i = 0; i < childConditionals.size(); ++i) { - // Check if we should merge the i^th child - if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { + for (size_t i = 0; i < nrChildren; ++i) { + if (merge[i]) { // Get a reference to the i, adjusting the index to account for children // previously merged and removed from the i list. sharedNode child = node->children[i]; nrKeys += child->orderedFrontalKeys.size(); nrFactors += child->factors.size(); - nrChildren += child->children.size(); - // Increment number of frontal variables - myNrFrontals += child->orderedFrontalKeys.size(); + nrNewChildren += child->children.size(); } else { - nrChildren += 1; // we keep the child + nrNewChildren += 1; // we keep the child } } @@ -126,14 +140,14 @@ struct ConstructorTraversalData { node->orderedFrontalKeys.reserve(nrKeys); node->factors.reserve(nrFactors); typename Node::Children newChildren; - newChildren.reserve(nrChildren); - myNrFrontals = 1; - int combinedProblemSize = (int) (myConditional->size() * symbolicFactors.size()); + newChildren.reserve(nrNewChildren); + int combinedProblemSize = (int) (myConditional->size() + * symbolicFactors.size()); // Loop over newChildren - for (size_t i = 0; i < childConditionals.size(); ++i) { + for (size_t i = 0; i < nrChildren; ++i) { // Check if we should merge the i^th child sharedNode child = node->children[i]; - if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { + if (merge[i]) { // Get a reference to the i, adjusting the index to account for newChildren // previously merged and removed from the i list. // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. @@ -141,18 +155,21 @@ struct ConstructorTraversalData { child->orderedFrontalKeys.rbegin(), child->orderedFrontalKeys.rend()); // Merge keys, factors, and children. - node->factors.insert(node->factors.end(), child->factors.begin(), child->factors.end()); - newChildren.insert(newChildren.end(), child->children.begin(), child->children.end()); + node->factors.insert(node->factors.end(), child->factors.begin(), + child->factors.end()); + newChildren.insert(newChildren.end(), child->children.begin(), + child->children.end()); // Increment problem size - combinedProblemSize = std::max(combinedProblemSize, child->problemSize_); + combinedProblemSize = std::max(combinedProblemSize, + child->problemSize_); // Increment number of frontal variables - myNrFrontals += child->orderedFrontalKeys.size(); } else { newChildren.push_back(child); // we keep the child } } node->children = newChildren; - std::reverse(node->orderedFrontalKeys.begin(), node->orderedFrontalKeys.end()); + std::reverse(node->orderedFrontalKeys.begin(), + node->orderedFrontalKeys.end()); gttoc(merge_children); node->problemSize_ = combinedProblemSize; } From 0d0a9e5b16308c1190ede3f4d4e9dfd901cf3b35 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 12:57:26 -0700 Subject: [PATCH 03/11] Formatting only --- gtsam/inference/ClusterTree.h | 209 +++++++++++++++++----------------- 1 file changed, 105 insertions(+), 104 deletions(-) diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index bb7cf17e1..260397ab1 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -13,118 +13,119 @@ #include #include -namespace gtsam -{ +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 +/** + * 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 Ordering Keys; - typedef FastVector Factors; - typedef FastVector > Children; + struct Cluster { + typedef Ordering Keys; + typedef FastVector Factors; + typedef FastVector > Children; - Cluster() {} - Cluster(Key key, const Factors& factors) : factors(factors) { - orderedFrontalKeys.push_back(key); - } + Cluster() { + } + Cluster(Key key, const Factors& factors) : + factors(factors) { + orderedFrontalKeys.push_back(key); + } - Keys orderedFrontalKeys; ///< Frontal keys of this node - Factors factors; ///< Factors associated with this node - Children children; ///< sub-trees - int problemSize_; + Keys orderedFrontalKeys; ///< 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() {} - - /// @} + 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() {} + + /// @} + +}; } - From c8f8791bab62fad97e9ed671533c9c733a09d500 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 13:06:42 -0700 Subject: [PATCH 04/11] Moved merging to ClusterTree --- gtsam/inference/ClusterTree.h | 55 +++++++++++++++++++++ gtsam/inference/JunctionTree-inst.h | 74 ++++------------------------- 2 files changed, 65 insertions(+), 64 deletions(-) diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 260397ab1..0b98d3e36 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -58,6 +58,61 @@ public: /** print this node */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + void mergeChildren(const std::vector& merge) { + gttic(merge_children); + size_t nrChildren = children.size(); + + // Count how many keys, factors and children we'll end up with + size_t nrKeys = this->orderedFrontalKeys.size(); + size_t nrFactors = this->factors.size(); + size_t nrNewChildren = 0; + // Loop over children + for (size_t i = 0; i < nrChildren; ++i) { + if (merge[i]) { + // Get a reference to the i, adjusting the index to account for children + // previously merged and removed from the i list. + sharedNode child = this->children[i]; + nrKeys += child->orderedFrontalKeys.size(); + nrFactors += child->factors.size(); + nrNewChildren += child->children.size(); + } else { + nrNewChildren += 1; // we keep the child + } + } + + // now reserve space, and really merge + this->orderedFrontalKeys.reserve(nrKeys); + this->factors.reserve(nrFactors); + typename Node::Children newChildren; + newChildren.reserve(nrNewChildren); + // Loop over newChildren + for (size_t i = 0; i < nrChildren; ++i) { + // Check if we should merge the i^th child + sharedNode child = this->children[i]; + if (merge[i]) { + // Get a reference to the i, adjusting the index to account for newChildren + // previously merged and removed from the i list. + // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. + this->orderedFrontalKeys.insert(this->orderedFrontalKeys.end(), + child->orderedFrontalKeys.rbegin(), + child->orderedFrontalKeys.rend()); + // Merge keys, factors, and children. + this->factors.insert(this->factors.end(), child->factors.begin(), + child->factors.end()); + newChildren.insert(newChildren.end(), child->children.begin(), + child->children.end()); + // Increment problem size + problemSize_ = std::max(problemSize_, child->problemSize_); + // Increment number of frontal variables + } else { + newChildren.push_back(child); // we keep the child + } + } + this->children = newChildren; + std::reverse(this->orderedFrontalKeys.begin(), + this->orderedFrontalKeys.end()); + } }; typedef boost::shared_ptr sharedCluster; ///< Shared pointer to Cluster diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 232246d5e..de349ddc8 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -94,6 +94,7 @@ struct ConstructorTraversalData { sharedNode node = myData.myJTNode; const FastVector& childConditionals = myData.childSymbolicConditionals; + node->problemSize_ = (int) (myConditional->size() * symbolicFactors.size()); // Merge our children if they are in our clique - if our conditional has // exactly one fewer parent than our child's conditional. @@ -105,76 +106,21 @@ struct ConstructorTraversalData { // decide which children to merge, as index into children std::vector merge(nrChildren, false); - { - size_t myNrFrontals = 1; - for (size_t i = 0; i < nrChildren; ++i) { - // Check if we should merge the i^th child - if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { - sharedNode child = node->children[i]; - // Increment number of frontal variables - myNrFrontals += child->orderedFrontalKeys.size(); - merge[i] = true; - } - } - } - - // Count how many keys, factors and children we'll end up with - size_t nrKeys = node->orderedFrontalKeys.size(); - size_t nrFactors = node->factors.size(); - size_t nrNewChildren = 0; - // Loop over children - for (size_t i = 0; i < nrChildren; ++i) { - if (merge[i]) { - // Get a reference to the i, adjusting the index to account for children - // previously merged and removed from the i list. - sharedNode child = node->children[i]; - nrKeys += child->orderedFrontalKeys.size(); - nrFactors += child->factors.size(); - nrNewChildren += child->children.size(); - } else { - nrNewChildren += 1; // we keep the child - } - } - - // now reserve space, and really merge - node->orderedFrontalKeys.reserve(nrKeys); - node->factors.reserve(nrFactors); - typename Node::Children newChildren; - newChildren.reserve(nrNewChildren); - int combinedProblemSize = (int) (myConditional->size() - * symbolicFactors.size()); - // Loop over newChildren + size_t myNrFrontals = 1; for (size_t i = 0; i < nrChildren; ++i) { // Check if we should merge the i^th child - sharedNode child = node->children[i]; - if (merge[i]) { - // Get a reference to the i, adjusting the index to account for newChildren - // previously merged and removed from the i list. - // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. - node->orderedFrontalKeys.insert(node->orderedFrontalKeys.end(), - child->orderedFrontalKeys.rbegin(), - child->orderedFrontalKeys.rend()); - // Merge keys, factors, and children. - node->factors.insert(node->factors.end(), child->factors.begin(), - child->factors.end()); - newChildren.insert(newChildren.end(), child->children.begin(), - child->children.end()); - // Increment problem size - combinedProblemSize = std::max(combinedProblemSize, - child->problemSize_); + if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { + sharedNode child = node->children[i]; // Increment number of frontal variables - } else { - newChildren.push_back(child); // we keep the child + myNrFrontals += child->orderedFrontalKeys.size(); + merge[i] = true; } } - node->children = newChildren; - std::reverse(node->orderedFrontalKeys.begin(), - node->orderedFrontalKeys.end()); - gttoc(merge_children); - node->problemSize_ = combinedProblemSize; + + // now really merge + node->mergeChildren(merge); } -} -; +}; /* ************************************************************************* */ template From 000f807eda758ee2423181e06b933b240e164fa9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 13:10:44 -0700 Subject: [PATCH 05/11] Formatting only --- gtsam/inference/ClusterTree-inst.h | 310 ++++++++++++++--------------- 1 file changed, 155 insertions(+), 155 deletions(-) diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index 1987a0a5a..b76430c20 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -7,178 +7,178 @@ * @brief Collects factorgraph fragments defined on variable clusters, arranged in a tree */ -#include -#include #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); - } - } - }; +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 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; + } } - /* ************************************************************************* */ - // 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; + // Do dense elimination step + std::pair, boost::shared_ptr > eliminationResult = + eliminationFunction(gatheredFactors, node->orderedFrontalKeys); - // Gather factors - FactorGraphType gatheredFactors; - gatheredFactors.reserve(node->factors.size() + node->children.size()); - gatheredFactors += node->factors; - gatheredFactors += myData.childFactors; + // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor + myData.bayesTreeNode->setEliminationResult(eliminationResult); - // 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; - } - } + // 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)); - // Do dense elimination step - std::pair, boost::shared_ptr > eliminationResult = - eliminationFunction(gatheredFactors, node->orderedFrontalKeys); - - // 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; - } - }; + // 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 +/* ************************************************************************* */ +template +void ClusterTree::Cluster::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::cout << s << " (" << problemSize_ << ")"; + PrintKeyVector(orderedFrontalKeys); +} + +/* ************************************************************************* */ +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< + BAYESTREE, GRAPH>::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_); { - std::cout << s << " (" << problemSize_ << ")" ; - PrintKeyVector(orderedFrontalKeys); - } - - /* ************************************************************************* */ - 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_); - { - TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP - treeTraversal::DepthFirstForestParallel(*this, rootsContainer, + TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP + 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); } + // 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< + FactorGraphType>(); + 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); +} + } From b95bc712f41f6c1dff21df02f431b8fedc2f9d02 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 13:18:28 -0700 Subject: [PATCH 06/11] Kill stray timing --- gtsam/inference/JunctionTree-inst.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index de349ddc8..2df7aa930 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -102,8 +102,6 @@ struct ConstructorTraversalData { const size_t nrChildren = node->children.size(); assert(childConditionals.size() == nrChildren); - gttic(merge_children); - // decide which children to merge, as index into children std::vector merge(nrChildren, false); size_t myNrFrontals = 1; From d34c1808a8d1f4b0dceea6a4b490ab367f89a96d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 13:18:43 -0700 Subject: [PATCH 07/11] Moved to inst file --- gtsam/inference/ClusterTree-inst.h | 56 +++++++++++++++++++++++++++++ gtsam/inference/ClusterTree.h | 58 ++---------------------------- 2 files changed, 59 insertions(+), 55 deletions(-) diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index b76430c20..bbe878907 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -123,6 +123,62 @@ void ClusterTree::Cluster::print(const std::string& s, PrintKeyVector(orderedFrontalKeys); } +/* ************************************************************************* */ +template +void ClusterTree::Cluster::mergeChildren( + const std::vector& merge) { + gttic(Cluster::mergeChildren); + size_t nrChildren = children.size(); + + // Count how many keys, factors and children we'll end up with + size_t nrKeys = orderedFrontalKeys.size(); + size_t nrFactors = factors.size(); + size_t nrNewChildren = 0; + // Loop over children + for (size_t i = 0; i < nrChildren; ++i) { + if (merge[i]) { + // Get a reference to the i, adjusting the index to account for children + // previously merged and removed from the i list. + sharedNode child = children[i]; + nrKeys += child->orderedFrontalKeys.size(); + nrFactors += child->factors.size(); + nrNewChildren += child->children.size(); + } else { + nrNewChildren += 1; // we keep the child + } + } + + // now reserve space, and really merge + orderedFrontalKeys.reserve(nrKeys); + factors.reserve(nrFactors); + typename Node::Children newChildren; + newChildren.reserve(nrNewChildren); + // Loop over newChildren + for (size_t i = 0; i < nrChildren; ++i) { + // Check if we should merge the i^th child + sharedNode child = children[i]; + if (merge[i]) { + // Get a reference to the i, adjusting the index to account for newChildren + // previously merged and removed from the i list. + // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. + orderedFrontalKeys.insert(orderedFrontalKeys.end(), + child->orderedFrontalKeys.rbegin(), child->orderedFrontalKeys.rend()); + // Merge keys, factors, and children. + factors.insert(factors.end(), child->factors.begin(), + child->factors.end()); + newChildren.insert(newChildren.end(), child->children.begin(), + child->children.end()); + // Increment problem size + problemSize_ = std::max(problemSize_, child->problemSize_); + // Increment number of frontal variables + } else { + newChildren.push_back(child); // we keep the child + } + } + children = newChildren; + std::reverse(orderedFrontalKeys.begin(), orderedFrontalKeys.end()); +} + /* ************************************************************************* */ template void ClusterTree::print(const std::string& s, diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 0b98d3e36..b00532d22 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -55,64 +55,12 @@ public: return problemSize_; } - /** print this node */ + /// print this node void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - void mergeChildren(const std::vector& merge) { - gttic(merge_children); - size_t nrChildren = children.size(); - - // Count how many keys, factors and children we'll end up with - size_t nrKeys = this->orderedFrontalKeys.size(); - size_t nrFactors = this->factors.size(); - size_t nrNewChildren = 0; - // Loop over children - for (size_t i = 0; i < nrChildren; ++i) { - if (merge[i]) { - // Get a reference to the i, adjusting the index to account for children - // previously merged and removed from the i list. - sharedNode child = this->children[i]; - nrKeys += child->orderedFrontalKeys.size(); - nrFactors += child->factors.size(); - nrNewChildren += child->children.size(); - } else { - nrNewChildren += 1; // we keep the child - } - } - - // now reserve space, and really merge - this->orderedFrontalKeys.reserve(nrKeys); - this->factors.reserve(nrFactors); - typename Node::Children newChildren; - newChildren.reserve(nrNewChildren); - // Loop over newChildren - for (size_t i = 0; i < nrChildren; ++i) { - // Check if we should merge the i^th child - sharedNode child = this->children[i]; - if (merge[i]) { - // Get a reference to the i, adjusting the index to account for newChildren - // previously merged and removed from the i list. - // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. - this->orderedFrontalKeys.insert(this->orderedFrontalKeys.end(), - child->orderedFrontalKeys.rbegin(), - child->orderedFrontalKeys.rend()); - // Merge keys, factors, and children. - this->factors.insert(this->factors.end(), child->factors.begin(), - child->factors.end()); - newChildren.insert(newChildren.end(), child->children.begin(), - child->children.end()); - // Increment problem size - problemSize_ = std::max(problemSize_, child->problemSize_); - // Increment number of frontal variables - } else { - newChildren.push_back(child); // we keep the child - } - } - this->children = newChildren; - std::reverse(this->orderedFrontalKeys.begin(), - this->orderedFrontalKeys.end()); - } + /// Merge all children for which bit is set into this node + void mergeChildren(const std::vector& merge); }; typedef boost::shared_ptr sharedCluster; ///< Shared pointer to Cluster From e2d49922d22ced379efe31fbaf4a7c0601bd56f6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 13:39:55 -0700 Subject: [PATCH 08/11] Switch to list - made code be container-agnostic --- gtsam/inference/ClusterTree-inst.h | 19 +++++++------------ gtsam/inference/ClusterTree.h | 2 +- gtsam/inference/JunctionTree-inst.h | 11 +++++++---- tests/testGaussianJunctionTreeB.cpp | 10 +++++----- 4 files changed, 20 insertions(+), 22 deletions(-) diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index bbe878907..d777fcfe7 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -128,38 +128,32 @@ template void ClusterTree::Cluster::mergeChildren( const std::vector& merge) { gttic(Cluster::mergeChildren); - size_t nrChildren = children.size(); // Count how many keys, factors and children we'll end up with size_t nrKeys = orderedFrontalKeys.size(); size_t nrFactors = factors.size(); size_t nrNewChildren = 0; // Loop over children - for (size_t i = 0; i < nrChildren; ++i) { + size_t i = 0; + BOOST_FOREACH(const sharedNode& child, children) { if (merge[i]) { - // Get a reference to the i, adjusting the index to account for children - // previously merged and removed from the i list. - sharedNode child = children[i]; nrKeys += child->orderedFrontalKeys.size(); nrFactors += child->factors.size(); nrNewChildren += child->children.size(); } else { nrNewChildren += 1; // we keep the child } + ++i; } // now reserve space, and really merge orderedFrontalKeys.reserve(nrKeys); factors.reserve(nrFactors); typename Node::Children newChildren; - newChildren.reserve(nrNewChildren); - // Loop over newChildren - for (size_t i = 0; i < nrChildren; ++i) { - // Check if we should merge the i^th child - sharedNode child = children[i]; + // newChildren.reserve(nrNewChildren); + i = 0; + BOOST_FOREACH(const sharedNode& child, children) { if (merge[i]) { - // Get a reference to the i, adjusting the index to account for newChildren - // previously merged and removed from the i list. // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. orderedFrontalKeys.insert(orderedFrontalKeys.end(), child->orderedFrontalKeys.rbegin(), child->orderedFrontalKeys.rend()); @@ -174,6 +168,7 @@ void ClusterTree::Cluster::mergeChildren( } else { newChildren.push_back(child); // we keep the child } + ++i; } children = newChildren; std::reverse(orderedFrontalKeys.begin(), orderedFrontalKeys.end()); diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index b00532d22..41232e1a1 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -37,7 +37,7 @@ public: struct Cluster { typedef Ordering Keys; typedef FastVector Factors; - typedef FastVector > Children; + typedef FastList > Children; Cluster() { } diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 2df7aa930..352a8dded 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -104,15 +104,15 @@ struct ConstructorTraversalData { // decide which children to merge, as index into children std::vector merge(nrChildren, false); - size_t myNrFrontals = 1; - for (size_t i = 0; i < nrChildren; ++i) { + size_t myNrFrontals = 1, i = 0; + BOOST_FOREACH(const sharedNode& child, node->children) { // Check if we should merge the i^th child if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { - sharedNode child = node->children[i]; // Increment number of frontal variables myNrFrontals += child->orderedFrontalKeys.size(); merge[i] = true; } + ++i; } // now really merge @@ -145,7 +145,10 @@ JunctionTree::JunctionTree( Data::ConstructorTraversalVisitorPostAlg2); // Assign roots from the dummy node - Base::roots_ = rootData.myJTNode->children; + typedef typename JunctionTree::Node Node; + const typename Node::Children& children = rootData.myJTNode->children; + Base::roots_.reserve(children.size()); + Base::roots_.insert(Base::roots_.begin(), children.begin(), children.end()); // Transfer remaining factors from elimination tree Base::remainingFactors_ = eliminationTree.remainingFactors(); diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index c5401512b..18d249894 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -103,15 +103,15 @@ TEST( GaussianJunctionTreeB, constructor2 ) GaussianJunctionTree::sharedNode x324 = actual.roots().front(); LONGS_EQUAL(2, x324->children.size()); #if defined(__APPLE__) // tie-breaking seems different :-( - GaussianJunctionTree::sharedNode x1 = x324->children[0]; - GaussianJunctionTree::sharedNode x56 = x324->children[1]; + GaussianJunctionTree::sharedNode x1 = x324->children.front(); + GaussianJunctionTree::sharedNode x56 = x324->children.back(); #else - GaussianJunctionTree::sharedNode x1 = x324->children[1]; - GaussianJunctionTree::sharedNode x56 = x324->children[0]; + GaussianJunctionTree::sharedNode x1 = x324->children.back(); + GaussianJunctionTree::sharedNode x56 = x324->children.front(); #endif LONGS_EQUAL(0, x1->children.size()); LONGS_EQUAL(1, x56->children.size()); - GaussianJunctionTree::sharedNode x7 = x56->children[0]; + GaussianJunctionTree::sharedNode x7 = x56->children.front(); LONGS_EQUAL(0, x7->children.size()); EXPECT(assert_equal(o324, x324->orderedFrontalKeys)); From 22b7d8276aa80a56a19b6ac3e719175402d6f700 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 13:56:48 -0700 Subject: [PATCH 09/11] Formatting --- gtsam/base/treeTraversal-inst.h | 395 ++++++++++++++++---------------- 1 file changed, 200 insertions(+), 195 deletions(-) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 3edd7d076..669186e3f 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -1,19 +1,19 @@ /* ---------------------------------------------------------------------------- -* 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) + * 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 + * See LICENSE for the license information -* -------------------------------------------------------------------------- */ + * -------------------------------------------------------------------------- */ /** -* @file treeTraversal-inst.h -* @author Richard Roberts -* @date April 9, 2013 -*/ + * @file treeTraversal-inst.h + * @author Richard Roberts + * @date April 9, 2013 + */ #pragma once #include @@ -33,192 +33,197 @@ namespace gtsam { - /** Internal functions used for traversing trees */ - namespace treeTraversal { +/** Internal functions used for traversing trees */ +namespace treeTraversal { - /* ************************************************************************* */ - namespace { - // Internal node used in DFS preorder stack - template - struct TraversalNode { - bool expanded; - const boost::shared_ptr& treeNode; - DATA& parentData; - typename FastList::iterator dataPointer; - TraversalNode(const boost::shared_ptr& _treeNode, DATA& _parentData) : - expanded(false), treeNode(_treeNode), parentData(_parentData) {} - }; - - // Do nothing - default argument for post-visitor for tree traversal - struct no_op { - template - void operator()(const boost::shared_ptr& node, const DATA& data) {} - }; - - } - - /** Traverse a forest depth-first with pre-order and post-order visits. - * @param forest The forest of trees to traverse. The method \c forest.roots() should exist - * and return a collection of (shared) pointers to \c FOREST::Node. - * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before - * visiting its children, and will be passed, by reference, the \c DATA object returned - * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object - * to pass to the children. The returned \c DATA object will be copy-constructed only - * upon returning to store internally, thus may be modified by visiting the children. - * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. - * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting - * its children, and will be passed, by reference, the \c DATA object returned by the - * call to \c visitorPre (the \c DATA object may be modified by visiting the children). - * @param rootData The data to pass by reference to \c visitorPre when it is called on each - * root node. */ - template - void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost) - { - // Typedefs - typedef typename FOREST::Node Node; - typedef boost::shared_ptr sharedNode; - - // Depth first traversal stack - typedef TraversalNode TraversalNode; - typedef FastList Stack; - Stack stack; - FastList dataList; // List to store node data as it is returned from the pre-order visitor - - // Add roots to stack (insert such that they are visited and processed in order - { - typename Stack::iterator insertLocation = stack.begin(); - BOOST_FOREACH(const sharedNode& root, forest.roots()) - stack.insert(insertLocation, TraversalNode(root, rootData)); - } - - // Traverse - while(!stack.empty()) - { - // Get next node - TraversalNode& node = stack.front(); - - if(node.expanded) { - // If already expanded, then the data stored in the node is no longer needed, so visit - // then delete it. - (void) visitorPost(node.treeNode, *node.dataPointer); - dataList.erase(node.dataPointer); - stack.pop_front(); - } else { - // If not already visited, visit the node and add its children (use reverse iterators so - // children are processed in the order they appear) - node.dataPointer = dataList.insert(dataList.end(), visitorPre(node.treeNode, node.parentData)); - typename Stack::iterator insertLocation = stack.begin(); - BOOST_FOREACH(const sharedNode& child, node.treeNode->children) - stack.insert(insertLocation, TraversalNode(child, *node.dataPointer)); - node.expanded = true; - } - } - assert(dataList.empty()); - } - - /** Traverse a forest depth-first, with a pre-order visit but no post-order visit. - * @param forest The forest of trees to traverse. The method \c forest.roots() should exist - * and return a collection of (shared) pointers to \c FOREST::Node. - * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before - * visiting its children, and will be passed, by reference, the \c DATA object returned - * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object - * to pass to the children. The returned \c DATA object will be copy-constructed only - * upon returning to store internally, thus may be modified by visiting the children. - * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. - * @param rootData The data to pass by reference to \c visitorPre when it is called on each - * root node. */ - template - void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) - { - no_op visitorPost; - DepthFirstForest(forest, rootData, visitorPre, visitorPost); - } - - /** Traverse a forest depth-first with pre-order and post-order visits. - * @param forest The forest of trees to traverse. The method \c forest.roots() should exist - * and return a collection of (shared) pointers to \c FOREST::Node. - * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before - * visiting its children, and will be passed, by reference, the \c DATA object returned - * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object - * to pass to the children. The returned \c DATA object will be copy-constructed only - * upon returning to store internally, thus may be modified by visiting the children. - * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. - * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting - * its children, and will be passed, by reference, the \c DATA object returned by the - * call to \c visitorPre (the \c DATA object may be modified by visiting the children). - * @param rootData The data to pass by reference to \c visitorPre when it is called on each - * root node. */ - template - void DepthFirstForestParallel(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold = 10) - { -#ifdef GTSAM_USE_TBB - // Typedefs - typedef typename FOREST::Node Node; - typedef boost::shared_ptr sharedNode; - - tbb::task::spawn_root_and_wait(internal::CreateRootTask( - forest.roots(), rootData, visitorPre, visitorPost, problemSizeThreshold)); -#else - DepthFirstForest(forest, rootData, visitorPre, visitorPost); -#endif - } - - - /* ************************************************************************* */ - /** Traversal function for CloneForest */ - namespace { - template - boost::shared_ptr - CloneForestVisitorPre(const boost::shared_ptr& node, const boost::shared_ptr& parentPointer) - { - // Clone the current node and add it to its cloned parent - boost::shared_ptr clone = boost::make_shared(*node); - clone->children.clear(); - parentPointer->children.push_back(clone); - return clone; - } - } - - /** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child - * pointers for a clone of the original tree. - * @param forest The forest of trees to clone. The method \c forest.roots() should exist and - * return a collection of shared pointers to \c FOREST::Node. - * @return The new collection of roots. */ - template - FastVector > CloneForest(const FOREST& forest) - { - typedef typename FOREST::Node Node; - boost::shared_ptr rootContainer = boost::make_shared(); - DepthFirstForest(forest, rootContainer, CloneForestVisitorPre); - return FastVector >(rootContainer->children.begin(), rootContainer->children.end()); - } - - - /* ************************************************************************* */ - /** Traversal function for PrintForest */ - namespace { - struct PrintForestVisitorPre - { - const KeyFormatter& formatter; - PrintForestVisitorPre(const KeyFormatter& formatter) : formatter(formatter) {} - template std::string operator()(const boost::shared_ptr& node, const std::string& parentString) - { - // Print the current node - node->print(parentString + "-", formatter); - // Increment the indentation - return parentString + "| "; - } - }; - } - - /** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter. - * To print each node, this function calls the \c print function of the tree nodes. */ - template - void PrintForest(const FOREST& forest, std::string str, const KeyFormatter& keyFormatter) { - PrintForestVisitorPre visitor(keyFormatter); - DepthFirstForest(forest, str, visitor); - } +/* ************************************************************************* */ +namespace { +// Internal node used in DFS preorder stack +template +struct TraversalNode { + bool expanded; + const boost::shared_ptr& treeNode; + DATA& parentData; + typename FastList::iterator dataPointer; + TraversalNode(const boost::shared_ptr& _treeNode, DATA& _parentData) : + expanded(false), treeNode(_treeNode), parentData(_parentData) { } +}; + +// Do nothing - default argument for post-visitor for tree traversal +struct no_op { + template + void operator()(const boost::shared_ptr& node, const DATA& data) { + } +}; + +} + +/** Traverse a forest depth-first with pre-order and post-order visits. + * @param forest The forest of trees to traverse. The method \c forest.roots() should exist + * and return a collection of (shared) pointers to \c FOREST::Node. + * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before + * visiting its children, and will be passed, by reference, the \c DATA object returned + * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object + * to pass to the children. The returned \c DATA object will be copy-constructed only + * upon returning to store internally, thus may be modified by visiting the children. + * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. + * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting + * its children, and will be passed, by reference, the \c DATA object returned by the + * call to \c visitorPre (the \c DATA object may be modified by visiting the children). + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. */ +template +void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, + VISITOR_POST& visitorPost) { + // Typedefs + typedef typename FOREST::Node Node; + typedef boost::shared_ptr sharedNode; + + // Depth first traversal stack + typedef TraversalNode TraversalNode; + typedef FastList Stack; + Stack stack; + FastList dataList; // List to store node data as it is returned from the pre-order visitor + + // Add roots to stack (insert such that they are visited and processed in order + { + typename Stack::iterator insertLocation = stack.begin(); + BOOST_FOREACH(const sharedNode& root, forest.roots()) + stack.insert(insertLocation, TraversalNode(root, rootData)); + } + + // Traverse + while (!stack.empty()) { + // Get next node + TraversalNode& node = stack.front(); + + if (node.expanded) { + // If already expanded, then the data stored in the node is no longer needed, so visit + // then delete it. + (void) visitorPost(node.treeNode, *node.dataPointer); + dataList.erase(node.dataPointer); + stack.pop_front(); + } else { + // If not already visited, visit the node and add its children (use reverse iterators so + // children are processed in the order they appear) + node.dataPointer = dataList.insert(dataList.end(), + visitorPre(node.treeNode, node.parentData)); + typename Stack::iterator insertLocation = stack.begin(); + BOOST_FOREACH(const sharedNode& child, node.treeNode->children) + stack.insert(insertLocation, TraversalNode(child, *node.dataPointer)); + node.expanded = true; + } + } + assert(dataList.empty()); +} + +/** Traverse a forest depth-first, with a pre-order visit but no post-order visit. + * @param forest The forest of trees to traverse. The method \c forest.roots() should exist + * and return a collection of (shared) pointers to \c FOREST::Node. + * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before + * visiting its children, and will be passed, by reference, the \c DATA object returned + * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object + * to pass to the children. The returned \c DATA object will be copy-constructed only + * upon returning to store internally, thus may be modified by visiting the children. + * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. */ +template +void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) { + no_op visitorPost; + DepthFirstForest(forest, rootData, visitorPre, visitorPost); +} + +/** Traverse a forest depth-first with pre-order and post-order visits. + * @param forest The forest of trees to traverse. The method \c forest.roots() should exist + * and return a collection of (shared) pointers to \c FOREST::Node. + * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before + * visiting its children, and will be passed, by reference, the \c DATA object returned + * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object + * to pass to the children. The returned \c DATA object will be copy-constructed only + * upon returning to store internally, thus may be modified by visiting the children. + * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. + * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting + * its children, and will be passed, by reference, the \c DATA object returned by the + * call to \c visitorPre (the \c DATA object may be modified by visiting the children). + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. */ +template +void DepthFirstForestParallel(FOREST& forest, DATA& rootData, + VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, + int problemSizeThreshold = 10) { +#ifdef GTSAM_USE_TBB + // Typedefs + typedef typename FOREST::Node Node; + typedef boost::shared_ptr sharedNode; + + tbb::task::spawn_root_and_wait( + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold)); +#else + DepthFirstForest(forest, rootData, visitorPre, visitorPost); +#endif +} + +/* ************************************************************************* */ +/** Traversal function for CloneForest */ +namespace { +template +boost::shared_ptr CloneForestVisitorPre( + const boost::shared_ptr& node, + const boost::shared_ptr& parentPointer) { + // Clone the current node and add it to its cloned parent + boost::shared_ptr clone = boost::make_shared(*node); + clone->children.clear(); + parentPointer->children.push_back(clone); + return clone; +} +} + +/** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child + * pointers for a clone of the original tree. + * @param forest The forest of trees to clone. The method \c forest.roots() should exist and + * return a collection of shared pointers to \c FOREST::Node. + * @return The new collection of roots. */ +template +FastVector > CloneForest( + const FOREST& forest) { + typedef typename FOREST::Node Node; + boost::shared_ptr rootContainer = boost::make_shared(); + DepthFirstForest(forest, rootContainer, CloneForestVisitorPre); + return FastVector >(rootContainer->children.begin(), + rootContainer->children.end()); +} + +/* ************************************************************************* */ +/** Traversal function for PrintForest */ +namespace { +struct PrintForestVisitorPre { + const KeyFormatter& formatter; + PrintForestVisitorPre(const KeyFormatter& formatter) : + formatter(formatter) { + } + template std::string operator()( + const boost::shared_ptr& node, const std::string& parentString) { + // Print the current node + node->print(parentString + "-", formatter); + // Increment the indentation + return parentString + "| "; + } +}; +} + +/** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter. + * To print each node, this function calls the \c print function of the tree nodes. */ +template +void PrintForest(const FOREST& forest, std::string str, + const KeyFormatter& keyFormatter) { + PrintForestVisitorPre visitor(keyFormatter); + DepthFirstForest(forest, str, visitor); +} +} } From a35adc127cc52c09048e0cd887bfab35a562001e Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 14:18:24 -0700 Subject: [PATCH 10/11] Reverted back to vector --- gtsam/inference/ClusterTree.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 41232e1a1..b00532d22 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -37,7 +37,7 @@ public: struct Cluster { typedef Ordering Keys; typedef FastVector Factors; - typedef FastList > Children; + typedef FastVector > Children; Cluster() { } From ef829c333e114646132bcf664ee785fd2dfbcbea Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 14:19:01 -0700 Subject: [PATCH 11/11] Refactored elimination traversal a tiny bit --- gtsam/inference/ClusterTree-inst.h | 156 ++++++++++++++--------------- 1 file changed, 77 insertions(+), 79 deletions(-) diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index d777fcfe7..2cf1f5c58 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -17,23 +17,29 @@ #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 { + // 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; + EliminationData* const parentData; size_t myIndexInParent; - FastVector childFactors; - boost::shared_ptr bayesTreeNode; + FastVector childFactors; + boost::shared_ptr bayesTreeNode; EliminationData(EliminationData* _parentData, size_t nChildren) : - parentData(_parentData), bayesTreeNode( - boost::make_shared()) { + parentData(_parentData), bayesTreeNode(boost::make_shared()) { if (parentData) { myIndexInParent = parentData->childFactors.size(); - parentData->childFactors.push_back(typename CLUSTERTREE::sharedFactor()); + parentData->childFactors.push_back(sharedFactor()); } else { myIndexInParent = 0; } @@ -44,76 +50,67 @@ struct EliminationData { 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) { + // Elimination pre-order visitor - creates the EliminationData structure for the visited node. + static EliminationData EliminationPreOrderVisitor( + const typename CLUSTERTREE::sharedNode& node, + EliminationData& parentData) { + assert(node); + EliminationData myData(&parentData, node->children.size()); + myData.bayesTreeNode->problemSize_ = node->problemSize(); + return myData; } - 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; - } + // 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. + 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) { + assert(node); - // Do dense elimination step - std::pair, boost::shared_ptr > eliminationResult = - eliminationFunction(gatheredFactors, node->orderedFrontalKeys); + // Gather factors + FactorGraphType gatheredFactors; + gatheredFactors.reserve(node->factors.size() + node->children.size()); + gatheredFactors += node->factors; + gatheredFactors += myData.childFactors; - // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor - myData.bayesTreeNode->setEliminationResult(eliminationResult); + // 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; + } + } - // 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)); + // Do dense elimination step + std::pair, + boost::shared_ptr > eliminationResult = + eliminationFunction(gatheredFactors, node->orderedFrontalKeys); - // Store remaining factor in parent's gathered factors - if (!eliminationResult.second->empty()) - myData.parentData->childFactors[myData.myIndexInParent] = - eliminationResult.second; - } + // 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 @@ -150,7 +147,7 @@ void ClusterTree::Cluster::mergeChildren( orderedFrontalKeys.reserve(nrKeys); factors.reserve(nrFactors); typename Node::Children newChildren; - // newChildren.reserve(nrNewChildren); + newChildren.reserve(nrNewChildren); i = 0; BOOST_FOREACH(const sharedNode& child, children) { if (merge[i]) { @@ -204,12 +201,14 @@ std::pair, boost::shared_ptr > ClusterTree< // 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_); + typedef EliminationData Data; + Data rootsContainer(0, roots_.size()); + typename Data::EliminationPostOrderVisitor visitorPost(function, + result->nodes_); { TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP treeTraversal::DepthFirstForestParallel(*this, rootsContainer, - eliminationPreOrderVisitor, visitorPost, 10); + Data::EliminationPreOrderVisitor, visitorPost, 10); } // Create BayesTree from roots stored in the dummy BayesTree node. @@ -218,18 +217,17 @@ std::pair, boost::shared_ptr > ClusterTree< rootsContainer.bayesTreeNode->children.end()); // Add remaining factors that were not involved with eliminated variables - boost::shared_ptr allRemainingFactors = boost::make_shared< + boost::shared_ptr remaining = boost::make_shared< FactorGraphType>(); - allRemainingFactors->reserve( + remaining->reserve( remainingFactors_.size() + rootsContainer.childFactors.size()); - allRemainingFactors->push_back(remainingFactors_.begin(), - remainingFactors_.end()); - BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) + remaining->push_back(remainingFactors_.begin(), remainingFactors_.end()); + BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) { if (factor) - allRemainingFactors->push_back(factor); - + remaining->push_back(factor); + } // Return result - return std::make_pair(result, allRemainingFactors); + return std::make_pair(result, remaining); } }