From 99825fcfe20f4957fcd983586d1aa98b3ca40f78 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 31 Dec 2022 12:24:58 +0100 Subject: [PATCH] use FactorAndConstant error() --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 6af0fb1a9..b35728f14 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -235,6 +235,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, gttoc_(hybrid_eliminate); #endif + //TODO(Varun) The normalizing constant has to be computed correctly return {conditional_factor.first, {conditional_factor.second, 0.0}}; }; @@ -257,7 +258,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // If there are no more continuous parents, then we should create here a // DiscreteFactor, with the error for each discrete choice. if (keysOfSeparator.empty()) { - VectorValues empty_values; auto factorProb = [&](const GaussianMixtureFactor::FactorAndConstant &factor_z) { GaussianFactor::shared_ptr factor = factor_z.factor; @@ -265,9 +265,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, return 0.0; // If nullptr, return 0.0 probability } else { // This is the probability q(μ) at the MLE point. - double error = - 0.5 * std::abs(factor->augmentedInformation().determinant()) + - factor_z.constant; + double error = factor_z.error(VectorValues()); return std::exp(-error); } };