update GaussianMixture::likelihood to compute the logNormalizers

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);
 }
 
 /* ************************************************************************* */