From 77bea319ddc727d41fffb8f26fc1907cbaeae61d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 13 Aug 2022 15:11:21 -0400 Subject: [PATCH] one more test passing --- gtsam/hybrid/tests/testHybridIncremental.cpp | 109 +++++++++---------- 1 file changed, 49 insertions(+), 60 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridIncremental.cpp b/gtsam/hybrid/tests/testHybridIncremental.cpp index eec260cbc..7ab64462c 100644 --- a/gtsam/hybrid/tests/testHybridIncremental.cpp +++ b/gtsam/hybrid/tests/testHybridIncremental.cpp @@ -18,7 +18,9 @@ #include #include +#include #include +#include #include #include #include @@ -108,7 +110,7 @@ TEST(HybridGaussianElimination, IncrementalInference) { // Create initial factor graph // * * * // | | | - // *- X1 -*- X2 -*- X3 + // X1 -*- X2 -*- X3 // | | // *-M1 - * - M2 graph1.push_back(switching.linearizedFactorGraph.at(0)); // P(X1) @@ -119,6 +121,10 @@ TEST(HybridGaussianElimination, IncrementalInference) { // Run update step isam.update(graph1); + auto discreteConditional_m1 = + isam[M(1)]->conditional()->asDiscreteConditional(); + EXPECT(discreteConditional_m1->keys() == KeyVector({M(1)})); + /********************************************************/ // New factor graph for incremental update. HybridGaussianFactorGraph graph2; @@ -165,65 +171,48 @@ TEST(HybridGaussianElimination, IncrementalInference) { // We only perform manual continuous elimination for 0,0. // The other discrete probabilities on M(2) are calculated the same way - auto m00_prob = [&]() { - GaussianFactorGraph gf; - auto x2_prior = boost::dynamic_pointer_cast( - switching.linearizedFactorGraph.at(3))->inner(); - gf.add(x2_prior); + Ordering discrete_ordering; + discrete_ordering += M(1); + discrete_ordering += M(2); + HybridBayesTree::shared_ptr discreteBayesTree = + expectedRemainingGraph->eliminateMultifrontal(discrete_ordering); - DiscreteValues m00; - m00[M(1)] = 0, m00[M(2)] = 0; - // P(X2, X3 | M2) - auto dcMixture = - dynamic_pointer_cast(graph2.at(0)); - gf.add(dcMixture->factors()(m00)); + DiscreteValues m00; + m00[M(1)] = 0, m00[M(2)] = 0; + DiscreteConditional decisionTree = *(*discreteBayesTree)[M(2)]->conditional()->asDiscreteConditional(); + double m00_prob = decisionTree(m00); - auto x2_mixed = - boost::dynamic_pointer_cast(isam[X(2)]->conditional()->inner()); - // Perform explicit cast so we can add the conditional to `gf`. - auto x2_cond = boost::dynamic_pointer_cast( - x2_mixed->conditionals()(m00)); - gf.add(x2_cond); + auto discreteConditional = isam[M(2)]->conditional()->asDiscreteConditional(); - auto result_gf = gf.eliminateSequential(); - return gf.probPrime(result_gf->optimize()); - }(); - - auto discreteConditional = isam[M(1)]->conditional()->asDiscreteConditional(); // Test if the probability values are as expected with regression tests. - // DiscreteValues assignment; - // EXPECT(assert_equal(m00_prob, 0.60656, 1e-5)); - // assignment[M(1)] = 0; - // assignment[M(2)] = 0; - // EXPECT(assert_equal(m00_prob, (*discreteConditional)(assignment), 1e-5)); -// assignment[M(1)] = 1; -// assignment[M(2)] = 0; -// EXPECT(assert_equal(0.612477, (*discreteConditional)(assignment), 1e-5)); -// assignment[M(1)] = 0; -// assignment[M(2)] = 1; -// EXPECT(assert_equal(0.999952, (*discreteConditional)(assignment), 1e-5)); -// assignment[M(1)] = 1; -// assignment[M(2)] = 1; -// EXPECT(assert_equal(1.0, (*discreteConditional)(assignment), 1e-5)); + DiscreteValues assignment; + EXPECT(assert_equal(m00_prob, 0.0619233, 1e-5)); + assignment[M(1)] = 0; + assignment[M(2)] = 0; + EXPECT(assert_equal(m00_prob, (*discreteConditional)(assignment), 1e-5)); + assignment[M(1)] = 1; + assignment[M(2)] = 0; + EXPECT(assert_equal(0.183743, (*discreteConditional)(assignment), 1e-5)); + assignment[M(1)] = 0; + assignment[M(2)] = 1; + EXPECT(assert_equal(0.204159, (*discreteConditional)(assignment), 1e-5)); + assignment[M(1)] = 1; + assignment[M(2)] = 1; + EXPECT(assert_equal(0.2, (*discreteConditional)(assignment), 1e-5)); -// DiscreteFactorGraph dfg; -// dfg.add(*discreteConditional); -// dfg.add(discreteConditional_m1); -// dfg.add_factors(switching.linearizedFactorGraph.discreteGraph()); - -// // Check if the chordal graph generated from incremental elimination -// matches -// // that of batch elimination. -// auto chordal = dfg.eliminateSequential(); -// auto expectedChordal = -// expectedRemainingGraph->discreteGraph().eliminateSequential(); - -// EXPECT(assert_equal(*expectedChordal, *chordal, 1e-6)); + // Check if the clique conditional generated from incremental elimination matches + // that of batch elimination. + auto expectedChordal = expectedRemainingGraph->eliminateMultifrontal(); + auto expectedConditional = dynamic_pointer_cast( + (*expectedChordal)[M(2)]->conditional()->inner()); + auto actualConditional = dynamic_pointer_cast( + isam[M(2)]->conditional()->inner()); + EXPECT(assert_equal(*actualConditional, *expectedConditional, 1e-6)); } /* ****************************************************************************/ -// // Test if we can approximately do the inference -// TEST(DCGaussianElimination, Approx_inference) { +// Test if we can approximately do the inference +TEST(HybridGaussianElimination, Approx_inference) { // Switching switching(4); // IncrementalHybrid incrementalHybrid; // HybridGaussianFactorGraph graph1; @@ -339,11 +328,11 @@ TEST(HybridGaussianElimination, IncrementalInference) { // *lastDensity(assignment))); // } // } -// } +} /* ****************************************************************************/ -// // Test approximate inference with an additional pruning step. -// TEST(DCGaussianElimination, Incremental_approximate) { +// Test approximate inference with an additional pruning step. +TEST(HybridGaussianElimination, Incremental_approximate) { // Switching switching(5); // IncrementalHybrid incrementalHybrid; // HybridGaussianFactorGraph graph1; @@ -395,12 +384,12 @@ TEST(HybridGaussianElimination, IncrementalInference) { // CHECK_EQUAL(2, actualBayesNet.size()); // EXPECT_LONGS_EQUAL(10, actualBayesNet.atGaussian(0)->nrComponents()); // EXPECT_LONGS_EQUAL(5, actualBayesNet.atGaussian(1)->nrComponents()); -// } +} /* ************************************************************************/ -// // Test for figuring out the optimal ordering to ensure we get -// // a discrete graph after elimination. -// TEST(IncrementalHybrid, NonTrivial) { +// Test for figuring out the optimal ordering to ensure we get +// a discrete graph after elimination. +TEST(IncrementalHybrid, NonTrivial) { // // This is a GTSAM-only test for running inference on a single legged // robot. // // The leg links are represented by the chain X-Y-Z-W, where X is the base @@ -637,7 +626,7 @@ TEST(HybridGaussianElimination, IncrementalInference) { // auto lastConditional = boost::dynamic_pointer_cast( // inc.hybridBayesNet().at(inc.hybridBayesNet().size() - 1)); // EXPECT_LONGS_EQUAL(3, lastConditional->nrComponents()); -// } +} /* ************************************************************************* */ int main() {