From 09bf00b54583a505087e7f0ade72b8e237c9cdb6 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 29 Jan 2025 00:51:16 -0500
Subject: [PATCH] Catch pruned away

---
 gtsam/hybrid/HybridGaussianConditional.cpp | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp
index 92ae05df6..964af7ffb 100644
--- a/gtsam/hybrid/HybridGaussianConditional.cpp
+++ b/gtsam/hybrid/HybridGaussianConditional.cpp
@@ -313,22 +313,21 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune(
   std::set_difference(theirs.begin(), theirs.end(), mine.begin(), mine.end(),
                       std::back_inserter(diff));
 
-  // Find maximum probability value for every combination of our keys.
-  Ordering keys(diff);
-  auto max = discreteProbs.max(keys);
+  // Find maximum probability value for every combination of *our* keys.
+  Ordering ordering(diff);
+  auto max = discreteProbs.max(ordering);
 
   // Check the max value for every combination of our keys.
   // If the max value is 0.0, we can prune the corresponding conditional.
+  bool allPruned = true;
   auto pruner =
       [&](const Assignment<Key> &choices,
           const GaussianFactorValuePair &pair) -> GaussianFactorValuePair {
-    // If Gaussian factor is nullptr, return infinity
-    if (!pair.first) {
+    // If this choice is zero probability or Gaussian is null, return infinity
+    if (!pair.first || max->evaluate(choices) == 0.0) {
       return {nullptr, std::numeric_limits<double>::infinity()};
-    }
-    if (max->evaluate(choices) == 0.0)
-      return {nullptr, std::numeric_limits<double>::infinity()};
-    else {
+    } else {
+      allPruned = false;
       // Add negLogConstant_ back so that the minimum negLogConstant in the
       // HybridGaussianConditional is set correctly.
       return {pair.first, pair.second + negLogConstant_};
@@ -336,6 +335,7 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune(
   };
 
   FactorValuePairs prunedConditionals = factors().apply(pruner);
+  if (allPruned) return nullptr;
   return std::make_shared<HybridGaussianConditional>(discreteKeys(),
                                                      prunedConditionals, true);
 }