From 5965d8f2fb45260a30637c417d0eedd2e2941ed6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 8 Aug 2022 17:16:47 -0400 Subject: [PATCH] change discrete key variable from C to M --- gtsam/hybrid/tests/Switching.h | 2 +- .../tests/testGaussianHybridFactorGraph.cpp | 178 +++++++++--------- 2 files changed, 89 insertions(+), 91 deletions(-) diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index d584dd60e..ac440d2a5 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -50,7 +50,7 @@ using symbol_shorthand::X; */ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( size_t n, std::function keyFunc = X, - std::function dKeyFunc = C) { + std::function dKeyFunc = M) { HybridGaussianFactorGraph hfg; hfg.add(JacobianFactor(keyFunc(1), I_3x3, Z_3x1)); diff --git a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp index 92fc699bb..0b2d5f190 100644 --- a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp @@ -52,8 +52,8 @@ using namespace boost::assign; using namespace std; using namespace gtsam; -using gtsam::symbol_shorthand::C; using gtsam::symbol_shorthand::D; +using gtsam::symbol_shorthand::M; using gtsam::symbol_shorthand::X; using gtsam::symbol_shorthand::Y; @@ -67,9 +67,9 @@ TEST(HybridGaussianFactorGraph, Creation) { // Define a gaussian mixture conditional P(x0|x1, c0) and add it to the factor // graph - GaussianMixture gm({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}), + GaussianMixture gm({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{M(0), 2}), GaussianMixture::Conditionals( - C(0), + M(0), boost::make_shared( X(0), Z_3x1, I_3x3, X(1), I_3x3), boost::make_shared( @@ -96,11 +96,11 @@ TEST(HybridGaussianFactorGraph, EliminateMultifrontal) { // Test multifrontal elimination HybridGaussianFactorGraph hfg; - DiscreteKey c(C(1), 2); + DiscreteKey m(M(1), 2); // Add priors on x0 and c1 hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); - hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8}))); + hfg.add(HybridDiscreteFactor(DecisionTreeFactor(m, {2, 8}))); Ordering ordering; ordering.push_back(X(0)); @@ -114,7 +114,7 @@ TEST(HybridGaussianFactorGraph, EliminateMultifrontal) { TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { HybridGaussianFactorGraph hfg; - DiscreteKey c1(C(1), 2); + DiscreteKey m1(M(1), 2); // Add prior on x0 hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); @@ -123,17 +123,17 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { // Add a gaussian mixture factor ϕ(x1, c1) DecisionTree dt( - C(1), boost::make_shared(X(1), I_3x3, Z_3x1), + M(1), boost::make_shared(X(1), I_3x3, Z_3x1), boost::make_shared(X(1), I_3x3, Vector3::Ones())); - hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt)); + hfg.add(GaussianMixtureFactor({X(1)}, {m1}, dt)); auto result = - hfg.eliminateSequential(Ordering::ColamdConstrainedLast(hfg, {C(1)})); + hfg.eliminateSequential(Ordering::ColamdConstrainedLast(hfg, {M(1)})); auto dc = result->at(2)->asDiscreteConditional(); DiscreteValues dv; - dv[C(1)] = 0; + dv[M(1)] = 0; EXPECT_DOUBLES_EQUAL(1, dc->operator()(dv), 1e-3); } @@ -141,27 +141,27 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) { HybridGaussianFactorGraph hfg; - DiscreteKey c1(C(1), 2); + DiscreteKey m1(M(1), 2); // Add prior on x0 hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); // Add factor between x0 and x1 hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - DecisionTree dt( - C(1), boost::make_shared(X(1), I_3x3, Z_3x1), - boost::make_shared(X(1), I_3x3, Vector3::Ones())); - hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt)); + std::vector factors = { + boost::make_shared(X(1), I_3x3, Z_3x1), + boost::make_shared(X(1), I_3x3, Vector3::Ones())}; + hfg.add(GaussianMixtureFactor({X(1)}, {m1}, factors)); // Discrete probability table for c1 - hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c1, {2, 8}))); + hfg.add(DecisionTreeFactor(m1, {2, 8})); // Joint discrete probability table for c1, c2 - hfg.add(HybridDiscreteFactor( - DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4"))); + hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); - auto result = hfg.eliminateSequential( - Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)})); + HybridBayesNet::shared_ptr result = hfg.eliminateSequential( + Ordering::ColamdConstrainedLast(hfg, {M(1), M(2)})); + // There are 4 variables (2 continuous + 2 discrete) in the bayes net. EXPECT_LONGS_EQUAL(4, result->size()); } @@ -169,31 +169,33 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) { TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { HybridGaussianFactorGraph hfg; - DiscreteKey c1(C(1), 2); + DiscreteKey m1(M(1), 2); hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); hfg.add(GaussianMixtureFactor::FromFactors( - {X(1)}, {{C(1), 2}}, + {X(1)}, {{M(1), 2}}, {boost::make_shared(X(1), I_3x3, Z_3x1), boost::make_shared(X(1), I_3x3, Vector3::Ones())})); - hfg.add(DecisionTreeFactor(c1, {2, 8})); - hfg.add(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")); + hfg.add(DecisionTreeFactor(m1, {2, 8})); + hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); - auto result = hfg.eliminateMultifrontal( - Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)})); + HybridBayesTree::shared_ptr result = hfg.eliminateMultifrontal( + Ordering::ColamdConstrainedLast(hfg, {M(1), M(2)})); + // The bayes tree should have 3 cliques + EXPECT_LONGS_EQUAL(3, result->size()); // GTSAM_PRINT(*result); - // GTSAM_PRINT(*result->marginalFactor(C(2))); + // GTSAM_PRINT(*result->marginalFactor(M(2))); } /* ************************************************************************* */ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { HybridGaussianFactorGraph hfg; - DiscreteKey c(C(1), 2); + DiscreteKey m(M(1), 2); // Prior on x0 hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); @@ -202,16 +204,16 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { // Decision tree with different modes on x1 DecisionTree dt( - C(1), boost::make_shared(X(1), I_3x3, Z_3x1), + M(1), boost::make_shared(X(1), I_3x3, Z_3x1), boost::make_shared(X(1), I_3x3, Vector3::Ones())); // Hybrid factor P(x1|c1) - hfg.add(GaussianMixtureFactor({X(1)}, {c}, dt)); + hfg.add(GaussianMixtureFactor({X(1)}, {m}, dt)); // Prior factor on c1 - hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8}))); + hfg.add(HybridDiscreteFactor(DecisionTreeFactor(m, {2, 8}))); // Get a constrained ordering keeping c1 last - auto ordering_full = Ordering::ColamdConstrainedLast(hfg, {C(1)}); + auto ordering_full = Ordering::ColamdConstrainedLast(hfg, {M(1)}); // Returns a Hybrid Bayes Tree with distribution P(x0|x1)P(x1|c1)P(c1) HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full); @@ -232,48 +234,48 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { { hfg.add(GaussianMixtureFactor::FromFactors( - {X(0)}, {{C(0), 2}}, + {X(0)}, {{M(0), 2}}, {boost::make_shared(X(0), I_3x3, Z_3x1), boost::make_shared(X(0), I_3x3, Vector3::Ones())})); DecisionTree dt1( - C(1), boost::make_shared(X(2), I_3x3, Z_3x1), + M(1), boost::make_shared(X(2), I_3x3, Z_3x1), boost::make_shared(X(2), I_3x3, Vector3::Ones())); - hfg.add(GaussianMixtureFactor({X(2)}, {{C(1), 2}}, dt1)); + hfg.add(GaussianMixtureFactor({X(2)}, {{M(1), 2}}, dt1)); } hfg.add(HybridDiscreteFactor( - DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4"))); + DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4"))); hfg.add(JacobianFactor(X(3), I_3x3, X(4), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1)); { DecisionTree dt( - C(3), boost::make_shared(X(3), I_3x3, Z_3x1), + M(3), boost::make_shared(X(3), I_3x3, Z_3x1), boost::make_shared(X(3), I_3x3, Vector3::Ones())); - hfg.add(GaussianMixtureFactor({X(3)}, {{C(3), 2}}, dt)); + hfg.add(GaussianMixtureFactor({X(3)}, {{M(3), 2}}, dt)); DecisionTree dt1( - C(2), boost::make_shared(X(5), I_3x3, Z_3x1), + M(2), boost::make_shared(X(5), I_3x3, Z_3x1), boost::make_shared(X(5), I_3x3, Vector3::Ones())); - hfg.add(GaussianMixtureFactor({X(5)}, {{C(2), 2}}, dt1)); + hfg.add(GaussianMixtureFactor({X(5)}, {{M(2), 2}}, dt1)); } auto ordering_full = - Ordering::ColamdConstrainedLast(hfg, {C(0), C(1), C(2), C(3)}); + Ordering::ColamdConstrainedLast(hfg, {M(0), M(1), M(2), M(3)}); HybridBayesTree::shared_ptr hbt; HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full); - // GTSAM_PRINT(*hbt); - // GTSAM_PRINT(*remaining); + // 9 cliques in the bayes tree and 0 remaining variables to eliminate. + EXPECT_LONGS_EQUAL(9, hbt->size()); + EXPECT_LONGS_EQUAL(0, remaining->size()); - // hbt->marginalFactor(X(1))->print("HBT: "); /* (Fan) Explanation: the Junction tree will need to reeliminate to get to the marginal on X(1), which is not possible because it involves eliminating @@ -307,13 +309,13 @@ TEST(HybridGaussianFactorGraph, Switching) { // X(3), X(7) // X(2), X(8) // X(1), X(4), X(6), X(9) - // C(5) will be the center, C(1-4), C(6-8) - // C(3), C(7) - // C(1), C(4), C(2), C(6), C(8) + // M(5) will be the center, M(1-4), M(6-8) + // M(3), M(7) + // M(1), M(4), M(2), M(6), M(8) // auto ordering_full = // Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), // X(5), - // C(1), C(4), C(2), C(6), C(8), C(3), C(7), C(5)}); + // M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)}); KeyVector ordering; { @@ -336,7 +338,7 @@ TEST(HybridGaussianFactorGraph, Switching) { std::iota(naturalC.begin(), naturalC.end(), 1); std::vector ordC; std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC), - [](int x) { return C(x); }); + [](int x) { return M(x); }); KeyVector ndC; std::vector lvls; @@ -353,9 +355,9 @@ TEST(HybridGaussianFactorGraph, Switching) { HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); - // GTSAM_PRINT(*hbt); - // GTSAM_PRINT(*remaining); - // hbt->marginalFactor(C(11))->print("HBT: "); + // 12 cliques in the bayes tree and 0 remaining variables to eliminate. + EXPECT_LONGS_EQUAL(12, hbt->size()); + EXPECT_LONGS_EQUAL(0, remaining->size()); } /* ************************************************************************* */ @@ -368,13 +370,13 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) { // X(3), X(7) // X(2), X(8) // X(1), X(4), X(6), X(9) - // C(5) will be the center, C(1-4), C(6-8) - // C(3), C(7) - // C(1), C(4), C(2), C(6), C(8) + // M(5) will be the center, M(1-4), M(6-8) + // M(3), M(7) + // M(1), M(4), M(2), M(6), M(8) // auto ordering_full = // Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), // X(5), - // C(1), C(4), C(2), C(6), C(8), C(3), C(7), C(5)}); + // M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)}); KeyVector ordering; { @@ -397,7 +399,7 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) { std::iota(naturalC.begin(), naturalC.end(), 1); std::vector ordC; std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC), - [](int x) { return C(x); }); + [](int x) { return M(x); }); KeyVector ndC; std::vector lvls; @@ -407,9 +409,6 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) { } auto ordering_full = Ordering(ordering); - // GTSAM_PRINT(*hfg); - // GTSAM_PRINT(ordering_full); - HybridBayesTree::shared_ptr hbt; HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); @@ -417,19 +416,18 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) { auto new_fg = makeSwitchingChain(12); auto isam = HybridGaussianISAM(*hbt); - { - HybridGaussianFactorGraph factorGraph; - factorGraph.push_back(new_fg->at(new_fg->size() - 2)); - factorGraph.push_back(new_fg->at(new_fg->size() - 1)); - isam.update(factorGraph); - // std::cout << isam.dot(); - // isam.marginalFactor(C(11))->print(); - } + // Run an ISAM update. + HybridGaussianFactorGraph factorGraph; + factorGraph.push_back(new_fg->at(new_fg->size() - 2)); + factorGraph.push_back(new_fg->at(new_fg->size() - 1)); + isam.update(factorGraph); + + // ISAM should have 12 factors after the last update + EXPECT_LONGS_EQUAL(12, isam.size()); } /* ************************************************************************* */ -// TODO(Varun) Actually test something! -TEST_DISABLED(HybridGaussianFactorGraph, SwitchingTwoVar) { +TEST(HybridGaussianFactorGraph, SwitchingTwoVar) { const int N = 7; auto hfg = makeSwitchingChain(N, X); hfg->push_back(*makeSwitchingChain(N, Y, D)); @@ -449,7 +447,7 @@ TEST_DISABLED(HybridGaussianFactorGraph, SwitchingTwoVar) { } for (size_t i = 1; i <= N - 1; i++) { - ordX.emplace_back(C(i)); + ordX.emplace_back(M(i)); } for (size_t i = 1; i <= N - 1; i++) { ordX.emplace_back(D(i)); @@ -461,8 +459,8 @@ TEST_DISABLED(HybridGaussianFactorGraph, SwitchingTwoVar) { dw.positionHints['c'] = 0; dw.positionHints['d'] = 3; dw.positionHints['y'] = 2; - std::cout << hfg->dot(DefaultKeyFormatter, dw); - std::cout << "\n"; + // std::cout << hfg->dot(DefaultKeyFormatter, dw); + // std::cout << "\n"; } { @@ -471,10 +469,10 @@ TEST_DISABLED(HybridGaussianFactorGraph, SwitchingTwoVar) { // dw.positionHints['c'] = 0; // dw.positionHints['d'] = 3; dw.positionHints['x'] = 1; - std::cout << "\n"; + // std::cout << "\n"; // std::cout << hfg->eliminateSequential(Ordering(ordX)) // ->dot(DefaultKeyFormatter, dw); - hfg->eliminateMultifrontal(Ordering(ordX))->dot(std::cout); + // hfg->eliminateMultifrontal(Ordering(ordX))->dot(std::cout); } Ordering ordering_partial; @@ -482,22 +480,22 @@ TEST_DISABLED(HybridGaussianFactorGraph, SwitchingTwoVar) { ordering_partial.emplace_back(X(i)); ordering_partial.emplace_back(Y(i)); } - { - HybridBayesNet::shared_ptr hbn; - HybridGaussianFactorGraph::shared_ptr remaining; - std::tie(hbn, remaining) = - hfg->eliminatePartialSequential(ordering_partial); + HybridBayesNet::shared_ptr hbn; + HybridGaussianFactorGraph::shared_ptr remaining; + std::tie(hbn, remaining) = + hfg->eliminatePartialSequential(ordering_partial); - // remaining->print(); - { - DotWriter dw; - dw.positionHints['x'] = 1; - dw.positionHints['c'] = 0; - dw.positionHints['d'] = 3; - dw.positionHints['y'] = 2; - std::cout << remaining->dot(DefaultKeyFormatter, dw); - std::cout << "\n"; - } + EXPECT_LONGS_EQUAL(14, hbn->size()); + EXPECT_LONGS_EQUAL(11, remaining->size()); + + { + DotWriter dw; + dw.positionHints['x'] = 1; + dw.positionHints['c'] = 0; + dw.positionHints['d'] = 3; + dw.positionHints['y'] = 2; + // std::cout << remaining->dot(DefaultKeyFormatter, dw); + // std::cout << "\n"; } }