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

remove custom orderings, let it happen automatically

release/4.3a0
Varun Agrawal 2022-08-21 12:56:14 -04:00
parent 29c19ee77b
commit f6df641b19
1 changed files with 21 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
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,14 +362,11 @@ TEST(HybridGaussianElimination, Incremental_approximate) {
}
/* ************************************************************************/
// Test for figuring out the optimal ordering to ensure we get
// a discrete graph after elimination.
TEST(HybridGaussianISAM, NonTrivial) {
// This is a GTSAM-only test for running inference on a single legged robot.
// 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) {
/*************** Run Round 1 ***************/
HybridNonlinearFactorGraph fg;
@ -427,19 +404,11 @@ TEST(HybridGaussianISAM, NonTrivial) {
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)`
// TODO(Varun) ClusterTree-inst.h L202 segfaults with custom ordering.
inc.update(gfg);
/*************** Run Round 2 ***************/
@ -478,26 +447,12 @@ TEST(HybridGaussianISAM, NonTrivial) {
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.
// TODO(Varun) ClusterTree-inst.h L202 segfaults with custom ordering.
inc.update(gfg);
/*************** Run Round 3 ***************/
@ -528,17 +483,6 @@ TEST(HybridGaussianISAM, NonTrivial) {
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 = HybridNonlinearFactorGraph();
@ -585,40 +529,31 @@ TEST(HybridGaussianISAM, NonTrivial) {
gfg = fg.linearize(initial);
fg = HybridNonlinearFactorGraph();
// 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);
// Keep pruning!
inc.update(gfg);
inc.prune(M(3), 3);
inc.print();
// The final discrete graph should not be empty since we have eliminated
// all continuous variables.
// EXPECT(!inc.remainingDiscreteGraph().empty());
auto discreteTree = inc[M(3)]->conditional()->asDiscreteConditional();
EXPECT_LONGS_EQUAL(3, discreteTree->size());
// // 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 the optimal discrete mode assignment is (1, 1, 1).
DiscreteFactorGraph discreteGraph;
discreteGraph.push_back(discreteTree);
DiscreteValues optimal_assignment = discreteGraph.optimize();
// // 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());
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 = inc[X(3)]->conditional()->asMixture();
EXPECT_LONGS_EQUAL(3, lastConditional->nrComponents());
}
/* ************************************************************************* */