From 82fc9b9eeb94427a093a2a86a76923ff5e798de1 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 3 Sep 2024 15:17:18 -0400
Subject: [PATCH] account for extra error when sigmas are different

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

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index 0a0332af8..228e851bb 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -87,7 +87,18 @@ GaussianFactorGraphTree GaussianMixture::add(
 
 /* *******************************************************************************/
 GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const {
-  auto wrap = [](const GaussianConditional::shared_ptr &gc) {
+  auto wrap = [this](const GaussianConditional::shared_ptr &gc) {
+    const double Cgm_Kgcm = this->logConstant_ - gc->logNormalizationConstant();
+    // If there is a difference in the covariances, we need to account for that
+    // since the error is dependent on the mode.
+    if (Cgm_Kgcm > 0.0) {
+      // We add a constant factor which will be used when computing
+      // the probability of the discrete variables.
+      Vector c(1);
+      c << std::sqrt(2.0 * Cgm_Kgcm);
+      auto constantFactor = std::make_shared<JacobianFactor>(c);
+      return GaussianFactorGraph{gc, constantFactor};
+    }
     return GaussianFactorGraph{gc};
   };
   return {conditionals_, wrap};