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

last test to get running

release/4.3a0
Varun Agrawal 2022-08-17 16:28:47 -04:00
parent ac20cff710
commit 83b8103db3
2 changed files with 183 additions and 189 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/hybrid/HybridGaussianISAM.cpp
View File

@ -75,8 +75,6 @@ void HybridGaussianISAM::updateInternal(
std::copy(allDiscrete.begin(), allDiscrete.end(), std::copy(allDiscrete.begin(), allDiscrete.end(),
std::back_inserter(newKeysDiscreteLast)); std::back_inserter(newKeysDiscreteLast));
// KeyVector new
// Get an ordering where the new keys are eliminated last // Get an ordering where the new keys are eliminated last
const VariableIndex index(factors); const VariableIndex index(factors);
Ordering elimination_ordering; Ordering elimination_ordering;

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

@ -393,220 +393,216 @@ TEST(HybridGaussianISAM, NonTrivial) {
/*************** Run Round 1 ***************/ /*************** Run Round 1 ***************/
HybridNonlinearFactorGraph fg; HybridNonlinearFactorGraph fg;
// // Add a prior on pose x1 at the origin. // Add a prior on pose x1 at the origin.
// // A prior factor consists of a mean and // A prior factor consists of a mean and
// // a noise model (covariance matrix) // a noise model (covariance matrix)
// Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
// auto priorNoise = noiseModel::Diagonal::Sigmas( auto priorNoise = noiseModel::Diagonal::Sigmas(
// Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta 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); fg.emplace_nonlinear<PriorFactor<Pose2>>(X(0), prior, priorNoise);
// // create a noise model for the landmark measurements // create a noise model for the landmark measurements
// auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1); auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1);
// // We model a robot's single leg as X - Y - Z - W // We model a robot's single leg as X - Y - Z - W
// // where X is the base link and W is the foot link. // where X is the base link and W is the foot link.
// // Add connecting poses similar to PoseFactors in GTD // Add connecting poses similar to PoseFactors in GTD
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(0), Y(0), Pose2(0, 1.0, 0), fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(0), Y(0), Pose2(0, 1.0, 0),
// poseNoise); poseNoise);
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(0), Z(0), Pose2(0, 1.0, 0), fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(0), Z(0), Pose2(0, 1.0, 0),
// poseNoise); poseNoise);
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(0), W(0), Pose2(0, 1.0, 0), fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(0), W(0), Pose2(0, 1.0, 0),
// poseNoise); poseNoise);
// // Create initial estimate // Create initial estimate
// Values initial; Values initial;
// initial.insert(X(0), Pose2(0.0, 0.0, 0.0)); initial.insert(X(0), Pose2(0.0, 0.0, 0.0));
// initial.insert(Y(0), Pose2(0.0, 1.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(Z(0), Pose2(0.0, 2.0, 0.0));
// initial.insert(W(0), Pose2(0.0, 3.0, 0.0)); initial.insert(W(0), Pose2(0.0, 3.0, 0.0));
// HybridGaussianFactorGraph gfg = fg.linearize(initial); HybridGaussianFactorGraph gfg = fg.linearize(initial);
// fg = HybridNonlinearFactorGraph(); fg = HybridNonlinearFactorGraph();
// HybridGaussianISAM inc; HybridGaussianISAM inc;
// // Regular ordering going up the chain. // Regular ordering going up the chain.
// Ordering ordering; Ordering ordering;
// ordering += W(0); ordering += W(0);
// ordering += Z(0); ordering += Z(0);
// ordering += Y(0); ordering += Y(0);
// ordering += X(0); ordering += X(0);
// // Update without pruning // Update without pruning
// // The result is a HybridBayesNet with no discrete variables // The result is a HybridBayesNet with no discrete variables
// // (equivalent to a GaussianBayesNet). // (equivalent to a GaussianBayesNet).
// // Factorization is: // Factorization is:
// // `P(X | measurements) = P(W0|Z0) P(Z0|Y0) P(Y0|X0) P(X0)` // `P(X | measurements) = P(W0|Z0) P(Z0|Y0) P(Y0|X0) P(X0)`
// inc.update(gfg, ordering); // TODO(Varun) ClusterTree-inst.h L202 segfaults with custom ordering.
inc.update(gfg);
// /*************** Run Round 2 ***************/ /*************** Run Round 2 ***************/
// using PlanarMotionModel = BetweenFactor<Pose2>; using PlanarMotionModel = BetweenFactor<Pose2>;
// // Add odometry factor with discrete modes. // Add odometry factor with discrete modes.
// Pose2 odometry(1.0, 0.0, 0.0); Pose2 odometry(1.0, 0.0, 0.0);
// KeyVector contKeys = {W(0), W(1)}; KeyVector contKeys = {W(0), W(1)};
// auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
// auto still = boost::make_shared<PlanarMotionModel>(W(0), W(1), Pose2(0, auto still = boost::make_shared<PlanarMotionModel>(W(0), W(1), Pose2(0, 0, 0),
// 0, 0), noise_model),
// noise_model), moving = boost::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
// moving = boost::make_shared<PlanarMotionModel>(W(0), W(1), odometry, noise_model);
// noise_model); std::vector<PlanarMotionModel::shared_ptr> components = {moving, still};
// std::vector<PlanarMotionModel::shared_ptr> components = {moving, still}; auto mixtureFactor = boost::make_shared<MixtureFactor>(
// auto dcFactor = boost::make_shared<DCMixtureFactor<PlanarMotionModel>>( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
// contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); fg.push_back(mixtureFactor);
// fg.push_back(dcFactor);
// // Add equivalent of ImuFactor // Add equivalent of ImuFactor
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
// 0), poseNoise);
// poseNoise); // PoseFactors-like at k=1
// // PoseFactors-like at k=1 fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(1), Y(1), Pose2(0, 1, 0),
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(1), Y(1), Pose2(0, 1, 0), poseNoise);
// poseNoise); fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(1), Z(1), Pose2(0, 1, 0),
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(1), Z(1), Pose2(0, 1, 0), poseNoise);
// poseNoise); fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(1), W(1), Pose2(-1, 1, 0),
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(1), W(1), Pose2(-1, 1, 0), poseNoise);
// poseNoise);
// initial.insert(X(1), Pose2(1.0, 0.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(Y(1), Pose2(1.0, 1.0, 0.0));
// initial.insert(Z(1), Pose2(1.0, 2.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. // 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(W(1), Pose2(0.0, 3.0, 0.0));
// // Ordering for k=1. gfg = fg.linearize(initial);
// // This ordering follows the intuition that we eliminate the previous fg = HybridNonlinearFactorGraph();
// // 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);
// gfg = fg.linearize(initial); // Ordering for k=1.
// fg = HybridNonlinearFactorGraph(); // 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 // Update without pruning
// // The result is a HybridBayesNet with 1 discrete variable M(1). // 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, // P(X | measurements) = P(W0|Z0, W1, M1) P(Z0|Y0, W1, M1) P(Y0|X0, W1, M1)
// M1) // P(X0 | X1, W1, M1) P(W1|Z1, X1, M1) P(Z1|Y1, X1, M1)
// // P(X0 | X1, W1, M1) P(W1|Z1, X1, M1) P(Z1|Y1, X1, // P(Y1 | X1, M1)P(X1 | M1)P(M1)
// M1) // The MHS tree is a 1 level tree for time indices (1,) with 2 leaves.
// // P(Y1 | X1, M1)P(X1 | M1)P(M1) // TODO(Varun) ClusterTree-inst.h L202 segfaults with custom ordering.
// // The MHS tree is a 2 level tree for time indices (0, 1) with 2 leaves. inc.update(gfg);
// inc.update(gfg, ordering);
// /*************** Run Round 3 ***************/ /*************** Run Round 3 ***************/
// // Add odometry factor with discrete modes. // Add odometry factor with discrete modes.
// contKeys = {W(1), W(2)}; contKeys = {W(1), W(2)};
// still = boost::make_shared<PlanarMotionModel>(W(1), W(2), Pose2(0, 0, 0), still = boost::make_shared<PlanarMotionModel>(W(1), W(2), Pose2(0, 0, 0),
// noise_model); noise_model);
// moving = moving =
// boost::make_shared<PlanarMotionModel>(W(1), W(2), odometry, boost::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
// noise_model); components = {moving, still};
// components = {moving, still}; mixtureFactor = boost::make_shared<MixtureFactor>(
// dcFactor = boost::make_shared<DCMixtureFactor<PlanarMotionModel>>( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components);
// contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); fg.push_back(mixtureFactor);
// fg.push_back(dcFactor);
// // Add equivalent of ImuFactor // Add equivalent of ImuFactor
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
// 0), poseNoise);
// poseNoise); // PoseFactors-like at k=1
// // PoseFactors-like at k=1 fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(2), Y(2), Pose2(0, 1, 0),
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(2), Y(2), Pose2(0, 1, 0), poseNoise);
// poseNoise); fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(2), Z(2), Pose2(0, 1, 0),
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(2), Z(2), Pose2(0, 1, 0), poseNoise);
// poseNoise); fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(2), W(2), Pose2(-2, 1, 0),
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(2), W(2), Pose2(-2, 1, 0), poseNoise);
// poseNoise);
// initial.insert(X(2), Pose2(2.0, 0.0, 0.0)); initial.insert(X(2), Pose2(2.0, 0.0, 0.0));
// initial.insert(Y(2), Pose2(2.0, 1.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(Z(2), Pose2(2.0, 2.0, 0.0));
// initial.insert(W(2), Pose2(0.0, 3.0, 0.0)); initial.insert(W(2), Pose2(0.0, 3.0, 0.0));
// // Ordering at k=2. Same intuition as before. // Ordering at k=2. Same intuition as before.
// ordering = Ordering(); ordering = Ordering();
// ordering += W(1); ordering += W(1);
// ordering += Z(1); ordering += Z(1);
// ordering += Y(1); ordering += Y(1);
// ordering += X(1); ordering += X(1);
// ordering += W(2); ordering += W(2);
// ordering += Z(2); ordering += Z(2);
// ordering += Y(2); ordering += Y(2);
// ordering += X(2); ordering += X(2);
// gfg = fg.linearize(initial); gfg = fg.linearize(initial);
// fg = HybridNonlinearFactorGraph(); fg = HybridNonlinearFactorGraph();
// // Now we prune! // Now we prune!
// // P(X | measurements) = P(W0|Z0, W1, M1) P(Z0|Y0, W1, M1) P(Y0|X0, W1, // P(X | measurements) = P(W0|Z0, W1, M1) P(Z0|Y0, W1, M1) P(Y0|X0, W1, M1)
// M1) P(X0 | X1, W1, M1) // P(X0 | X1, W1, M1) P(W1|W2, Z1, X1, M1, M2)
// // P(W1|W2, Z1, X1, M1, M2) P(Z1| W2, Y1, X1, M1, // P(Z1| W2, Y1, X1, M1, M2) P(Y1 | W2, X1, M1, M2)
// M2) P(Y1 | W2, X1, M1, M2) // P(X1 | W2, X2, M1, M2) P(W2|Z2, X2, 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(Z2|Y2, X2, M1, M2) // P(X2 | M1, M2) P(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).
// // The MHS at this point should be a 3 level tree on (0, 1, 2). // 1 has 2 choices, and 2 has 4 choices.
// // 0 has 2 choices, 1 has 4 choices and 2 has 4 choices. inc.update(gfg);
// inc.update(gfg, ordering, 2); inc.prune(M(2), 2);
// /*************** Run Round 4 ***************/ /*************** Run Round 4 ***************/
// // Add odometry factor with discrete modes. // Add odometry factor with discrete modes.
// contKeys = {W(2), W(3)}; contKeys = {W(2), W(3)};
// still = boost::make_shared<PlanarMotionModel>(W(2), W(3), Pose2(0, 0, 0), still = boost::make_shared<PlanarMotionModel>(W(2), W(3), Pose2(0, 0, 0),
// noise_model); noise_model);
// moving = moving =
// boost::make_shared<PlanarMotionModel>(W(2), W(3), odometry, boost::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
// noise_model); components = {moving, still};
// components = {moving, still}; mixtureFactor = boost::make_shared<MixtureFactor>(
// dcFactor = boost::make_shared<DCMixtureFactor<PlanarMotionModel>>( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components);
// contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); fg.push_back(mixtureFactor);
// fg.push_back(dcFactor);
// // Add equivalent of ImuFactor // Add equivalent of ImuFactor
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),
// 0), poseNoise);
// poseNoise); // PoseFactors-like at k=3
// // PoseFactors-like at k=3 fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(3), Y(3), Pose2(0, 1, 0),
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(3), Y(3), Pose2(0, 1, 0), poseNoise);
// poseNoise); fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(3), Z(3), Pose2(0, 1, 0),
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(3), Z(3), Pose2(0, 1, 0), poseNoise);
// poseNoise); fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(3), W(3), Pose2(-3, 1, 0),
// fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(3), W(3), Pose2(-3, 1, 0), poseNoise);
// poseNoise);
// initial.insert(X(3), Pose2(3.0, 0.0, 0.0)); initial.insert(X(3), Pose2(3.0, 0.0, 0.0));
// initial.insert(Y(3), Pose2(3.0, 1.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(Z(3), Pose2(3.0, 2.0, 0.0));
// initial.insert(W(3), Pose2(0.0, 3.0, 0.0)); initial.insert(W(3), Pose2(0.0, 3.0, 0.0));
// // Ordering at k=3. Same intuition as before. gfg = fg.linearize(initial);
// ordering = Ordering(); fg = HybridNonlinearFactorGraph();
// ordering += W(2);
// ordering += Z(2);
// ordering += Y(2);
// ordering += X(2);
// ordering += W(3);
// ordering += Z(3);
// ordering += Y(3);
// ordering += X(3);
// gfg = fg.linearize(initial); // Ordering at k=3. Same intuition as before.
// fg = HybridNonlinearFactorGraph(); 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! // Keep pruning!
// inc.update(gfg, ordering, 3); inc.update(gfg);
inc.prune(M(3), 3);
inc.print();
// // The final discrete graph should not be empty since we have eliminated // The final discrete graph should not be empty since we have eliminated
// // all continuous variables. // all continuous variables.
// EXPECT(!inc.remainingDiscreteGraph().empty()); // EXPECT(!inc.remainingDiscreteGraph().empty());
// // Test if the optimal discrete mode assignment is (1, 1, 1). // // Test if the optimal discrete mode assignment is (1, 1, 1).