From 1b7435361e85450a4d4339b7b9869cc74cdc2fad Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 15 Sep 2024 09:30:59 -0400
Subject: [PATCH] leverage hiding inside HybridGaussianFactor

---
 gtsam/hybrid/HybridGaussianConditional.cpp | 11 +++--------
 1 file changed, 3 insertions(+), 8 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp
index eeeed7d13..2b2062e6f 100644
--- a/gtsam/hybrid/HybridGaussianConditional.cpp
+++ b/gtsam/hybrid/HybridGaussianConditional.cpp
@@ -229,15 +229,10 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
         if (Cgm_Kgcm == 0.0) {
           return {likelihood_m, 0.0};
         } else {
-          // Add a constant factor to the likelihood in case the noise models
+          // Add a constant 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), 0.0};
+          double c = 2.0 * Cgm_Kgcm;
+          return {likelihood_m, c};
         }
       });
   return std::make_shared<HybridGaussianFactor>(