From 655f57ef0dc851366b0f8c67d60ef014620449db Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 26 Jan 2025 11:14:16 -0500
Subject: [PATCH 1/2] check for valid factor in
 HybridNonlinearFactor::errorTree

---
 gtsam/hybrid/HybridNonlinearFactor.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp
index 376bc66f1..fa22051e5 100644
--- a/gtsam/hybrid/HybridNonlinearFactor.cpp
+++ b/gtsam/hybrid/HybridNonlinearFactor.cpp
@@ -99,7 +99,8 @@ AlgebraicDecisionTree<Key> HybridNonlinearFactor::errorTree(
   auto errorFunc =
       [continuousValues](const std::pair<sharedFactor, double>& f) {
         auto [factor, val] = f;
-        return factor->error(continuousValues) + val;
+        return factor ? factor->error(continuousValues) + val
+                      : std::numeric_limits<double>::infinity();
       };
   return {factors_, errorFunc};
 }

From 087c0cc5258bf1711f0c3f3aef74340d7de9355e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 26 Jan 2025 12:17:05 -0500
Subject: [PATCH 2/2] allow for only continuous variables in HybridBayesTree

---
 gtsam/hybrid/HybridBayesTree.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index c74930a8e..fbf892235 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -202,6 +202,11 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const {
 
 /* ************************************************************************* */
 void HybridBayesTree::prune(const size_t maxNrLeaves) {
+  if (!this->roots_.at(0)->conditional()->asDiscrete()) {
+    // Root of the BayesTree is not a discrete clique, so we do nothing.
+    return;
+  }
+
   auto prunedDiscreteProbs =
       this->roots_.at(0)->conditional()->asDiscrete<TableDistribution>();