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 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(c); - gfg.push_back(constantFactor); - return {std::make_shared(gfg), 0.0}; + double c = 2.0 * Cgm_Kgcm; + return {likelihood_m, c}; } }); return std::make_shared(