diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 7725742cf..422c200a4 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -31,9 +31,7 @@ template class EliminatableClusterTree; struct HybridConstructorTraversalData { - typedef - typename JunctionTree::Node - Node; + typedef HybridJunctionTree::Node Node; typedef typename JunctionTree::sharedNode sharedNode; @@ -62,6 +60,7 @@ struct HybridConstructorTraversalData { data.junctionTreeNode = boost::make_shared(node->key, node->factors); parentData.junctionTreeNode->addChild(data.junctionTreeNode); + // Add all the discrete keys in the hybrid factors to the current data for (HybridFactor::shared_ptr& f : node->factors) { for (auto& k : f->discreteKeys()) { data.discreteKeys.insert(k.first); @@ -72,8 +71,8 @@ struct HybridConstructorTraversalData { } // Post-order visitor function - static void ConstructorTraversalVisitorPostAlg2( - const boost::shared_ptr& ETreeNode, + static void ConstructorTraversalVisitorPost( + const boost::shared_ptr& node, const HybridConstructorTraversalData& data) { // In this post-order visitor, we combine the symbolic elimination results // from the elimination tree children and symbolically eliminate the current @@ -86,15 +85,15 @@ struct HybridConstructorTraversalData { // Do symbolic elimination for this node SymbolicFactors symbolicFactors; - symbolicFactors.reserve(ETreeNode->factors.size() + + symbolicFactors.reserve(node->factors.size() + data.childSymbolicFactors.size()); // Add ETree node factors - symbolicFactors += ETreeNode->factors; + symbolicFactors += node->factors; // Add symbolic factors passed up from children symbolicFactors += data.childSymbolicFactors; Ordering keyAsOrdering; - keyAsOrdering.push_back(ETreeNode->key); + keyAsOrdering.push_back(node->key); SymbolicConditional::shared_ptr conditional; SymbolicFactor::shared_ptr separatorFactor; boost::tie(conditional, separatorFactor) = @@ -105,19 +104,19 @@ struct HybridConstructorTraversalData { data.parentData->childSymbolicFactors.push_back(separatorFactor); data.parentData->discreteKeys.merge(data.discreteKeys); - sharedNode node = data.junctionTreeNode; + sharedNode jt_node = data.junctionTreeNode; const FastVector& childConditionals = data.childSymbolicConditionals; - node->problemSize_ = (int)(conditional->size() * symbolicFactors.size()); + jt_node->problemSize_ = (int)(conditional->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. const size_t nrParents = conditional->nrParents(); - const size_t nrChildren = node->nrChildren(); + const size_t nrChildren = jt_node->nrChildren(); assert(childConditionals.size() == nrChildren); // decide which children to merge, as index into children - std::vector nrChildrenFrontals = node->nrFrontalsOfChildren(); + std::vector nrChildrenFrontals = jt_node->nrFrontalsOfChildren(); std::vector merge(nrChildren, false); size_t nrFrontals = 1; for (size_t i = 0; i < nrChildren; i++) { @@ -137,7 +136,7 @@ struct HybridConstructorTraversalData { } // now really merge - node->mergeChildren(merge); + jt_node->mergeChildren(merge); } }; @@ -161,7 +160,7 @@ HybridJunctionTree::HybridJunctionTree( // the junction tree roots treeTraversal::DepthFirstForest(eliminationTree, rootData, Data::ConstructorTraversalVisitorPre, - Data::ConstructorTraversalVisitorPostAlg2); + Data::ConstructorTraversalVisitorPost); // Assign roots from the dummy node this->addChildrenAsRoots(rootData.junctionTreeNode);