Fix Pruning

gtsam/hybrid/HybridBayesNet.cpp

@@ -73,7 +73,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
   // per pruned Discrete joint.
   for (auto &&conditional : *this) {
     if (auto hgc = conditional->asHybrid()) {
-      // Make a copy of the hybrid Gaussian conditional and prune it!
+      // Prune the hybrid Gaussian conditional!
       auto prunedHybridGaussianConditional = hgc->prune(pruned);
 
       // Type-erase and add to the pruned Bayes Net fragment.

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -240,7 +240,9 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
       // In this case, compute discrete probabilities.
       auto logProbability =
           [&](const GaussianFactor::shared_ptr &factor) -> double {
-        if (!factor) return 0.0;
+        // If the factor is null, it is has been pruned hence return ∞
+        // so that the exp(-∞)=0.
+        if (!factor) return std::numeric_limits<double>::infinity();
         return factor->error(VectorValues());
       };
       AlgebraicDecisionTree<Key> logProbabilities =
@@ -300,11 +302,15 @@ static std::shared_ptr<Factor> createDiscreteFactor(
   auto negLogProbability = [&](const Result &pair) -> double {
     const auto &[conditional, factor] = pair;
     static const VectorValues kEmpty;
-    // If the factor is not null, it has no keys, just contains the residual.
-    if (!factor) return 1.0;  // TODO(dellaert): not loving this.
+    // If the factor is null, it has been pruned, hence return ∞
+    // so that the exp(-∞)=0.
+    if (!factor) return std::numeric_limits<double>::infinity();
 
     // Negative logspace version of:
     // exp(-factor->error(kEmpty)) / conditional->normalizationConstant();
+    // = exp(-factor->error(kEmpty)) * \sqrt{|2πΣ|};
+    // log = -(-factor->error(kEmpty) + log(\sqrt{|2πΣ|}))
+    // = factor->error(kEmpty) - log(\sqrt{|2πΣ|})
     // negLogConstant gives `-log(k)`
     // which is `-log(k) = log(1/k) = log(\sqrt{|2πΣ|})`.
     return factor->error(kEmpty) - conditional->negLogConstant();