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