From 83b8103db30e6e3426f542563021ebadb63db5e8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 17 Aug 2022 16:28:47 -0400 Subject: [PATCH 1/6] last test to get running --- gtsam/hybrid/HybridGaussianISAM.cpp | 2 - gtsam/hybrid/tests/testHybridIncremental.cpp | 426 +++++++++---------- 2 files changed, 211 insertions(+), 217 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index df78037221..afcda8c208 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 afd0d0a70d..8feab52216 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); - - // // 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. - - // // 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)); - - // HybridGaussianFactorGraph gfg = fg.linearize(initial); - // fg = HybridNonlinearFactorGraph(); - - // 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)` - // inc.update(gfg, ordering); - - // /*************** 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 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)); - - // // 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(); - - // // 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); - - // /*************** 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); - - // // 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)); - - // // 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(); - - // // 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); - - // /*************** 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); - - // // 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)); - - // // 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(); - - // // Keep pruning! - // inc.update(gfg, ordering, 3); - - // // The final discrete graph should not be empty since we have eliminated - // // all continuous variables. - // EXPECT(!inc.remainingDiscreteGraph().empty()); + // 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); + + // 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); + + // 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(); + + 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 ***************/ + 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 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); + + 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)); + + 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 ***************/ + // 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); + + 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); + + 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 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}; + 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); + + 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)); + + 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()); // // Test if the optimal discrete mode assignment is (1, 1, 1). // DiscreteValues optimal_assignment = From 07f0101db7401be00917c1f2c099cd6326e3369c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 21 Aug 2022 12:48:33 -0400 Subject: [PATCH 2/6] check subset rather than equality for GaussianISAM pruning --- gtsam/hybrid/HybridGaussianISAM.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index afcda8c208..23a95c021d 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -105,6 +105,22 @@ void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors, this->updateInternal(newFactors, &orphans, ordering, function); } +/* ************************************************************************* */ +/** + * @brief Check if `b` is a subset of `a`. + * Non-const since they need to be sorted. + * + * @param a KeyVector + * @param b KeyVector + * @return True if the keys of b is a subset of a, else false. + */ +bool IsSubset(KeyVector a, KeyVector b) { + std::sort(a.begin(), a.end()); + std::sort(b.begin(), b.end()); + return std::includes(a.begin(), a.end(), b.begin(), b.end()); +} + +/* ************************************************************************* */ void HybridGaussianISAM::prune(const Key& root, const size_t maxNrLeaves) { auto decisionTree = boost::dynamic_pointer_cast( this->clique(root)->conditional()->inner()); @@ -131,7 +147,7 @@ void HybridGaussianISAM::prune(const Key& root, const size_t maxNrLeaves) { parents.push_back(parent); } - if (parents == decisionTree->keys()) { + if (IsSubset(parents, decisionTree->keys())) { auto gaussianMixture = boost::dynamic_pointer_cast( clique.second->conditional()->inner()); From 29c19ee77b5fda682fabdf110baf1a7f320c87b3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 21 Aug 2022 12:49:13 -0400 Subject: [PATCH 3/6] handle HybridConditional and explicitly set Gaussian Factor Graphs to empty --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 31 +++++++++++++++++++--- 1 file changed, 27 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 55fa9a908a..af381de041 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -96,8 +96,15 @@ GaussianMixtureFactor::Sum sumFrontals( } } else if (f->isContinuous()) { - deferredFactors.push_back( - boost::dynamic_pointer_cast(f)->inner()); + // Check if f is HybridConditional or HybridGaussianFactor. + if (auto hc = boost::dynamic_pointer_cast(f)) { + auto conditional = + boost::dynamic_pointer_cast(hc->inner()); + deferredFactors.push_back(conditional); + } else if (auto gf = boost::dynamic_pointer_cast(f) + ->inner()) { + deferredFactors.push_back(gf); + } } else if (f->isDiscrete()) { // Don't do anything for discrete-only factors @@ -184,6 +191,19 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // sum out frontals, this is the factor on the separator GaussianMixtureFactor::Sum sum = sumFrontals(factors); + // If a tree leaf contains nullptr, + // convert that leaf to an empty GaussianFactorGraph. + // Needed since the DecisionTree will otherwise create + // a GFG with a single (null) factor. + auto emptyGaussian = [](const GaussianFactorGraph &gfg) { + bool hasNull = + std::any_of(gfg.begin(), gfg.end(), + [](const GaussianFactor::shared_ptr &ptr) { return !ptr; }); + + return hasNull ? GaussianFactorGraph() : gfg; + }; + sum = GaussianMixtureFactor::Sum(sum, emptyGaussian); + using EliminationPair = GaussianFactorGraph::EliminationResult; KeyVector keysOfEliminated; // Not the ordering @@ -195,7 +215,10 @@ hybridElimination(const HybridGaussianFactorGraph &factors, if (graph.empty()) { return {nullptr, nullptr}; } - auto result = EliminatePreferCholesky(graph, frontalKeys); + std::pair, + boost::shared_ptr> + result = EliminatePreferCholesky(graph, frontalKeys); + if (keysOfEliminated.empty()) { keysOfEliminated = result.first->keys(); // Initialize the keysOfEliminated to be the @@ -235,7 +258,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, boost::make_shared(discreteFactor)}; } else { - // Create a resulting DCGaussianMixture on the separator. + // Create a resulting GaussianMixtureFactor on the separator. auto factor = boost::make_shared( KeyVector(continuousSeparator.begin(), continuousSeparator.end()), discreteSeparator, separatorFactors); From f6df641b191ef7b1e4bd8b867c707abdc86a4549 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 21 Aug 2022 12:56:14 -0400 Subject: [PATCH 4/6] remove custom orderings, let it happen automatically --- gtsam/hybrid/tests/testHybridIncremental.cpp | 111 ++++--------------- 1 file changed, 23 insertions(+), 88 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridIncremental.cpp b/gtsam/hybrid/tests/testHybridIncremental.cpp index 8feab52216..4449aba0b3 100644 --- a/gtsam/hybrid/tests/testHybridIncremental.cpp +++ b/gtsam/hybrid/tests/testHybridIncremental.cpp @@ -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. +// 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 as constraints between each of the poses. TEST(HybridGaussianISAM, NonTrivial) { - // This is 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 as constraints between each of the poses. - /*************** 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()); - - // // 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 pruning worked correctly by checking that we only have 3 - // leaves in - // // the last node. - // auto lastConditional = boost::dynamic_pointer_cast( - // inc.hybridBayesNet().at(inc.hybridBayesNet().size() - 1)); - // EXPECT_LONGS_EQUAL(3, lastConditional->nrComponents()); + auto discreteTree = inc[M(3)]->conditional()->asDiscreteConditional(); + EXPECT_LONGS_EQUAL(3, discreteTree->size()); + + // Test if the optimal discrete mode assignment is (1, 1, 1). + DiscreteFactorGraph discreteGraph; + discreteGraph.push_back(discreteTree); + DiscreteValues optimal_assignment = discreteGraph.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 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()); } /* ************************************************************************* */ From 6b792c0a4ca19b750a9c259c1dd84468786884ff Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 21 Aug 2022 12:56:59 -0400 Subject: [PATCH 5/6] add note about sumFrontals --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index af381de041..b57d495d8d 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -77,6 +77,19 @@ static GaussianMixtureFactor::Sum &addGaussian( } /* ************************************************************************ */ +/* Function to eliminate variables **under the following assumptions**: + * 1. When the ordering is fully continuous, and the graph only contains + * continuous and hybrid factors + * 2. When the ordering is fully discrete, and the graph only contains discrete + * factors + * + * Any usage outside of this is considered incorrect. + * + * \warning This function is not meant to be used with arbitrary hybrid factor + * graphs. For example, if there exists continuous parents, and one tries to + * eliminate a discrete variable (as specified in the ordering), the result will + * be INCORRECT and there will be NO error raised. + */ GaussianMixtureFactor::Sum sumFrontals( const HybridGaussianFactorGraph &factors) { // sum out frontals, this is the factor on the separator From 893c5f77f827b6c21af6b04b9c2e236010e7298e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 22 Aug 2022 08:29:32 -0400 Subject: [PATCH 6/6] cast to only HybridGaussianFactor --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 39 +++++++++------------- 1 file changed, 16 insertions(+), 23 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index b57d495d8d..c024c1255f 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -77,19 +77,6 @@ static GaussianMixtureFactor::Sum &addGaussian( } /* ************************************************************************ */ -/* Function to eliminate variables **under the following assumptions**: - * 1. When the ordering is fully continuous, and the graph only contains - * continuous and hybrid factors - * 2. When the ordering is fully discrete, and the graph only contains discrete - * factors - * - * Any usage outside of this is considered incorrect. - * - * \warning This function is not meant to be used with arbitrary hybrid factor - * graphs. For example, if there exists continuous parents, and one tries to - * eliminate a discrete variable (as specified in the ordering), the result will - * be INCORRECT and there will be NO error raised. - */ GaussianMixtureFactor::Sum sumFrontals( const HybridGaussianFactorGraph &factors) { // sum out frontals, this is the factor on the separator @@ -109,15 +96,8 @@ GaussianMixtureFactor::Sum sumFrontals( } } else if (f->isContinuous()) { - // Check if f is HybridConditional or HybridGaussianFactor. - if (auto hc = boost::dynamic_pointer_cast(f)) { - auto conditional = - boost::dynamic_pointer_cast(hc->inner()); - deferredFactors.push_back(conditional); - } else if (auto gf = boost::dynamic_pointer_cast(f) - ->inner()) { - deferredFactors.push_back(gf); - } + deferredFactors.push_back( + boost::dynamic_pointer_cast(f)->inner()); } else if (f->isDiscrete()) { // Don't do anything for discrete-only factors @@ -278,7 +258,20 @@ hybridElimination(const HybridGaussianFactorGraph &factors, return {boost::make_shared(conditional), factor}; } } -/* ************************************************************************ */ +/* ************************************************************************ + * Function to eliminate variables **under the following assumptions**: + * 1. When the ordering is fully continuous, and the graph only contains + * continuous and hybrid factors + * 2. When the ordering is fully discrete, and the graph only contains discrete + * factors + * + * Any usage outside of this is considered incorrect. + * + * \warning This function is not meant to be used with arbitrary hybrid factor + * graphs. For example, if there exists continuous parents, and one tries to + * eliminate a discrete variable (as specified in the ordering), the result will + * be INCORRECT and there will be NO error raised. + */ std::pair // EliminateHybrid(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) {