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

more tests running

release/4.3a0
Varun Agrawal 2022-08-12 16:18:40 -04:00
parent 0e4db30713
commit aa48658626
1 changed files with 44 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -116,12 +116,6 @@ TEST(HybridGaussianElimination, IncrementalInference) {
graph1.push_back(switching.linearizedFactorGraph.at(3)); // P(X2) graph1.push_back(switching.linearizedFactorGraph.at(3)); // P(X2)
graph1.push_back(switching.linearizedFactorGraph.at(5)); // P(M1) graph1.push_back(switching.linearizedFactorGraph.at(5)); // P(M1)
//TODO(Varun) we cannot enforce ordering
// // Create ordering.
// Ordering ordering1;
// ordering1 += X(1);
// ordering1 += X(2);
// Run update step // Run update step
isam.update(graph1); isam.update(graph1);
@ -133,14 +127,7 @@ TEST(HybridGaussianElimination, IncrementalInference) {
graph2.push_back(switching.linearizedFactorGraph.at(4)); // P(X3) graph2.push_back(switching.linearizedFactorGraph.at(4)); // P(X3)
graph2.push_back(switching.linearizedFactorGraph.at(6)); // P(M1, M2) graph2.push_back(switching.linearizedFactorGraph.at(6)); // P(M1, M2)
//TODO(Varun) we cannot enforce ordering
// // Create ordering.
// Ordering ordering2;
// ordering2 += X(2);
// ordering2 += X(3);
isam.update(graph2); isam.update(graph2);
GTSAM_PRINT(isam);
/********************************************************/ /********************************************************/
// Run batch elimination so we can compare results. // Run batch elimination so we can compare results.
@ -150,68 +137,78 @@ TEST(HybridGaussianElimination, IncrementalInference) {
ordering += X(3); ordering += X(3);
// Now we calculate the actual factors using full elimination // Now we calculate the actual factors using full elimination
HybridBayesNet::shared_ptr expectedHybridBayesNet; HybridBayesTree::shared_ptr expectedHybridBayesTree;
HybridGaussianFactorGraph::shared_ptr expectedRemainingGraph; HybridGaussianFactorGraph::shared_ptr expectedRemainingGraph;
std::tie(expectedHybridBayesNet, expectedRemainingGraph) = std::tie(expectedHybridBayesTree, expectedRemainingGraph) =
switching.linearizedFactorGraph.eliminatePartialSequential(ordering); switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering);
// The densities on X(1) should be the same // The densities on X(1) should be the same
auto x1_conditional = auto x1_conditional =
dynamic_pointer_cast<GaussianMixture>(isam[X(1)]->conditional()->inner()); dynamic_pointer_cast<GaussianMixture>(isam[X(1)]->conditional()->inner());
EXPECT( auto actual_x1_conditional = dynamic_pointer_cast<GaussianMixture>(
assert_equal(*x1_conditional, *(expectedHybridBayesNet->atGaussian(0)))); (*expectedHybridBayesTree)[X(1)]->conditional()->inner());
EXPECT(assert_equal(*x1_conditional, *actual_x1_conditional));
// The densities on X(2) should be the same // The densities on X(2) should be the same
auto x2_conditional = auto x2_conditional =
dynamic_pointer_cast<GaussianMixture>(isam[X(2)]->conditional()->inner()); dynamic_pointer_cast<GaussianMixture>(isam[X(2)]->conditional()->inner());
EXPECT( auto actual_x2_conditional = dynamic_pointer_cast<GaussianMixture>(
assert_equal(*x2_conditional, *(expectedHybridBayesNet->atGaussian(1)))); (*expectedHybridBayesTree)[X(2)]->conditional()->inner());
EXPECT(assert_equal(*x2_conditional, *actual_x2_conditional));
// // The densities on X(3) should be the same // The densities on X(3) should be the same
// auto x3_conditional = auto x3_conditional =
// dynamic_pointer_cast<GaussianMixture>(isam[X(3)]->conditional()->inner()); dynamic_pointer_cast<GaussianMixture>(isam[X(3)]->conditional()->inner());
// EXPECT( auto actual_x3_conditional = dynamic_pointer_cast<GaussianMixture>(
// assert_equal(*x3_conditional, *(expectedHybridBayesNet->atGaussian(2)))); (*expectedHybridBayesTree)[X(2)]->conditional()->inner());
EXPECT(assert_equal(*x3_conditional, *actual_x3_conditional));
GTSAM_PRINT(*expectedHybridBayesNet); // We only perform manual continuous elimination for 0,0.
// The other discrete probabilities on M(2) are calculated the same way
// 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 = [&]() { auto m00_prob = [&]() {
GaussianFactorGraph gf; GaussianFactorGraph gf;
// gf.add(switching.linearizedFactorGraph.gaussianGraph().at(3)); auto x2_prior = boost::dynamic_pointer_cast<HybridGaussianFactor>(
switching.linearizedFactorGraph.at(3))->inner();
gf.add(x2_prior);
DiscreteValues m00; DiscreteValues m00;
m00[M(1)] = 0, m00[M(2)] = 0; m00[M(1)] = 0, m00[M(2)] = 0;
// auto dcMixture = // P(X2, X3 | M2)
// dynamic_pointer_cast<DCGaussianMixtureFactor>(graph2.dcGraph().at(0)); auto dcMixture =
// gf.add(dcMixture->factors()(m00)); dynamic_pointer_cast<GaussianMixtureFactor>(graph2.at(0));
// auto x2_mixed = gf.add(dcMixture->factors()(m00));
// boost::dynamic_pointer_cast<GaussianMixture>(hybridBayesNet.at(1));
// gf.add(x2_mixed->factors()(m00)); auto x2_mixed =
boost::dynamic_pointer_cast<GaussianMixture>(isam[X(2)]->conditional()->inner());
// Perform explicit cast so we can add the conditional to `gf`.
auto x2_cond = boost::dynamic_pointer_cast<GaussianFactor>(
x2_mixed->conditionals()(m00));
gf.add(x2_cond);
auto result_gf = gf.eliminateSequential(); auto result_gf = gf.eliminateSequential();
return gf.probPrime(result_gf->optimize()); return gf.probPrime(result_gf->optimize());
}(); }();
/// Test if the probability values are as expected with regression tests. auto discreteConditional = isam[M(1)]->conditional()->asDiscreteConditional();
// DiscreteValues assignment; // Test if the probability values are as expected with regression tests.
// EXPECT(assert_equal(m00_prob, 0.60656, 1e-5)); // DiscreteValues assignment;
// assignment[M(1)] = 0; // EXPECT(assert_equal(m00_prob, 0.60656, 1e-5));
// assignment[M(2)] = 0; // assignment[M(1)] = 0;
// EXPECT(assert_equal(m00_prob, (*discreteFactor)(assignment), 1e-5)); // assignment[M(2)] = 0;
// EXPECT(assert_equal(m00_prob, (*discreteConditional)(assignment), 1e-5));
// assignment[M(1)] = 1; // assignment[M(1)] = 1;
// assignment[M(2)] = 0; // assignment[M(2)] = 0;
// EXPECT(assert_equal(0.612477, (*discreteFactor)(assignment), 1e-5)); // EXPECT(assert_equal(0.612477, (*discreteConditional)(assignment), 1e-5));
// assignment[M(1)] = 0; // assignment[M(1)] = 0;
// assignment[M(2)] = 1; // assignment[M(2)] = 1;
// EXPECT(assert_equal(0.999952, (*discreteFactor)(assignment), 1e-5)); // EXPECT(assert_equal(0.999952, (*discreteConditional)(assignment), 1e-5));
// assignment[M(1)] = 1; // assignment[M(1)] = 1;
// assignment[M(2)] = 1; // assignment[M(2)] = 1;
// EXPECT(assert_equal(1.0, (*discreteFactor)(assignment), 1e-5)); // EXPECT(assert_equal(1.0, (*discreteConditional)(assignment), 1e-5));
// DiscreteFactorGraph dfg; // DiscreteFactorGraph dfg;
// dfg.add(*discreteFactor); // dfg.add(*discreteConditional);
// dfg.add(discreteFactor_m1); // dfg.add(discreteConditional_m1);
// dfg.add_factors(switching.linearizedFactorGraph.discreteGraph()); // dfg.add_factors(switching.linearizedFactorGraph.discreteGraph());
// // Check if the chordal graph generated from incremental elimination // // Check if the chordal graph generated from incremental elimination