Merge pull request #2004 from borglab/hybrid-fixes

Hybrid Fixes
borglab · Jan 26, 2025 · 648c68d · 648c68d
2 parents 131f51a + 087c0cc
commit 648c68d
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 1 deletion.
diff --git 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>();
 

diff --git 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};
 }