From 939fdcc7201729a2ef686087d6a178ba5cf14ef0 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 20 Sep 2024 16:20:49 -0400
Subject: [PATCH] HybridGaussianFactorGraph::errorTree is better encompassing

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

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 01ecfe5ac..a6fe955eb 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -542,10 +542,15 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::errorTree(
   // Iterate over each factor.
   for (auto &factor : factors_) {
     if (auto f = std::dynamic_pointer_cast<HybridFactor>(factor)) {
+      // Check for HybridFactor, and call errorTree
       error_tree = error_tree + f->errorTree(continuousValues);
-    } else if (auto f = std::dynamic_pointer_cast<GaussianFactor>(factor)) {
-      error_tree =
-          error_tree + AlgebraicDecisionTree<Key>(f->error(continuousValues));
+    } else if (auto f = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
+      // Skip discrete factors
+      continue;
+    } else {
+      // Everything else is a continuous only factor
+      HybridValues hv(continuousValues, DiscreteValues());
+      error_tree = error_tree + AlgebraicDecisionTree<Key>(factor->error(hv));
     }
   }
   return error_tree;