From ee124c33c3b535e651e8a5c5d31bdb863828302a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 2 Aug 2022 19:10:27 -0400 Subject: [PATCH] fix discrete only elimination (use EliminateForMPE) --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 2 +- .../tests/testHybridNonlinearFactorGraph.cpp | 21 ++++++++----------- 2 files changed, 10 insertions(+), 13 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 3536f6a36..55fa9a908 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -164,7 +164,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors, } } - auto result = EliminateDiscrete(dfg, frontalKeys); + auto result = EliminateForMPE(dfg, frontalKeys); return {boost::make_shared(result.first), boost::make_shared(result.second)}; diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 947fafacb..6e66e5fcb 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -465,18 +465,15 @@ TEST(HybridFactorGraph, Full_Elimination) { // P(m1 | m2) EXPECT(hybridBayesNet->at(3)->frontals() == KeyVector{M(1)}); EXPECT(hybridBayesNet->at(3)->parents() == KeyVector({M(2)})); - GTSAM_PRINT(*(hybridBayesNet->at(3))); - GTSAM_PRINT(*(discreteBayesNet.at(0))); - //TODO(Varun) FIX!! - // EXPECT( - // dynamic_pointer_cast(hybridBayesNet->at(3)->inner()) - // ->equals(*discreteBayesNet.at(0))); - // // P(m2) - // EXPECT(hybridBayesNet->at(4)->frontals() == KeyVector{M(2)}); - // EXPECT_LONGS_EQUAL(0, hybridBayesNet->at(4)->nrParents()); - // EXPECT( - // dynamic_pointer_cast(hybridBayesNet->at(4)->inner()) - // ->equals(*discreteBayesNet.at(1))); + EXPECT( + dynamic_pointer_cast(hybridBayesNet->at(3)->inner()) + ->equals(*discreteBayesNet.at(0))); + // P(m2) + EXPECT(hybridBayesNet->at(4)->frontals() == KeyVector{M(2)}); + EXPECT_LONGS_EQUAL(0, hybridBayesNet->at(4)->nrParents()); + EXPECT( + dynamic_pointer_cast(hybridBayesNet->at(4)->inner()) + ->equals(*discreteBayesNet.at(1))); } /*