From 9a6d2cf32384d437960def61cd3d547c5f73f41e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 21 Aug 2024 20:10:40 -0400
Subject: [PATCH] update GaussianMixture::likelihood to compute the
 logNormalizers

---
 gtsam/hybrid/GaussianMixture.cpp | 19 ++++++++++++++++++-
 1 file changed, 18 insertions(+), 1 deletion(-)

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index 1440f7e9e..6c92f0252 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -202,8 +202,25 @@ std::shared_ptr<GaussianMixtureFactor> GaussianMixture::likelihood(
         const auto likelihood_m = conditional->likelihood(given);
         return likelihood_m;
       });
+
+  // First compute all the sqrt(|2 pi Sigma|) terms
+  auto computeLogNormalizers = [](const GaussianFactor::shared_ptr &gf) {
+    auto jf = std::dynamic_pointer_cast<JacobianFactor>(gf);
+    // If we have, say, a Hessian factor, then no need to do anything
+    if (!jf) return 0.0;
+
+    auto model = jf->get_model();
+    // If there is no noise model, there is nothing to do.
+    if (!model) {
+      return 0.0;
+    }
+    return ComputeLogNormalizer(model);
+  };
+
+  AlgebraicDecisionTree<Key> log_normalizers =
+      DecisionTree<Key, double>(likelihoods, computeLogNormalizers);
   return std::make_shared<GaussianMixtureFactor>(
-      continuousParentKeys, discreteParentKeys, likelihoods, true);
+      continuousParentKeys, discreteParentKeys, likelihoods, log_normalizers);
 }
 
 /* ************************************************************************* */