diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 55fa9a908..c024c1255 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -184,6 +184,19 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // sum out frontals, this is the factor on the separator GaussianMixtureFactor::Sum sum = sumFrontals(factors); + // If a tree leaf contains nullptr, + // convert that leaf to an empty GaussianFactorGraph. + // Needed since the DecisionTree will otherwise create + // a GFG with a single (null) factor. + auto emptyGaussian = [](const GaussianFactorGraph &gfg) { + bool hasNull = + std::any_of(gfg.begin(), gfg.end(), + [](const GaussianFactor::shared_ptr &ptr) { return !ptr; }); + + return hasNull ? GaussianFactorGraph() : gfg; + }; + sum = GaussianMixtureFactor::Sum(sum, emptyGaussian); + using EliminationPair = GaussianFactorGraph::EliminationResult; KeyVector keysOfEliminated; // Not the ordering @@ -195,7 +208,10 @@ hybridElimination(const HybridGaussianFactorGraph &factors, if (graph.empty()) { return {nullptr, nullptr}; } - auto result = EliminatePreferCholesky(graph, frontalKeys); + std::pair, + boost::shared_ptr> + result = EliminatePreferCholesky(graph, frontalKeys); + if (keysOfEliminated.empty()) { keysOfEliminated = result.first->keys(); // Initialize the keysOfEliminated to be the @@ -235,14 +251,27 @@ hybridElimination(const HybridGaussianFactorGraph &factors, boost::make_shared(discreteFactor)}; } else { - // Create a resulting DCGaussianMixture on the separator. + // Create a resulting GaussianMixtureFactor on the separator. auto factor = boost::make_shared( KeyVector(continuousSeparator.begin(), continuousSeparator.end()), discreteSeparator, separatorFactors); return {boost::make_shared(conditional), factor}; } } -/* ************************************************************************ */ +/* ************************************************************************ + * Function to eliminate variables **under the following assumptions**: + * 1. When the ordering is fully continuous, and the graph only contains + * continuous and hybrid factors + * 2. When the ordering is fully discrete, and the graph only contains discrete + * factors + * + * Any usage outside of this is considered incorrect. + * + * \warning This function is not meant to be used with arbitrary hybrid factor + * graphs. For example, if there exists continuous parents, and one tries to + * eliminate a discrete variable (as specified in the ordering), the result will + * be INCORRECT and there will be NO error raised. + */ std::pair // EliminateHybrid(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index df7803722..23a95c021 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -75,8 +75,6 @@ void HybridGaussianISAM::updateInternal( std::copy(allDiscrete.begin(), allDiscrete.end(), std::back_inserter(newKeysDiscreteLast)); - // KeyVector new - // Get an ordering where the new keys are eliminated last const VariableIndex index(factors); Ordering elimination_ordering; @@ -107,6 +105,22 @@ void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors, this->updateInternal(newFactors, &orphans, ordering, function); } +/* ************************************************************************* */ +/** + * @brief Check if `b` is a subset of `a`. + * Non-const since they need to be sorted. + * + * @param a KeyVector + * @param b KeyVector + * @return True if the keys of b is a subset of a, else false. + */ +bool IsSubset(KeyVector a, KeyVector b) { + std::sort(a.begin(), a.end()); + std::sort(b.begin(), b.end()); + return std::includes(a.begin(), a.end(), b.begin(), b.end()); +} + +/* ************************************************************************* */ void HybridGaussianISAM::prune(const Key& root, const size_t maxNrLeaves) { auto decisionTree = boost::dynamic_pointer_cast( this->clique(root)->conditional()->inner()); @@ -133,7 +147,7 @@ void HybridGaussianISAM::prune(const Key& root, const size_t maxNrLeaves) { parents.push_back(parent); } - if (parents == decisionTree->keys()) { + if (IsSubset(parents, decisionTree->keys())) { auto gaussianMixture = boost::dynamic_pointer_cast( clique.second->conditional()->inner()); diff --git a/gtsam/hybrid/tests/testHybridIncremental.cpp b/gtsam/hybrid/tests/testHybridIncremental.cpp index afd0d0a70..4449aba0b 100644 --- a/gtsam/hybrid/tests/testHybridIncremental.cpp +++ b/gtsam/hybrid/tests/testHybridIncremental.cpp @@ -61,11 +61,6 @@ TEST(HybridGaussianElimination, IncrementalElimination) { 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); @@ -85,11 +80,6 @@ TEST(HybridGaussianElimination, IncrementalElimination) { 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 @@ -336,12 +326,6 @@ TEST(HybridGaussianElimination, Incremental_approximate) { graph1.push_back(switching.linearizedFactorGraph.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); @@ -364,10 +348,6 @@ TEST(HybridGaussianElimination, Incremental_approximate) { graph2.push_back(switching.linearizedFactorGraph.at(4)); graph2.push_back(switching.linearizedFactorGraph.at(8)); - Ordering ordering2; - ordering2 += X(4); - ordering2 += X(5); - // Run update with pruning a second time. incrementalHybrid.update(graph2); incrementalHybrid.prune(M(4), maxComponents); @@ -382,247 +362,198 @@ TEST(HybridGaussianElimination, Incremental_approximate) { } /* ************************************************************************/ -// Test for figuring out the optimal ordering to ensure we get -// a discrete graph after elimination. +// 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. TEST(HybridGaussianISAM, 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 ***************/ HybridNonlinearFactorGraph 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); + // 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); + // 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. + // 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); + // 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)); + // 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 = HybridNonlinearFactorGraph(); + HybridGaussianFactorGraph gfg = fg.linearize(initial); + fg = HybridNonlinearFactorGraph(); - // HybridGaussianISAM inc; + HybridGaussianISAM 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); - // // 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; - // /*************** 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 mixtureFactor = boost::make_shared( + contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); + fg.push_back(mixtureFactor); - // // 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); - // // 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)); - // 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)); + gfg = fg.linearize(initial); + fg = HybridNonlinearFactorGraph(); - // // 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); + // 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 1 level tree for time indices (1,) with 2 leaves. + inc.update(gfg); - // gfg = fg.linearize(initial); - // fg = HybridNonlinearFactorGraph(); + /*************** 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}; + mixtureFactor = boost::make_shared( + contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); + fg.push_back(mixtureFactor); - // // 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); + // 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); - // /*************** 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); + 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)); - // // 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); + gfg = fg.linearize(initial); + fg = HybridNonlinearFactorGraph(); - // 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)); + // 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 2 level tree on (1, 2). + // 1 has 2 choices, and 2 has 4 choices. + inc.update(gfg); + inc.prune(M(2), 2); - // // 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); + /*************** 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}; + mixtureFactor = boost::make_shared( + contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); + fg.push_back(mixtureFactor); - // gfg = fg.linearize(initial); - // fg = HybridNonlinearFactorGraph(); + // 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); - // // 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); + 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)); - // /*************** 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); + gfg = fg.linearize(initial); + fg = HybridNonlinearFactorGraph(); - // // 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); + // Keep pruning! + inc.update(gfg); + inc.prune(M(3), 3); - // 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)); + // The final discrete graph should not be empty since we have eliminated + // all continuous variables. + auto discreteTree = inc[M(3)]->conditional()->asDiscreteConditional(); + EXPECT_LONGS_EQUAL(3, discreteTree->size()); - // // 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); + // Test if the optimal discrete mode assignment is (1, 1, 1). + DiscreteFactorGraph discreteGraph; + discreteGraph.push_back(discreteTree); + DiscreteValues optimal_assignment = discreteGraph.optimize(); - // gfg = fg.linearize(initial); - // fg = HybridNonlinearFactorGraph(); + DiscreteValues expected_assignment; + expected_assignment[M(1)] = 1; + expected_assignment[M(2)] = 1; + expected_assignment[M(3)] = 1; - // // Keep pruning! - // inc.update(gfg, ordering, 3); + EXPECT(assert_equal(expected_assignment, optimal_assignment)); - // // 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()); + // Test if pruning worked correctly by checking that we only have 3 leaves in + // the last node. + auto lastConditional = inc[X(3)]->conditional()->asMixture(); + EXPECT_LONGS_EQUAL(3, lastConditional->nrComponents()); } /* ************************************************************************* */