diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 603b29fb5..4845a4cd5 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -233,8 +233,8 @@ continuousElimination(const HybridGaussianFactorGraph &factors, /* ************************************************************************ */ /** - * @brief Exponentiate log-values, not necessarily normalized, normalize, and - * return as AlgebraicDecisionTree. + * @brief Exponentiate (not necessarily normalized) log-values, normalize, and + * then return as AlgebraicDecisionTree. * * @param logValues DecisionTree of (unnormalized) log values. * @return AlgebraicDecisionTree @@ -242,9 +242,9 @@ continuousElimination(const HybridGaussianFactorGraph &factors, static AlgebraicDecisionTree probabilitiesFromLogValues( const AlgebraicDecisionTree &logValues) { // Perform normalization - double max_log = logValues.max(); + double min_log = logValues.min(); AlgebraicDecisionTree probabilities = DecisionTree( - logValues, [&max_log](const double x) { return exp(x - max_log); }); + logValues, [&min_log](const double x) { return exp(-(x - min_log)); }); probabilities = probabilities.normalize(probabilities.sum()); return probabilities; @@ -265,7 +265,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors, auto logProbability = [&](const GaussianFactor::shared_ptr &factor) -> double { if (!factor) return 0.0; - return -factor->error(VectorValues()); + return factor->error(VectorValues()); }; AlgebraicDecisionTree logProbabilities = DecisionTree(gmf->factors(), logProbability); @@ -327,11 +327,11 @@ static std::shared_ptr createDiscreteFactor( // If the factor is not null, it has no keys, just contains the residual. if (!factor) return 1.0; // TODO(dellaert): not loving this. - // Logspace version of: + // Negative logspace version of: // exp(-factor->error(kEmpty)) / conditional->normalizationConstant(); // negLogConstant gives `-log(k)` // which is `-log(k) = log(1/k) = log(\sqrt{|2πΣ|})`. - return -factor->error(kEmpty) + conditional->negLogConstant(); + return factor->error(kEmpty) - conditional->negLogConstant(); }; AlgebraicDecisionTree logProbabilities(