12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1273 from borglab/hybrid-incremental

release/4.3a0
Varun Agrawal 2022-08-22 14:20:42 -04:00 committed by GitHub
parent 4c9c106d45 893c5f77f8
commit 84456f499a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 209 additions and 235 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -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<GaussianConditional>,
boost::shared_ptr<GaussianFactor>>
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<HybridDiscreteFactor>(discreteFactor)};
} else {
// Create a resulting DCGaussianMixture on the separator.
// Create a resulting GaussianMixtureFactor on the separator.
auto factor = boost::make_shared<GaussianMixtureFactor>(
KeyVector(continuousSeparator.begin(), continuousSeparator.end()),
discreteSeparator, separatorFactors);
return {boost::make_shared<HybridConditional>(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<HybridConditional::shared_ptr, HybridFactor::shared_ptr> //
EliminateHybrid(const HybridGaussianFactorGraph &factors,
const Ordering &frontalKeys) {

20
gtsam/hybrid/HybridGaussianISAM.cpp
View File

@ -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<DecisionTreeFactor>(
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<GaussianMixture>(
clique.second->conditional()->inner());

389
gtsam/hybrid/tests/testHybridIncremental.cpp
View File

@ -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<Pose2> 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<Pose2> 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<PriorFactor<Pose2>>(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<PriorFactor<Pose2>>(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<BetweenFactor<Pose2>>(X(0), Y(0), Pose2(0, 1.0, 0),
// poseNoise);
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(0), Z(0), Pose2(0, 1.0, 0),
// poseNoise);
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(0), W(0), Pose2(0, 1.0, 0),
// poseNoise);
// Add connecting poses similar to PoseFactors in GTD
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(0), Y(0), Pose2(0, 1.0, 0),
poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(0), Z(0), Pose2(0, 1.0, 0),
poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(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<Pose2>;
// /*************** Run Round 2 ***************/
// using PlanarMotionModel = BetweenFactor<Pose2>;
// 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<PlanarMotionModel>(W(0), W(1), Pose2(0, 0, 0),
noise_model),
moving = boost::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
noise_model);
std::vector<PlanarMotionModel::shared_ptr> components = {moving, still};
auto mixtureFactor = boost::make_shared<MixtureFactor>(
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<PlanarMotionModel>(W(0), W(1), Pose2(0,
// 0, 0),
// noise_model),
// moving = boost::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
// noise_model);
// std::vector<PlanarMotionModel::shared_ptr> components = {moving, still};
// auto dcFactor = boost::make_shared<DCMixtureFactor<PlanarMotionModel>>(
// contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
// fg.push_back(dcFactor);
// Add equivalent of ImuFactor
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
poseNoise);
// PoseFactors-like at k=1
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(1), Y(1), Pose2(0, 1, 0),
poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(1), Z(1), Pose2(0, 1, 0),
poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(1), W(1), Pose2(-1, 1, 0),
poseNoise);
// // Add equivalent of ImuFactor
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0,
// 0),
// poseNoise);
// // PoseFactors-like at k=1
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(1), Y(1), Pose2(0, 1, 0),
// poseNoise);
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(1), Z(1), Pose2(0, 1, 0),
// poseNoise);
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(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<PlanarMotionModel>(W(1), W(2), Pose2(0, 0, 0),
noise_model);
moving =
boost::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
components = {moving, still};
mixtureFactor = boost::make_shared<MixtureFactor>(
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<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
poseNoise);
// PoseFactors-like at k=1
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(2), Y(2), Pose2(0, 1, 0),
poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(2), Z(2), Pose2(0, 1, 0),
poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(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<PlanarMotionModel>(W(1), W(2), Pose2(0, 0, 0),
// noise_model);
// moving =
// boost::make_shared<PlanarMotionModel>(W(1), W(2), odometry,
// noise_model);
// components = {moving, still};
// dcFactor = boost::make_shared<DCMixtureFactor<PlanarMotionModel>>(
// 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<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0,
// 0),
// poseNoise);
// // PoseFactors-like at k=1
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(2), Y(2), Pose2(0, 1, 0),
// poseNoise);
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(2), Z(2), Pose2(0, 1, 0),
// poseNoise);
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(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<PlanarMotionModel>(W(2), W(3), Pose2(0, 0, 0),
noise_model);
moving =
boost::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
components = {moving, still};
mixtureFactor = boost::make_shared<MixtureFactor>(
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<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),
poseNoise);
// PoseFactors-like at k=3
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(3), Y(3), Pose2(0, 1, 0),
poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(3), Z(3), Pose2(0, 1, 0),
poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(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<PlanarMotionModel>(W(2), W(3), Pose2(0, 0, 0),
// noise_model);
// moving =
// boost::make_shared<PlanarMotionModel>(W(2), W(3), odometry,
// noise_model);
// components = {moving, still};
// dcFactor = boost::make_shared<DCMixtureFactor<PlanarMotionModel>>(
// 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<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0,
// 0),
// poseNoise);
// // PoseFactors-like at k=3
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(3), Y(3), Pose2(0, 1, 0),
// poseNoise);
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(3), Z(3), Pose2(0, 1, 0),
// poseNoise);
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(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<GaussianMixture>(
// 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());
}
/* ************************************************************************* */