From e16460358fc877a475bf711432ad2a43d63fae8d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 2 Sep 2022 15:01:59 -0400 Subject: [PATCH 1/2] add multifrontal test to reproduce issue --- gtsam/hybrid/tests/testHybridBayesNet.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index bf9385bc48..0c15ee83da 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include "Switching.h" @@ -149,6 +150,25 @@ TEST(HybridBayesNet, Optimize) { EXPECT(assert_equal(expectedValues, delta.continuous(), 1e-5)); } +/* ****************************************************************************/ +// Test bayes net multifrontal optimize +TEST(HybridBayesNet, OptimizeMultifrontal) { + Switching s(4); + + Ordering hybridOrdering = s.linearizedFactorGraph.getHybridOrdering(); + HybridBayesTree::shared_ptr hybridBayesTree = + s.linearizedFactorGraph.eliminateMultifrontal(hybridOrdering); + HybridValues delta = hybridBayesTree->optimize(); + + VectorValues expectedValues; + expectedValues.insert(X(1), -0.999904 * Vector1::Ones()); + expectedValues.insert(X(2), -0.99029 * Vector1::Ones()); + expectedValues.insert(X(3), -1.00971 * Vector1::Ones()); + expectedValues.insert(X(4), -1.0001 * Vector1::Ones()); + + EXPECT(assert_equal(expectedValues, delta.continuous(), 1e-5)); +} + /* ****************************************************************************/ // Test HybridBayesNet serialization. TEST(HybridBayesNet, Serialization) { From 773af1ed446fb389a604c80e29783c21403fc113 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 2 Sep 2022 15:04:28 -0400 Subject: [PATCH 2/2] refactor HybridBayesTree::optimize --- gtsam/base/treeTraversal-inst.h | 4 +- gtsam/hybrid/HybridBayesTree.cpp | 174 +++++++++++++++++-------------- 2 files changed, 96 insertions(+), 82 deletions(-) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 30cec3b9a4..be45a248ef 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -221,6 +221,6 @@ void PrintForest(const FOREST& forest, std::string str, PrintForestVisitorPre visitor(keyFormatter); DepthFirstForest(forest, str, visitor); } -} +} // namespace treeTraversal -} +} // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 49ae0bd877..3fa4d6268b 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -24,6 +24,7 @@ #include #include #include +#include namespace gtsam { @@ -39,95 +40,108 @@ bool HybridBayesTree::equals(const This& other, double tol) const { /* ************************************************************************* */ HybridValues HybridBayesTree::optimize() const { - HybridBayesNet hbn; DiscreteBayesNet dbn; - - KeyVector added_keys; - - // Iterate over all the nodes in the BayesTree - for (auto&& node : nodes()) { - // Check if conditional being added is already in the Bayes net. - if (std::find(added_keys.begin(), added_keys.end(), node.first) == - added_keys.end()) { - // Access the clique and get the underlying hybrid conditional - HybridBayesTreeClique::shared_ptr clique = node.second; - HybridConditional::shared_ptr conditional = clique->conditional(); - - // Record the key being added - added_keys.insert(added_keys.end(), conditional->frontals().begin(), - conditional->frontals().end()); - - if (conditional->isDiscrete()) { - // If discrete, we use it to compute the MPE - dbn.push_back(conditional->asDiscreteConditional()); - - } else { - // Else conditional is hybrid or continuous-only, - // so we directly add it to the Hybrid Bayes net. - hbn.push_back(conditional); - } - } + DiscreteValues mpe; + + auto root = roots_.at(0); + // Access the clique and get the underlying hybrid conditional + HybridConditional::shared_ptr root_conditional = root->conditional(); + + // The root should be discrete only, we compute the MPE + if (root_conditional->isDiscrete()) { + dbn.push_back(root_conditional->asDiscreteConditional()); + mpe = DiscreteFactorGraph(dbn).optimize(); + } else { + throw std::runtime_error( + "HybridBayesTree root is not discrete-only. Please check elimination " + "ordering or use continuous factor graph."); } - // Get the MPE - DiscreteValues mpe = DiscreteFactorGraph(dbn).optimize(); - // Given the MPE, compute the optimal continuous values. - GaussianBayesNet gbn = hbn.choose(mpe); - - // If TBB is enabled, the bayes net order gets reversed, - // so we pre-reverse it -#ifdef GTSAM_USE_TBB - auto reversed = boost::adaptors::reverse(gbn); - gbn = GaussianBayesNet(reversed.begin(), reversed.end()); -#endif - - return HybridValues(mpe, gbn.optimize()); + + VectorValues values = optimize(mpe); + return HybridValues(mpe, values); } /* ************************************************************************* */ -VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { - GaussianBayesNet gbn; - - KeyVector added_keys; - - // Iterate over all the nodes in the BayesTree - for (auto&& node : nodes()) { - // Check if conditional being added is already in the Bayes net. - if (std::find(added_keys.begin(), added_keys.end(), node.first) == - added_keys.end()) { - // Access the clique and get the underlying hybrid conditional - HybridBayesTreeClique::shared_ptr clique = node.second; - HybridConditional::shared_ptr conditional = clique->conditional(); - - // Record the key being added - added_keys.insert(added_keys.end(), conditional->frontals().begin(), - conditional->frontals().end()); - - // If conditional is hybrid (and not discrete-only), we get the Gaussian - // Conditional corresponding to the assignment and add it to the Gaussian - // Bayes Net. - if (conditional->isHybrid()) { - auto gm = conditional->asMixture(); - GaussianConditional::shared_ptr gaussian_conditional = - (*gm)(assignment); - - gbn.push_back(gaussian_conditional); - - } else if (conditional->isContinuous()) { - // If conditional is Gaussian, we simply add it to the Bayes net. - gbn.push_back(conditional->asGaussian()); - } +/** + * @brief Helper class for Depth First Forest traversal on the HybridBayesTree. + * + * When traversing the tree, the pre-order visitor will receive an instance of + * this class with the parent clique data. + */ +struct HybridAssignmentData { + const DiscreteValues assignment_; + GaussianBayesTree::sharedNode parentClique_; + // The gaussian bayes tree that will be recursively created. + GaussianBayesTree* gaussianbayesTree_; + + /** + * @brief Construct a new Hybrid Assignment Data object. + * + * @param assignment The MPE assignment for the optimal Gaussian cliques. + * @param parentClique The clique from the parent node of the current node. + * @param gbt The Gaussian Bayes Tree being generated during tree traversal. + */ + HybridAssignmentData(const DiscreteValues& assignment, + const GaussianBayesTree::sharedNode& parentClique, + GaussianBayesTree* gbt) + : assignment_(assignment), + parentClique_(parentClique), + gaussianbayesTree_(gbt) {} + + /** + * @brief A function used during tree traversal that operators on each node + * before visiting the node's children. + * + * @param node The current node being visited. + * @param parentData The HybridAssignmentData from the parent node. + * @return HybridAssignmentData + */ + static HybridAssignmentData AssignmentPreOrderVisitor( + const HybridBayesTree::sharedNode& node, + HybridAssignmentData& parentData) { + // Extract the gaussian conditional from the Hybrid clique + HybridConditional::shared_ptr hybrid_conditional = node->conditional(); + GaussianConditional::shared_ptr conditional; + if (hybrid_conditional->isHybrid()) { + conditional = (*hybrid_conditional->asMixture())(parentData.assignment_); + } else if (hybrid_conditional->isContinuous()) { + conditional = hybrid_conditional->asGaussian(); + } else { + // Discrete only conditional, so we set to empty gaussian conditional + conditional = boost::make_shared(); } + + // Create the GaussianClique for the current node + auto clique = boost::make_shared(conditional); + // Add the current clique to the GaussianBayesTree. + parentData.gaussianbayesTree_->addClique(clique, parentData.parentClique_); + + // Create new HybridAssignmentData where the current node is the parent + // This will be passed down to the children nodes + HybridAssignmentData data(parentData.assignment_, clique, + parentData.gaussianbayesTree_); + return data; + } +}; + +/* ************************************************************************* + */ +VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { + GaussianBayesTree gbt; + HybridAssignmentData rootData(assignment, 0, &gbt); + { + treeTraversal::no_op visitorPost; + // Limits OpenMP threads since we're mixing TBB and OpenMP + TbbOpenMPMixedScope threadLimiter; + treeTraversal::DepthFirstForestParallel( + *this, rootData, HybridAssignmentData::AssignmentPreOrderVisitor, + visitorPost); } - // If TBB is enabled, the bayes net order gets reversed, - // so we pre-reverse it -#ifdef GTSAM_USE_TBB - auto reversed = boost::adaptors::reverse(gbn); - gbn = GaussianBayesNet(reversed.begin(), reversed.end()); -#endif + VectorValues result = gbt.optimize(); - // Return the optimized bayes net. - return gbn.optimize(); + // Return the optimized bayes net result. + return result; } } // namespace gtsam