fixes

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -224,7 +224,24 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
       [&](const GaussianConditional::shared_ptr &conditional)
           -> GaussianFactorValuePair {
         const auto likelihood_m = conditional->likelihood(given);
-  s.insert(discreteKeys.begin(), discreteKeys.end());
+        const double Cgm_Kgcm =
+            logConstant_ - conditional->logNormalizationConstant();
+        if (Cgm_Kgcm == 0.0) {
+          return {likelihood_m, 0.0};
+        } else {
+          // Add a constant to the likelihood in case the noise models
+          // are not all equal.
+          double c = 2.0 * Cgm_Kgcm;
+          return {likelihood_m, c};
+        }
+      });
+  return std::make_shared<HybridGaussianFactor>(
+      continuousParentKeys, discreteParentKeys, likelihoods);
+}
+
+/* ************************************************************************* */
+std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &discreteKeys) {
+  std::set<DiscreteKey> s(discreteKeys.begin(), discreteKeys.end());
   return s;
 }
 
@@ -237,6 +254,7 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
  * const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
  */
 std::function<GaussianConditional::shared_ptr(
+    const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
 HybridGaussianConditional::prunerFunc(const DecisionTreeFactor &discreteProbs) {
   // Get the discrete keys as sets for the decision tree
   // and the gaussian mixture.

gtsam/hybrid/HybridGaussianFactor.cpp

@@ -163,20 +163,4 @@ double HybridGaussianFactor::error(const HybridValues &values) const {
   return gf->error(values.continuous());
 }
 
-/* *******************************************************************************/
-double ComputeLogNormalizer(
-    const noiseModel::Gaussian::shared_ptr &noise_model) {
-  // Since noise models are Gaussian, we can get the logDeterminant using
-  // the same trick as in GaussianConditional
-  double logDetR = noise_model->R()
-                       .diagonal()
-                       .unaryExpr([](double x) { return log(x); })
-                       .sum();
-  double logDeterminantSigma = -2.0 * logDetR;
-
-  size_t n = noise_model->dim();
-  constexpr double log2pi = 1.8378770664093454835606594728112;
-  return n * log2pi + logDeterminantSigma;
-}
-
 }  // namespace gtsam

gtsam/hybrid/HybridGaussianFactor.h

@@ -175,16 +175,4 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
 template <>
 struct traits<HybridGaussianFactor> : public Testable<HybridGaussianFactor> {};
 
-/**
- * @brief Helper function to compute the sqrt(|2πΣ|) normalizer values
- * for a Gaussian noise model.
- * We compute this in the log-space for numerical accuracy.
- *
- * @param noise_model The Gaussian noise model
- * whose normalizer we wish to compute.
- * @return double
- */
-GTSAM_EXPORT double ComputeLogNormalizer(
-    const noiseModel::Gaussian::shared_ptr &noise_model);
-
 }  // namespace gtsam