diff --git a/gtsam/hybrid/tests/testHybridIncremental.cpp b/gtsam/hybrid/tests/testHybridIncremental.cpp new file mode 100644 index 000000000..420dcd339 --- /dev/null +++ b/gtsam/hybrid/tests/testHybridIncremental.cpp @@ -0,0 +1,639 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testHybridIncremental.cpp + * @brief Unit tests for incremental inference + * @author Fan Jiang, Varun Agrawal, Frank Dellaert + * @date Jan 2021 + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "Switching.h" + +// Include for test suite +#include + +using namespace std; +using namespace gtsam; +using noiseModel::Isotropic; +using symbol_shorthand::L; +using symbol_shorthand::M; +using symbol_shorthand::W; +using symbol_shorthand::X; +using symbol_shorthand::Y; +using symbol_shorthand::Z; + +/* ****************************************************************************/ +// Test if we can perform elimination incrementally. +TEST(HybridGaussianElimination, IncrementalElimination) { + Switching switching(3); + HybridGaussianISAM isam; + HybridGaussianFactorGraph graph1; + + // Create initial factor graph + // * * * + // | | | + // *- X1 -*- X2 -*- X3 + // \*-M1-*/ + graph1.push_back(switching.linearizedFactorGraph.at(0)); // P(X1) + graph1.push_back(switching.linearizedFactorGraph.at(1)); // P(X1, X2 | M1) + graph1.push_back(switching.linearizedFactorGraph.at(2)); // P(X2, X3 | M2) + graph1.push_back(switching.linearizedFactorGraph.at(5)); // P(M1) + + // Create ordering. + Ordering ordering; + ordering += X(1); + ordering += X(2); + + // Run update step + isam.update(graph1); + + // Check that after update we have 3 hybrid Bayes net nodes: + // P(X1 | X2, M1) and P(X2, X3 | M1, M2), P(M1, M2) + EXPECT_LONGS_EQUAL(3, isam.size()); + EXPECT(isam[X(1)]->conditional()->frontals() == KeyVector{X(1)}); + EXPECT(isam[X(1)]->conditional()->parents() == KeyVector({X(2), M(1)})); + EXPECT(isam[X(2)]->conditional()->frontals() == KeyVector({X(2), X(3)})); + EXPECT(isam[X(2)]->conditional()->parents() == KeyVector({M(1), M(2)})); + + /********************************************************/ + // New factor graph for incremental update. + HybridGaussianFactorGraph graph2; + + graph1.push_back(switching.linearizedFactorGraph.at(3)); // P(X2) + graph2.push_back(switching.linearizedFactorGraph.at(4)); // P(X3) + graph2.push_back(switching.linearizedFactorGraph.at(6)); // P(M1, M2) + + // Create ordering. + Ordering ordering2; + ordering2 += X(2); + ordering2 += X(3); + + isam.update(graph2); + + // Check that after the second update we have + // 1 additional hybrid Bayes net node: + // P(X2, X3 | M1, M2) + EXPECT_LONGS_EQUAL(3, isam.size()); + EXPECT(isam[X(3)]->conditional()->frontals() == KeyVector({X(2), X(3)})); + EXPECT(isam[X(3)]->conditional()->parents() == KeyVector({M(1), M(2)})); +} + +/* ****************************************************************************/ +// Test if we can incrementally do the inference +TEST(HybridGaussianElimination, IncrementalInference) { + Switching switching(3); + HybridGaussianISAM isam; + HybridGaussianFactorGraph graph1; + + // Create initial factor graph + // * * * + // | | | + // *- X1 -*- X2 -*- X3 + // \*-M1-*/ + graph1.push_back(switching.linearizedFactorGraph.at(0)); // P(X1) + graph1.push_back(switching.linearizedFactorGraph.at(1)); // P(X1, X2 | M1) + graph1.push_back(switching.linearizedFactorGraph.at(2)); // P(X2, X3 | M2) + graph1.push_back(switching.linearizedFactorGraph.at(5)); // P(M1) + + // Create ordering. + Ordering ordering; + ordering += X(1); + ordering += X(2); + + // Run update step + isam.update(graph1); + + /********************************************************/ + // New factor graph for incremental update. + HybridGaussianFactorGraph graph2; + + graph1.push_back(switching.linearizedFactorGraph.at(3)); // P(X2) + graph2.push_back(switching.linearizedFactorGraph.at(4)); // P(X3) + graph2.push_back(switching.linearizedFactorGraph.at(6)); // P(M1, M2) + + // Create ordering. + Ordering ordering2; + ordering2 += X(2); + ordering2 += X(3); + + isam.update(graph2); + + /********************************************************/ + // Run batch elimination so we can compare results. + ordering.clear(); + ordering += X(1); + ordering += X(2); + ordering += X(3); + + // Now we calculate the actual factors using full elimination + HybridBayesNet::shared_ptr expectedHybridBayesNet; + HybridGaussianFactorGraph::shared_ptr expectedRemainingGraph; + std::tie(expectedHybridBayesNet, expectedRemainingGraph) = + switching.linearizedFactorGraph.eliminatePartialSequential(ordering); + + // The densities on X(1) should be the same + expectedHybridBayesNet->atGaussian(0)->print(); + auto x1_conditional = isam[X(1)]->conditional(); + GTSAM_PRINT(*x1_conditional); + // EXPECT(assert_equal(*(hybridBayesNet.atGaussian(0)), + // *(expectedHybridBayesNet->atGaussian(0)))); + + // // The densities on X(2) should be the same + // EXPECT(assert_equal(*(hybridBayesNet2.atGaussian(1)), + // *(expectedHybridBayesNet->atGaussian(1)))); + + // // The densities on X(3) should be the same + // EXPECT(assert_equal(*(hybridBayesNet2.atGaussian(2)), + // *(expectedHybridBayesNet->atGaussian(2)))); + + // // we only do the manual continuous elimination for 0,0 + // // the other discrete probabilities on M(2) are calculated the same way + // auto m00_prob = [&]() { + // GaussianFactorGraph gf; + // gf.add(switching.linearizedFactorGraph.gaussianGraph().at(3)); + + // DiscreteValues m00; + // m00[M(1)] = 0, m00[M(2)] = 0; + // auto dcMixture = + // dynamic_pointer_cast(graph2.dcGraph().at(0)); + // gf.add(dcMixture->factors()(m00)); + // auto x2_mixed = + // boost::dynamic_pointer_cast(hybridBayesNet.at(1)); + // gf.add(x2_mixed->factors()(m00)); + // auto result_gf = gf.eliminateSequential(); + // return gf.probPrime(result_gf->optimize()); + // }(); + + /// 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, (*discreteFactor)(assignment), 1e-5)); +// assignment[M(1)] = 1; +// assignment[M(2)] = 0; +// EXPECT(assert_equal(0.612477, (*discreteFactor)(assignment), 1e-5)); +// assignment[M(1)] = 0; +// assignment[M(2)] = 1; +// EXPECT(assert_equal(0.999952, (*discreteFactor)(assignment), 1e-5)); +// assignment[M(1)] = 1; +// assignment[M(2)] = 1; +// EXPECT(assert_equal(1.0, (*discreteFactor)(assignment), 1e-5)); + +// DiscreteFactorGraph dfg; +// dfg.add(*discreteFactor); +// dfg.add(discreteFactor_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)); +} + +/* ****************************************************************************/ +// // Test if we can approximately do the inference +// TEST(DCGaussianElimination, Approx_inference) { +// Switching switching(4); +// IncrementalHybrid incrementalHybrid; +// HybridGaussianFactorGraph graph1; + +// // Add the 3 DC factors, x1-x2, x2-x3, x3-x4 +// for (size_t i = 0; i < 3; i++) { +// graph1.push_back(switching.linearizedFactorGraph.dcGraph().at(i)); +// } + +// // Add the Gaussian factors, 1 prior on X(1), 4 measurements +// for (size_t i = 0; i <= 4; i++) { +// graph1.push_back(switching.linearizedFactorGraph.gaussianGraph().at(i)); +// } + +// // Create ordering. +// Ordering ordering; +// for (size_t j = 1; j <= 4; j++) { +// ordering += X(j); +// } + +// // Now we calculate the actual factors using full elimination +// HybridBayesNet::shared_ptr unprunedHybridBayesNet; +// HybridGaussianFactorGraph::shared_ptr unprunedRemainingGraph; +// std::tie(unprunedHybridBayesNet, unprunedRemainingGraph) = +// switching.linearizedFactorGraph.eliminatePartialSequential(ordering); + +// size_t maxComponents = 5; +// incrementalHybrid.update(graph1, ordering, maxComponents); + +// /* +// unpruned factor is: +// Choice(m3) +// 0 Choice(m2) +// 0 0 Choice(m1) +// 0 0 0 Leaf 0.2248 - +// 0 0 1 Leaf 0.3715 - +// 0 1 Choice(m1) +// 0 1 0 Leaf 0.3742 * +// 0 1 1 Leaf 0.6125 * +// 1 Choice(m2) +// 1 0 Choice(m1) +// 1 0 0 Leaf 0.3706 - +// 1 0 1 Leaf 0.6124 * +// 1 1 Choice(m1) +// 1 1 0 Leaf 0.611 * +// 1 1 1 Leaf 1 * + +// pruned factors is: +// Choice(m3) +// 0 Choice(m2) +// 0 0 Leaf 0 +// 0 1 Choice(m1) +// 0 1 0 Leaf 0.3742 +// 0 1 1 Leaf 0.6125 +// 1 Choice(m2) +// 1 0 Choice(m1) +// 1 0 0 Leaf 0 +// 1 0 1 Leaf 0.6124 +// 1 1 Choice(m1) +// 1 1 0 Leaf 0.611 +// 1 1 1 Leaf 1 +// */ + +// // Test that the remaining factor graph has one +// // DecisionTreeFactor on {M3, M2, M1}. +// auto remainingFactorGraph = incrementalHybrid.remainingFactorGraph(); +// EXPECT_LONGS_EQUAL(1, remainingFactorGraph.size()); + +// auto discreteFactor_m1 = *dynamic_pointer_cast( +// incrementalHybrid.remainingDiscreteGraph().at(0)); +// EXPECT(discreteFactor_m1.keys() == KeyVector({M(3), M(2), M(1)})); + +// // Get the number of elements which are equal to 0. +// auto count = [](const double &value, int count) { +// return value > 0 ? count + 1 : count; +// }; +// // Check that the number of leaves after pruning is 5. +// EXPECT_LONGS_EQUAL(5, discreteFactor_m1.fold(count, 0)); + +// /* Expected hybrid Bayes net +// * factor 0: [x1 | x2 m1 ], 2 components +// * factor 1: [x2 | x3 m2 m1 ], 4 components +// * factor 2: [x3 | x4 m3 m2 m1 ], 8 components +// * factor 3: [x4 | m3 m2 m1 ], 8 components +// */ +// auto hybridBayesNet = incrementalHybrid.hybridBayesNet(); + +// // Check if we have a bayes net with 4 hybrid nodes, +// // each with 2, 4, 5 (pruned), and 5 (pruned) leaves respetively. +// EXPECT_LONGS_EQUAL(4, hybridBayesNet.size()); +// EXPECT_LONGS_EQUAL(2, hybridBayesNet.atGaussian(0)->nrComponents()); +// EXPECT_LONGS_EQUAL(4, hybridBayesNet.atGaussian(1)->nrComponents()); +// EXPECT_LONGS_EQUAL(5, hybridBayesNet.atGaussian(2)->nrComponents()); +// EXPECT_LONGS_EQUAL(5, hybridBayesNet.atGaussian(3)->nrComponents()); + +// // Check that the hybrid nodes of the bayes net match those of the bayes +// net +// // before pruning, at the same positions. +// auto &lastDensity = *(hybridBayesNet.atGaussian(3)); +// auto &unprunedLastDensity = *(unprunedHybridBayesNet->atGaussian(3)); +// std::vector> assignments = +// discreteFactor_m1.enumerate(); +// // Loop over all assignments and check the pruned components +// for (auto &&av : assignments) { +// const DiscreteValues &assignment = av.first; +// const double value = av.second; + +// if (value == 0.0) { +// EXPECT(lastDensity(assignment) == nullptr); +// } else { +// CHECK(lastDensity(assignment)); +// EXPECT(assert_equal(*unprunedLastDensity(assignment), +// *lastDensity(assignment))); +// } +// } +// } + +/* ****************************************************************************/ +// // Test approximate inference with an additional pruning step. +// TEST(DCGaussianElimination, Incremental_approximate) { +// Switching switching(5); +// IncrementalHybrid incrementalHybrid; +// HybridGaussianFactorGraph graph1; + +// // Add the 3 DC factors, x1-x2, x2-x3, x3-x4 +// for (size_t i = 0; i < 3; i++) { +// graph1.push_back(switching.linearizedFactorGraph.dcGraph().at(i)); +// } + +// // Add the Gaussian factors, 1 prior on X(1), 4 measurements +// for (size_t i = 0; i <= 4; i++) { +// graph1.push_back(switching.linearizedFactorGraph.gaussianGraph().at(i)); +// } + +// // Create ordering. +// Ordering ordering; +// for (size_t j = 1; j <= 4; j++) { +// ordering += X(j); +// } + +// // Run update with pruning +// size_t maxComponents = 5; +// incrementalHybrid.update(graph1, ordering, maxComponents); + +// // Check if we have a bayes net with 4 hybrid nodes, +// // each with 2, 4, 8, and 5 (pruned) leaves respetively. +// auto actualBayesNet1 = incrementalHybrid.hybridBayesNet(); +// CHECK_EQUAL(4, actualBayesNet1.size()); +// EXPECT_LONGS_EQUAL(2, actualBayesNet1.atGaussian(0)->nrComponents()); +// EXPECT_LONGS_EQUAL(4, actualBayesNet1.atGaussian(1)->nrComponents()); +// EXPECT_LONGS_EQUAL(8, actualBayesNet1.atGaussian(2)->nrComponents()); +// EXPECT_LONGS_EQUAL(5, actualBayesNet1.atGaussian(3)->nrComponents()); + +// /***** Run Round 2 *****/ +// HybridGaussianFactorGraph graph2; +// graph2.push_back(switching.linearizedFactorGraph.dcGraph().at(3)); +// graph2.push_back(switching.linearizedFactorGraph.gaussianGraph().at(5)); + +// Ordering ordering2; +// ordering2 += X(4); +// ordering2 += X(5); + +// // Run update with pruning a second time. +// incrementalHybrid.update(graph2, ordering2, maxComponents); + +// // Check if we have a bayes net with 2 hybrid nodes, +// // each with 10 (pruned), and 5 (pruned) leaves respetively. +// auto actualBayesNet = incrementalHybrid.hybridBayesNet(); +// 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) { +// // 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 +// and +// // W is the foot. +// // We use BetweenFactor as constraints between each of the poses. + +// /*************** Run Round 1 ***************/ +// NonlinearHybridFactorGraph fg; + +// // Add a prior on pose x1 at the origin. +// // A prior factor consists of a mean and +// // a noise model (covariance matrix) +// Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin +// auto priorNoise = noiseModel::Diagonal::Sigmas( +// Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta +// fg.emplace_nonlinear>(X(0), prior, priorNoise); + +// // create a noise model for the landmark measurements +// auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1); + +// // We model a robot's single leg as X - Y - Z - W +// // where X is the base link and W is the foot link. + +// // Add connecting poses similar to PoseFactors in GTD +// fg.emplace_nonlinear>(X(0), Y(0), Pose2(0, 1.0, 0), +// poseNoise); +// fg.emplace_nonlinear>(Y(0), Z(0), Pose2(0, 1.0, 0), +// poseNoise); +// fg.emplace_nonlinear>(Z(0), W(0), Pose2(0, 1.0, 0), +// poseNoise); + +// // Create initial estimate +// Values initial; +// initial.insert(X(0), Pose2(0.0, 0.0, 0.0)); +// initial.insert(Y(0), Pose2(0.0, 1.0, 0.0)); +// initial.insert(Z(0), Pose2(0.0, 2.0, 0.0)); +// initial.insert(W(0), Pose2(0.0, 3.0, 0.0)); + +// HybridGaussianFactorGraph gfg = fg.linearize(initial); +// fg = NonlinearHybridFactorGraph(); + +// IncrementalHybrid inc; + +// // Regular ordering going up the chain. +// Ordering ordering; +// ordering += W(0); +// ordering += Z(0); +// ordering += Y(0); +// ordering += X(0); + +// // Update without pruning +// // The result is a HybridBayesNet with no discrete variables +// // (equivalent to a GaussianBayesNet). +// // Factorization is: +// // `P(X | measurements) = P(W0|Z0) P(Z0|Y0) P(Y0|X0) P(X0)` +// inc.update(gfg, ordering); + +// /*************** Run Round 2 ***************/ +// using PlanarMotionModel = BetweenFactor; + +// // Add odometry factor with discrete modes. +// Pose2 odometry(1.0, 0.0, 0.0); +// KeyVector contKeys = {W(0), W(1)}; +// auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); +// auto still = boost::make_shared(W(0), W(1), Pose2(0, 0, +// 0), +// noise_model), +// moving = boost::make_shared(W(0), W(1), odometry, +// noise_model); +// std::vector components = {moving, still}; +// auto dcFactor = boost::make_shared>( +// contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); +// fg.push_back(dcFactor); + +// // Add equivalent of ImuFactor +// fg.emplace_nonlinear>(X(0), X(1), Pose2(1.0, 0.0, 0), +// poseNoise); +// // PoseFactors-like at k=1 +// fg.emplace_nonlinear>(X(1), Y(1), Pose2(0, 1, 0), +// poseNoise); +// fg.emplace_nonlinear>(Y(1), Z(1), Pose2(0, 1, 0), +// poseNoise); +// fg.emplace_nonlinear>(Z(1), W(1), Pose2(-1, 1, 0), +// poseNoise); + +// initial.insert(X(1), Pose2(1.0, 0.0, 0.0)); +// initial.insert(Y(1), Pose2(1.0, 1.0, 0.0)); +// initial.insert(Z(1), Pose2(1.0, 2.0, 0.0)); +// // The leg link did not move so we set the expected pose accordingly. +// initial.insert(W(1), Pose2(0.0, 3.0, 0.0)); + +// // Ordering for k=1. +// // This ordering follows the intuition that we eliminate the previous +// // timestep, and then the current timestep. +// ordering = Ordering(); +// ordering += W(0); +// ordering += Z(0); +// ordering += Y(0); +// ordering += X(0); +// ordering += W(1); +// ordering += Z(1); +// ordering += Y(1); +// ordering += X(1); + +// gfg = fg.linearize(initial); +// fg = NonlinearHybridFactorGraph(); + +// // Update without pruning +// // The result is a HybridBayesNet with 1 discrete variable M(1). +// // P(X | measurements) = P(W0|Z0, W1, M1) P(Z0|Y0, W1, M1) P(Y0|X0, W1, M1) +// // P(X0 | X1, W1, M1) P(W1|Z1, X1, M1) P(Z1|Y1, X1, +// M1) +// // P(Y1 | X1, M1)P(X1 | M1)P(M1) +// // The MHS tree is a 2 level tree for time indices (0, 1) with 2 leaves. +// inc.update(gfg, ordering); + +// /*************** Run Round 3 ***************/ +// // Add odometry factor with discrete modes. +// contKeys = {W(1), W(2)}; +// still = boost::make_shared(W(1), W(2), Pose2(0, 0, 0), +// noise_model); +// moving = +// boost::make_shared(W(1), W(2), odometry, +// noise_model); +// components = {moving, still}; +// dcFactor = boost::make_shared>( +// contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); +// fg.push_back(dcFactor); + +// // Add equivalent of ImuFactor +// fg.emplace_nonlinear>(X(1), X(2), Pose2(1.0, 0.0, 0), +// poseNoise); +// // PoseFactors-like at k=1 +// fg.emplace_nonlinear>(X(2), Y(2), Pose2(0, 1, 0), +// poseNoise); +// fg.emplace_nonlinear>(Y(2), Z(2), Pose2(0, 1, 0), +// poseNoise); +// fg.emplace_nonlinear>(Z(2), W(2), Pose2(-2, 1, 0), +// poseNoise); + +// initial.insert(X(2), Pose2(2.0, 0.0, 0.0)); +// initial.insert(Y(2), Pose2(2.0, 1.0, 0.0)); +// initial.insert(Z(2), Pose2(2.0, 2.0, 0.0)); +// initial.insert(W(2), Pose2(0.0, 3.0, 0.0)); + +// // Ordering at k=2. Same intuition as before. +// ordering = Ordering(); +// ordering += W(1); +// ordering += Z(1); +// ordering += Y(1); +// ordering += X(1); +// ordering += W(2); +// ordering += Z(2); +// ordering += Y(2); +// ordering += X(2); + +// gfg = fg.linearize(initial); +// fg = NonlinearHybridFactorGraph(); + +// // Now we prune! +// // P(X | measurements) = P(W0|Z0, W1, M1) P(Z0|Y0, W1, M1) P(Y0|X0, W1, M1) +// P(X0 | X1, W1, M1) +// // P(W1|W2, Z1, X1, M1, M2) P(Z1| W2, Y1, X1, M1, M2) +// P(Y1 | W2, X1, M1, M2) +// // P(X1 | W2, X2, M1, M2) P(W2|Z2, X2, M1, M2) +// P(Z2|Y2, X2, M1, M2) +// // P(Y2 | X2, M1, M2)P(X2 | M1, M2) P(M1, M2) +// // The MHS at this point should be a 3 level tree on (0, 1, 2). +// // 0 has 2 choices, 1 has 4 choices and 2 has 4 choices. +// inc.update(gfg, ordering, 2); + +// /*************** Run Round 4 ***************/ +// // Add odometry factor with discrete modes. +// contKeys = {W(2), W(3)}; +// still = boost::make_shared(W(2), W(3), Pose2(0, 0, 0), +// noise_model); +// moving = +// boost::make_shared(W(2), W(3), odometry, +// noise_model); +// components = {moving, still}; +// dcFactor = boost::make_shared>( +// contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); +// fg.push_back(dcFactor); + +// // Add equivalent of ImuFactor +// fg.emplace_nonlinear>(X(2), X(3), Pose2(1.0, 0.0, 0), +// poseNoise); +// // PoseFactors-like at k=3 +// fg.emplace_nonlinear>(X(3), Y(3), Pose2(0, 1, 0), +// poseNoise); +// fg.emplace_nonlinear>(Y(3), Z(3), Pose2(0, 1, 0), +// poseNoise); +// fg.emplace_nonlinear>(Z(3), W(3), Pose2(-3, 1, 0), +// poseNoise); + +// initial.insert(X(3), Pose2(3.0, 0.0, 0.0)); +// initial.insert(Y(3), Pose2(3.0, 1.0, 0.0)); +// initial.insert(Z(3), Pose2(3.0, 2.0, 0.0)); +// initial.insert(W(3), Pose2(0.0, 3.0, 0.0)); + +// // Ordering at k=3. Same intuition as before. +// ordering = Ordering(); +// ordering += W(2); +// ordering += Z(2); +// ordering += Y(2); +// ordering += X(2); +// ordering += W(3); +// ordering += Z(3); +// ordering += Y(3); +// ordering += X(3); + +// gfg = fg.linearize(initial); +// fg = NonlinearHybridFactorGraph(); + +// // Keep pruning! +// inc.update(gfg, ordering, 3); + +// // The final discrete graph should not be empty since we have eliminated +// // all continuous variables. +// EXPECT(!inc.remainingDiscreteGraph().empty()); + +// // Test if the optimal discrete mode assignment is (1, 1, 1). +// DiscreteValues optimal_assignment = +// inc.remainingDiscreteGraph().optimize(); DiscreteValues +// expected_assignment; expected_assignment[M(1)] = 1; +// expected_assignment[M(2)] = 1; +// expected_assignment[M(3)] = 1; +// EXPECT(assert_equal(expected_assignment, optimal_assignment)); + +// // Test if pruning worked correctly by checking that we only have 3 leaves +// in +// // the last node. +// auto lastConditional = boost::dynamic_pointer_cast( +// inc.hybridBayesNet().at(inc.hybridBayesNet().size() - 1)); +// EXPECT_LONGS_EQUAL(3, lastConditional->nrComponents()); +// } + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +}