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

got some more tests working

release/4.3a0
Varun Agrawal 2022-08-10 04:54:52 -04:00
parent 4ea897cfbf
commit fbceda3e56
1 changed files with 52 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -21,6 +21,7 @@
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/hybrid/HybridGaussianISAM.h> #include <gtsam/hybrid/HybridGaussianISAM.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/PriorFactor.h> #include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/sam/BearingRangeFactor.h> #include <gtsam/sam/BearingRangeFactor.h>
@ -108,16 +109,18 @@ TEST(HybridGaussianElimination, IncrementalInference) {
// * * * // * * *
// | | | // | | |
// *- X1 -*- X2 -*- X3 // *- X1 -*- X2 -*- X3
// \*-M1-*/ // | |
// *-M1 - * - M2
graph1.push_back(switching.linearizedFactorGraph.at(0)); // P(X1) 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(1)); // P(X1, X2 | M1)
graph1.push_back(switching.linearizedFactorGraph.at(2)); // P(X2, X3 | M2) 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)
// Create ordering. //TODO(Varun) we cannot enforce ordering
Ordering ordering; // // Create ordering.
ordering += X(1); // Ordering ordering1;
ordering += X(2); // ordering1 += X(1);
// ordering1 += X(2);
// Run update step // Run update step
isam.update(graph1); isam.update(graph1);
@ -126,20 +129,22 @@ TEST(HybridGaussianElimination, IncrementalInference) {
// New factor graph for incremental update. // New factor graph for incremental update.
HybridGaussianFactorGraph graph2; HybridGaussianFactorGraph graph2;
graph1.push_back(switching.linearizedFactorGraph.at(3)); // P(X2) graph2.push_back(switching.linearizedFactorGraph.at(2)); // P(X2, X3 | M2)
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)
// Create ordering. //TODO(Varun) we cannot enforce ordering
Ordering ordering2; // // Create ordering.
ordering2 += X(2); // Ordering ordering2;
ordering2 += X(3); // 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.
ordering.clear(); Ordering ordering;
ordering += X(1); ordering += X(1);
ordering += X(2); ordering += X(2);
ordering += X(3); ordering += X(3);
@ -151,37 +156,42 @@ TEST(HybridGaussianElimination, IncrementalInference) {
switching.linearizedFactorGraph.eliminatePartialSequential(ordering); switching.linearizedFactorGraph.eliminatePartialSequential(ordering);
// The densities on X(1) should be the same // The densities on X(1) should be the same
expectedHybridBayesNet->atGaussian(0)->print(); auto x1_conditional =
auto x1_conditional = isam[X(1)]->conditional(); dynamic_pointer_cast<GaussianMixture>(isam[X(1)]->conditional()->inner());
GTSAM_PRINT(*x1_conditional); EXPECT(
// EXPECT(assert_equal(*(hybridBayesNet.atGaussian(0)), assert_equal(*x1_conditional, *(expectedHybridBayesNet->atGaussian(0))));
// *(expectedHybridBayesNet->atGaussian(0))));
// // The densities on X(2) should be the same // The densities on X(2) should be the same
// EXPECT(assert_equal(*(hybridBayesNet2.atGaussian(1)), auto x2_conditional =
// *(expectedHybridBayesNet->atGaussian(1)))); dynamic_pointer_cast<GaussianMixture>(isam[X(2)]->conditional()->inner());
EXPECT(
assert_equal(*x2_conditional, *(expectedHybridBayesNet->atGaussian(1))));
// // The densities on X(3) should be the same // // The densities on X(3) should be the same
// EXPECT(assert_equal(*(hybridBayesNet2.atGaussian(2)), // auto x3_conditional =
// *(expectedHybridBayesNet->atGaussian(2)))); // dynamic_pointer_cast<GaussianMixture>(isam[X(3)]->conditional()->inner());
// EXPECT(
// assert_equal(*x3_conditional, *(expectedHybridBayesNet->atGaussian(2))));
// // we only do the manual continuous elimination for 0,0 GTSAM_PRINT(*expectedHybridBayesNet);
// // the other discrete probabilities on M(2) are calculated the same way
// auto m00_prob = [&]() { // we only do the manual continuous elimination for 0,0
// GaussianFactorGraph gf; // the other discrete probabilities on M(2) are calculated the same way
auto m00_prob = [&]() {
GaussianFactorGraph gf;
// gf.add(switching.linearizedFactorGraph.gaussianGraph().at(3)); // gf.add(switching.linearizedFactorGraph.gaussianGraph().at(3));
// DiscreteValues m00; DiscreteValues m00;
// m00[M(1)] = 0, m00[M(2)] = 0; m00[M(1)] = 0, m00[M(2)] = 0;
// auto dcMixture = // auto dcMixture =
// dynamic_pointer_cast<DCGaussianMixtureFactor>(graph2.dcGraph().at(0)); // dynamic_pointer_cast<DCGaussianMixtureFactor>(graph2.dcGraph().at(0));
// gf.add(dcMixture->factors()(m00)); // gf.add(dcMixture->factors()(m00));
// auto x2_mixed = // auto x2_mixed =
// boost::dynamic_pointer_cast<GaussianMixture>(hybridBayesNet.at(1)); // boost::dynamic_pointer_cast<GaussianMixture>(hybridBayesNet.at(1));
// gf.add(x2_mixed->factors()(m00)); // gf.add(x2_mixed->factors()(m00));
// 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. /// Test if the probability values are as expected with regression tests.
// DiscreteValues assignment; // DiscreteValues assignment;