From bb4ee207b837aa5a613461b6c20a67e2d01ef29d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 19:13:13 -0500
Subject: [PATCH] custom path for empty separator

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 49 +++++++++++-----------
 1 file changed, 24 insertions(+), 25 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 387a5849f..7cc890dc0 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -341,41 +341,40 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
 #if GTSAM_HYBRID_TIMING
   gttic_(EliminateDiscrete);
 #endif
-  /**** NOTE: This does sum-product. ****/
-  // Get product factor
-  TableFactor product = ProductAndNormalize(dfg);
+  // Check if separator is empty
+  Ordering allKeys(dfg.keyVector());
+  Ordering separator;
+  std::set_difference(allKeys.begin(), allKeys.end(), frontalKeys.begin(),
+                      frontalKeys.end(),
+                      std::inserter(separator, separator.begin()));
+
+  // If the separator is empty, we have a clique of all the discrete variables
+  // so we can use the TableFactor for efficiency.
+  if (separator.size() == 0) {
+    // Get product factor
+    TableFactor product = ProductAndNormalize(dfg);
 
 #if GTSAM_HYBRID_TIMING
-  gttic_(EliminateDiscreteSum);
+    gttic_(EliminateDiscreteFormDiscreteConditional);
 #endif
-  // All the discrete variables should form a single clique,
-  // so we can sum out on all the variables as frontals.
-  // This should give an empty separator.
-  TableFactor::shared_ptr sum = product.sum(frontalKeys);
+    auto conditional = std::make_shared<DiscreteConditional>(
+        frontalKeys.size(), product.toDecisionTreeFactor());
 #if GTSAM_HYBRID_TIMING
-  gttoc_(EliminateDiscreteSum);
+    gttoc_(EliminateDiscreteFormDiscreteConditional);
 #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());
-
+    TableFactor::shared_ptr sum = product.sum(frontalKeys);
 #if GTSAM_HYBRID_TIMING
-  gttic_(EliminateDiscreteFormDiscreteConditional);
-#endif
-  auto c = product / (*sum);
-  auto conditional = std::make_shared<DiscreteConditional>(
-      frontalKeys.size(), c.toDecisionTreeFactor(), orderedKeys);
-#if GTSAM_HYBRID_TIMING
-  gttoc_(EliminateDiscreteFormDiscreteConditional);
+    gttoc_(EliminateDiscrete);
 #endif
 
-#if GTSAM_HYBRID_TIMING
-  gttoc_(EliminateDiscrete);
-#endif
+    return {std::make_shared<HybridConditional>(conditional), sum};
 
-  return {std::make_shared<HybridConditional>(conditional), sum};
+  } else {
+    // Perform sum-product.
+    auto result = EliminateDiscrete(dfg, frontalKeys);
+    return {std::make_shared<HybridConditional>(result.first), result.second};
+  }
 }
 
 /* ************************************************************************ */