From d54cf484de9ed9c86b387d4776faba13e367627e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 9 Nov 2022 20:04:51 -0500
Subject: [PATCH] fix creation of updatedBayesTree

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index a0c1b67da..d07b728b5 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -768,10 +768,13 @@ HybridGaussianFactorGraph::eliminateHybridMultifrontal(
 
   auto discrete_clique = (*updatedBayesTree)[discrete_ordering.at(0)];
 
-  // Set the root of the bayes tree as the discrete clique
+  std::set<HybridBayesTreeClique::shared_ptr> clique_set;
   for (auto node : bayesTree->nodes()) {
-    auto clique = node.second;
+    clique_set.insert(node.second);
+  }
 
+  // Set the root of the bayes tree as the discrete clique
+  for (auto clique : clique_set) {
     if (clique->conditional()->parents() ==
         discrete_clique->conditional()->frontals()) {
       updatedBayesTree->addClique(clique, discrete_clique);