diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index eeacc929b8..d343decc8f 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -291,7 +291,7 @@ AlgebraicDecisionTree HybridBayesNet::modelSelection() const { std::numeric_limits::max()); } - // Compute the error for X* and the assignment + // Compute the error at the MLE point X* for the current assignment double error = this->error(HybridValues(mu, DiscreteValues(assignment))); diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 42402076ea..be94f42eee 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -295,8 +295,9 @@ static std::shared_ptr createDiscreteFactor( if (!factor) return 1.0; // TODO(dellaert): not loving this. // Logspace version of: - // exp(-factor->error(kEmpty)) / conditional->normalizationConstant(); - return -factor->error(kEmpty) - conditional->logNormalizationConstant(); + // exp(-factor->error(kEmpty)) * conditional->normalizationConstant(); + // We take negative of the logNormalizationConstant (1/k) to get k + return -factor->error(kEmpty) + (-conditional->logNormalizationConstant()); }; AlgebraicDecisionTree logProbabilities( @@ -324,6 +325,7 @@ static std::shared_ptr createGaussianMixtureFactor( if (factor) { auto hf = std::dynamic_pointer_cast(factor); if (!hf) throw std::runtime_error("Expected HessianFactor!"); + // Add 2.0 term since the constant term will be premultiplied by 0.5 hf->constantTerm() += 2.0 * conditional->logNormalizationConstant(); } return factor;