From 191fc3da118aedc9461c6cbb22a213538b9b2a27 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 21 Aug 2024 06:13:43 -0400
Subject: [PATCH] remove augmentation in GaussianMixture in favor of
 augmentation in GaussianMixtureFactor

---
 gtsam/hybrid/GaussianMixture.cpp       | 18 ++----------------
 gtsam/hybrid/GaussianMixtureFactor.cpp |  6 +++---
 2 files changed, 5 insertions(+), 19 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index 0a0332af8..1440f7e9e 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -200,24 +200,10 @@ std::shared_ptr<GaussianMixtureFactor> GaussianMixture::likelihood(
   const GaussianMixtureFactor::Factors likelihoods(
       conditionals_, [&](const GaussianConditional::shared_ptr &conditional) {
         const auto likelihood_m = conditional->likelihood(given);
-        const double Cgm_Kgcm =
-            logConstant_ - conditional->logNormalizationConstant();
-        if (Cgm_Kgcm == 0.0) {
-          return likelihood_m;
-        } else {
-          // Add a constant factor to the likelihood in case the noise models
-          // are not all equal.
-          GaussianFactorGraph gfg;
-          gfg.push_back(likelihood_m);
-          Vector c(1);
-          c << std::sqrt(2.0 * Cgm_Kgcm);
-          auto constantFactor = std::make_shared<JacobianFactor>(c);
-          gfg.push_back(constantFactor);
-          return std::make_shared<JacobianFactor>(gfg);
-        }
+        return likelihood_m;
       });
   return std::make_shared<GaussianMixtureFactor>(
-      continuousParentKeys, discreteParentKeys, likelihoods);
+      continuousParentKeys, discreteParentKeys, likelihoods, true);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp
index e519cefe6..7fb16f0d1 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/GaussianMixtureFactor.cpp
@@ -29,7 +29,7 @@
 namespace gtsam {
 
 /**
- * @brief Helper function to correct the [A|b] matrices in the factor components
+ * @brief Helper function to augment the [A|b] matrices in the factor components
  * with the normalizer values.
  * This is done by storing the normalizer value in
  * the `b` vector as an additional row.
@@ -39,7 +39,7 @@ namespace gtsam {
  * each component.
  * @return GaussianMixtureFactor::Factors
  */
-GaussianMixtureFactor::Factors correct(
+GaussianMixtureFactor::Factors augment(
     const GaussianMixtureFactor::Factors &factors, bool varyingNormalizers) {
   if (!varyingNormalizers) {
     return factors;
@@ -107,7 +107,7 @@ GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys,
                                              const Factors &factors,
                                              bool varyingNormalizers)
     : Base(continuousKeys, discreteKeys),
-      factors_(correct(factors, varyingNormalizers)) {}
+      factors_(augment(factors, varyingNormalizers)) {}
 
 /* *******************************************************************************/
 bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {