From 12d02bed1a32591e0fae50d1f1fdf3f4be392bc8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 2 Jan 2023 13:22:16 -0500 Subject: [PATCH] Right marginals for tiny1 --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 23 +++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index e8b7eb950..5b28e06d3 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -209,7 +209,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // decision tree indexed by all discrete keys involved. GaussianFactorGraphTree sum = factors.assembleGraphTree(); - // TODO(dellaert): does assembleGraphTree not guarantee we do not need this? + // TODO(dellaert): does assembleGraphTree not guarantee this? sum = removeEmpty(sum); using EliminationPair = std::pair, @@ -234,16 +234,16 @@ hybridElimination(const HybridGaussianFactorGraph &factors, gttoc_(hybrid_eliminate); #endif + const double logZ = graph_z.constant - conditional->logNormalizationConstant(); // Get the log of the log normalization constant inverse. - double logZ = -conditional->logNormalizationConstant(); - - // IF this is the last continuous variable to eliminated, we need to - // calculate the error here: the value of all factors at the mean, see - // ml_map_rao.pdf. - if (continuousSeparator.empty()) { - const auto posterior_mean = conditional->solve(VectorValues()); - logZ += graph_z.graph.error(posterior_mean); - } + // double logZ = -conditional->logNormalizationConstant(); + // // IF this is the last continuous variable to eliminated, we need to + // // calculate the error here: the value of all factors at the mean, see + // // ml_map_rao.pdf. + // if (continuousSeparator.empty()) { + // const auto posterior_mean = conditional->solve(VectorValues()); + // logZ += graph_z.graph.error(posterior_mean); + // } return {conditional, {newFactor, logZ}}; }; @@ -270,11 +270,12 @@ hybridElimination(const HybridGaussianFactorGraph &factors, auto factorProb = [&](const GaussianMixtureFactor::FactorAndConstant &factor_z) { // This is the probability q(μ) at the MLE point. + // factor_z.factor is a factor without keys, just containing the residual. // 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); + return exp(factor_z.constant); }; const DecisionTree fdt(newFactors, factorProb); const auto discreteFactor =