From 0e2e8bb8ced3be38e150e72df03869f7ca49632f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 18:23:37 -0500
Subject: [PATCH] full discrete elimination

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

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 831e0ccc2..ca971191c 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -356,13 +356,17 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
   gttoc_(EliminateDiscreteSum);
 #endif
 
+  // Ordering keys for the conditional so that frontalKeys are really in front
+  Ordering orderedKeys;
+  orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), frontalKeys.end());
+  orderedKeys.insert(orderedKeys.end(), sum->keys().begin(), sum->keys().end());
+
 #if GTSAM_HYBRID_TIMING
   gttic_(EliminateDiscreteFormDiscreteConditional);
 #endif
   // Finally, get the conditional
-  auto c = product / (*sum);
   auto conditional = std::make_shared<DiscreteConditional>(
-      frontalKeys.size(), c.toDecisionTreeFactor());
+      product.toDecisionTreeFactor(), sum->toDecisionTreeFactor(), orderedKeys);
 #if GTSAM_HYBRID_TIMING
   gttoc_(EliminateDiscreteFormDiscreteConditional);
 #endif