diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index adba9e88b..a5d27c188 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -274,7 +274,11 @@ hybridElimination(const HybridGaussianFactorGraph &factors, auto factorProb = [&](const GaussianMixtureFactor::FactorAndConstant &factor_z) { // This is the probability q(μ) at the MLE point. - return factor_z.error(VectorValues()); + // return exp(-factor_z.error(VectorValues())); + // TODO(dellaert): this is not correct, since VectorValues() is not + // the MLE point. But it does not matter, as at the MLE point the + // error will be zero, hence: + return exp(-factor_z.constant); }; const DecisionTree fdt(newFactors, factorProb); const auto discreteFactor =