diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index df7803722..afcda8c20 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -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; diff --git a/gtsam/hybrid/tests/testHybridIncremental.cpp b/gtsam/hybrid/tests/testHybridIncremental.cpp index afd0d0a70..8feab5221 100644 --- a/gtsam/hybrid/tests/testHybridIncremental.cpp +++ b/gtsam/hybrid/tests/testHybridIncremental.cpp @@ -393,221 +393,217 @@ TEST(HybridGaussianISAM, NonTrivial) { /*************** 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>(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>(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>(X(0), Y(0), Pose2(0, 1.0, 0), - // poseNoise); - // fg.emplace_nonlinear>(Y(0), Z(0), Pose2(0, 1.0, 0), - // poseNoise); - // fg.emplace_nonlinear>(Z(0), W(0), Pose2(0, 1.0, 0), - // poseNoise); + // Add connecting poses similar to PoseFactors in GTD + fg.emplace_nonlinear>(X(0), Y(0), Pose2(0, 1.0, 0), + poseNoise); + fg.emplace_nonlinear>(Y(0), Z(0), Pose2(0, 1.0, 0), + poseNoise); + fg.emplace_nonlinear>(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); + // 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, ordering); + // 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 ***************/ - // using PlanarMotionModel = BetweenFactor; + /*************** Run Round 2 ***************/ + using PlanarMotionModel = BetweenFactor; - // // 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(W(0), W(1), Pose2(0, - // 0, 0), - // noise_model), - // moving = boost::make_shared(W(0), W(1), odometry, - // noise_model); - // std::vector components = {moving, still}; - // auto dcFactor = boost::make_shared>( - // contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); - // fg.push_back(dcFactor); + // 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(W(0), W(1), Pose2(0, 0, 0), + noise_model), + moving = boost::make_shared(W(0), W(1), odometry, + noise_model); + std::vector components = {moving, still}; + auto mixtureFactor = boost::make_shared( + contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); + fg.push_back(mixtureFactor); - // // Add equivalent of ImuFactor - // fg.emplace_nonlinear>(X(0), X(1), Pose2(1.0, 0.0, - // 0), - // poseNoise); - // // PoseFactors-like at k=1 - // fg.emplace_nonlinear>(X(1), Y(1), Pose2(0, 1, 0), - // poseNoise); - // fg.emplace_nonlinear>(Y(1), Z(1), Pose2(0, 1, 0), - // poseNoise); - // fg.emplace_nonlinear>(Z(1), W(1), Pose2(-1, 1, 0), - // poseNoise); + // Add equivalent of ImuFactor + fg.emplace_nonlinear>(X(0), X(1), Pose2(1.0, 0.0, 0), + poseNoise); + // PoseFactors-like at k=1 + fg.emplace_nonlinear>(X(1), Y(1), Pose2(0, 1, 0), + poseNoise); + fg.emplace_nonlinear>(Y(1), Z(1), Pose2(0, 1, 0), + poseNoise); + fg.emplace_nonlinear>(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)); - // // 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); + gfg = fg.linearize(initial); + fg = HybridNonlinearFactorGraph(); - // 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 2 level tree for time indices (0, 1) with 2 leaves. - // inc.update(gfg, ordering); + // 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 ***************/ - // // Add odometry factor with discrete modes. - // contKeys = {W(1), W(2)}; - // still = boost::make_shared(W(1), W(2), Pose2(0, 0, 0), - // noise_model); - // moving = - // boost::make_shared(W(1), W(2), odometry, - // noise_model); - // components = {moving, still}; - // dcFactor = boost::make_shared>( - // contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); - // fg.push_back(dcFactor); + /*************** Run Round 3 ***************/ + // Add odometry factor with discrete modes. + contKeys = {W(1), W(2)}; + still = boost::make_shared(W(1), W(2), Pose2(0, 0, 0), + noise_model); + moving = + boost::make_shared(W(1), W(2), odometry, noise_model); + components = {moving, still}; + mixtureFactor = boost::make_shared( + contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); + fg.push_back(mixtureFactor); - // // Add equivalent of ImuFactor - // fg.emplace_nonlinear>(X(1), X(2), Pose2(1.0, 0.0, - // 0), - // poseNoise); - // // PoseFactors-like at k=1 - // fg.emplace_nonlinear>(X(2), Y(2), Pose2(0, 1, 0), - // poseNoise); - // fg.emplace_nonlinear>(Y(2), Z(2), Pose2(0, 1, 0), - // poseNoise); - // fg.emplace_nonlinear>(Z(2), W(2), Pose2(-2, 1, 0), - // poseNoise); + // Add equivalent of ImuFactor + fg.emplace_nonlinear>(X(1), X(2), Pose2(1.0, 0.0, 0), + poseNoise); + // PoseFactors-like at k=1 + fg.emplace_nonlinear>(X(2), Y(2), Pose2(0, 1, 0), + poseNoise); + fg.emplace_nonlinear>(Y(2), Z(2), Pose2(0, 1, 0), + poseNoise); + fg.emplace_nonlinear>(Z(2), W(2), Pose2(-2, 1, 0), + poseNoise); - // 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)); + 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)); - // // 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); + // 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(); + gfg = fg.linearize(initial); + fg = HybridNonlinearFactorGraph(); - // // 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); + // 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); - // /*************** Run Round 4 ***************/ - // // Add odometry factor with discrete modes. - // contKeys = {W(2), W(3)}; - // still = boost::make_shared(W(2), W(3), Pose2(0, 0, 0), - // noise_model); - // moving = - // boost::make_shared(W(2), W(3), odometry, - // noise_model); - // components = {moving, still}; - // dcFactor = boost::make_shared>( - // contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); - // fg.push_back(dcFactor); + /*************** Run Round 4 ***************/ + // Add odometry factor with discrete modes. + contKeys = {W(2), W(3)}; + still = boost::make_shared(W(2), W(3), Pose2(0, 0, 0), + noise_model); + moving = + boost::make_shared(W(2), W(3), odometry, noise_model); + components = {moving, still}; + mixtureFactor = boost::make_shared( + contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); + fg.push_back(mixtureFactor); - // // Add equivalent of ImuFactor - // fg.emplace_nonlinear>(X(2), X(3), Pose2(1.0, 0.0, - // 0), - // poseNoise); - // // PoseFactors-like at k=3 - // fg.emplace_nonlinear>(X(3), Y(3), Pose2(0, 1, 0), - // poseNoise); - // fg.emplace_nonlinear>(Y(3), Z(3), Pose2(0, 1, 0), - // poseNoise); - // fg.emplace_nonlinear>(Z(3), W(3), Pose2(-3, 1, 0), - // poseNoise); + // Add equivalent of ImuFactor + fg.emplace_nonlinear>(X(2), X(3), Pose2(1.0, 0.0, 0), + poseNoise); + // PoseFactors-like at k=3 + fg.emplace_nonlinear>(X(3), Y(3), Pose2(0, 1, 0), + poseNoise); + fg.emplace_nonlinear>(Y(3), Z(3), Pose2(0, 1, 0), + poseNoise); + fg.emplace_nonlinear>(Z(3), W(3), Pose2(-3, 1, 0), + poseNoise); - // 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)); + 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)); - // // 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); + gfg = fg.linearize(initial); + fg = HybridNonlinearFactorGraph(); - // 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, ordering, 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()); + // 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 =