From 7358effe038d0cb82c9b378a21c8afabfa0ea269 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 25 Aug 2024 00:49:28 -0400
Subject: [PATCH] allow for GaussianMixtureFactors with 0 continuous keys

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index bf11a50fc..cb8ceed20 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -242,6 +242,18 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
   for (auto &f : factors) {
     if (auto df = dynamic_pointer_cast<DiscreteFactor>(f)) {
       dfg.push_back(df);
+    } else if (auto gmf = dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
+      // Case where we have a GaussianMixtureFactor with no continuous keys.
+      // In this case, compute discrete probabilities.
+      auto probability =
+          [&](const GaussianFactor::shared_ptr &factor) -> double {
+        if (!factor) return 0.0;
+        return exp(-factor->error(VectorValues()));
+      };
+      dfg.emplace_shared<DecisionTreeFactor>(
+          gmf->discreteKeys(),
+          DecisionTree<Key, double>(gmf->factors(), probability));
+
     } else if (auto orphan = dynamic_pointer_cast<OrphanWrapper>(f)) {
       // Ignore orphaned clique.
       // TODO(dellaert): is this correct? If so explain here.