diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 40da42412..d199d7611 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -500,6 +500,7 @@ TEST(HybridGaussianFactorGraph, SwitchingTwoVar) { } } +/* ************************************************************************* */ TEST(HybridGaussianFactorGraph, optimize) { HybridGaussianFactorGraph hfg; @@ -521,6 +522,46 @@ TEST(HybridGaussianFactorGraph, optimize) { EXPECT(assert_equal(hv.atDiscrete(C(1)), int(0))); } + +/* ************************************************************************* */ +// Test adding of gaussian conditional and re-elimination. +TEST(HybridGaussianFactorGraph, Conditionals) { + Switching switching(4); + HybridGaussianFactorGraph hfg; + + hfg.push_back(switching.linearizedFactorGraph.at(0)); // P(X1) + Ordering ordering; + ordering.push_back(X(1)); + HybridBayesNet::shared_ptr bayes_net = hfg.eliminateSequential(ordering); + + hfg.push_back(switching.linearizedFactorGraph.at(1)); // P(X1, X2 | M1) + hfg.push_back(*bayes_net); + hfg.push_back(switching.linearizedFactorGraph.at(2)); // P(X2, X3 | M2) + hfg.push_back(switching.linearizedFactorGraph.at(5)); // P(M1) + ordering.push_back(X(2)); + ordering.push_back(X(3)); + ordering.push_back(M(1)); + ordering.push_back(M(2)); + + bayes_net = hfg.eliminateSequential(ordering); + + HybridValues result = bayes_net->optimize(); + + Values expected_continuous; + expected_continuous.insert(X(1), 0); + expected_continuous.insert(X(2), 1); + expected_continuous.insert(X(3), 2); + expected_continuous.insert(X(4), 4); + Values result_continuous = + switching.linearizationPoint.retract(result.continuous()); + EXPECT(assert_equal(expected_continuous, result_continuous)); + + DiscreteValues expected_discrete; + expected_discrete[M(1)] = 1; + expected_discrete[M(2)] = 1; + EXPECT(assert_equal(expected_discrete, result.discrete())); +} + /* ************************************************************************* */ int main() { TestResult tr;