From 906330f0e4788d16cd0d6bc45d86f5deb7d6b09a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 13 Jan 2023 08:24:31 -0800
Subject: [PATCH] Add discrete contribution to logProbability

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

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index fd1d24722..59dfd809d 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -311,9 +311,11 @@ AlgebraicDecisionTree<Key> HybridBayesNet::logProbability(
         return leaf_value + logProbability;
       });
     } else if (auto dc = conditional->asDiscrete()) {
-      // TODO(dellaert): if discrete, we need to add logProbability in the right
-      // branch?
-      continue;
+      // If discrete, add the discrete logProbability in the right branch
+      result = result.apply(
+          [dc](const Assignment<Key> &assignment, double leaf_value) {
+            return leaf_value + dc->logProbability(DiscreteValues(assignment));
+          });
     }
   }