From 0938159706f1d28e089c936f6a73834baab59367 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 9 Nov 2022 18:38:42 -0500 Subject: [PATCH 01/21] overload multifrontal elimination --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 176 +++++++++++++++++++-- gtsam/hybrid/HybridGaussianFactorGraph.h | 26 +++ 2 files changed, 189 insertions(+), 13 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 983817f03c..a0c1b67da9 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -546,6 +546,24 @@ HybridGaussianFactorGraph::continuousDelta( return delta_tree; } +/* ************************************************************************ */ +DecisionTree +HybridGaussianFactorGraph::continuousDelta( + const DiscreteKeys &discrete_keys, + const boost::shared_ptr &continuousBayesTree, + const std::vector &assignments) const { + // Create a decision tree of all the different VectorValues + std::vector vector_values; + for (const DiscreteValues &assignment : assignments) { + VectorValues values = continuousBayesTree->optimize(assignment); + vector_values.push_back(boost::make_shared(values)); + } + DecisionTree delta_tree(discrete_keys, + vector_values); + + return delta_tree; +} + /* ************************************************************************ */ AlgebraicDecisionTree HybridGaussianFactorGraph::continuousProbPrimes( const DiscreteKeys &orig_discrete_keys, @@ -584,6 +602,67 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::continuousProbPrimes( return probPrimeTree; } +/* ************************************************************************ */ +AlgebraicDecisionTree HybridGaussianFactorGraph::continuousProbPrimes( + const DiscreteKeys &orig_discrete_keys, + const boost::shared_ptr &continuousBayesTree) const { + // Generate all possible assignments. + const std::vector assignments = + DiscreteValues::CartesianProduct(orig_discrete_keys); + + // Save a copy of the original discrete key ordering + DiscreteKeys discrete_keys(orig_discrete_keys); + // Reverse discrete keys order for correct tree construction + std::reverse(discrete_keys.begin(), discrete_keys.end()); + + // Create a decision tree of all the different VectorValues + DecisionTree delta_tree = + this->continuousDelta(discrete_keys, continuousBayesTree, assignments); + + // Get the probPrime tree with the correct leaf probabilities + std::vector probPrimes; + for (const DiscreteValues &assignment : assignments) { + VectorValues delta = *delta_tree(assignment); + + // If VectorValues is empty, it means this is a pruned branch. + // Set thr probPrime to 0.0. + if (delta.size() == 0) { + probPrimes.push_back(0.0); + continue; + } + + // Compute the error given the delta and the assignment. + double error = this->error(delta, assignment); + probPrimes.push_back(exp(-error)); + } + + AlgebraicDecisionTree probPrimeTree(discrete_keys, probPrimes); + return probPrimeTree; +} + +/* ************************************************************************ */ +std::pair +HybridGaussianFactorGraph::separateContinuousDiscreteOrdering( + const Ordering &ordering) const { + KeySet all_continuous_keys = this->continuousKeys(); + KeySet all_discrete_keys = this->discreteKeys(); + Ordering continuous_ordering, discrete_ordering; + + for (auto &&key : ordering) { + if (std::find(all_continuous_keys.begin(), all_continuous_keys.end(), + key) != all_continuous_keys.end()) { + continuous_ordering.push_back(key); + } else if (std::find(all_discrete_keys.begin(), all_discrete_keys.end(), + key) != all_discrete_keys.end()) { + discrete_ordering.push_back(key); + } else { + throw std::runtime_error("Key in ordering not present in factors."); + } + } + + return std::make_pair(continuous_ordering, discrete_ordering); +} + /* ************************************************************************ */ boost::shared_ptr HybridGaussianFactorGraph::eliminateHybridSequential( @@ -640,25 +719,96 @@ boost::shared_ptr HybridGaussianFactorGraph::eliminateSequential( const Ordering &ordering, const Eliminate &function, OptionalVariableIndex variableIndex) const { - KeySet all_continuous_keys = this->continuousKeys(); - KeySet all_discrete_keys = this->discreteKeys(); + // Segregate the continuous and the discrete keys Ordering continuous_ordering, discrete_ordering; + std::tie(continuous_ordering, discrete_ordering) = + this->separateContinuousDiscreteOrdering(ordering); + + return this->eliminateHybridSequential(continuous_ordering, discrete_ordering, + function, variableIndex); +} + +/* ************************************************************************ */ +boost::shared_ptr +HybridGaussianFactorGraph::eliminateHybridMultifrontal( + const boost::optional continuous, + const boost::optional discrete, const Eliminate &function, + OptionalVariableIndex variableIndex) const { + Ordering continuous_ordering = + continuous ? *continuous : Ordering(this->continuousKeys()); + Ordering discrete_ordering = + discrete ? *discrete : Ordering(this->discreteKeys()); + + // Eliminate continuous + HybridBayesTree::shared_ptr bayesTree; + HybridGaussianFactorGraph::shared_ptr discreteGraph; + std::tie(bayesTree, discreteGraph) = + BaseEliminateable::eliminatePartialMultifrontal(continuous_ordering, + function, variableIndex); + + // Get the last continuous conditional which will have all the discrete + Key last_continuous_key = + continuous_ordering.at(continuous_ordering.size() - 1); + auto last_conditional = (*bayesTree)[last_continuous_key]->conditional(); + DiscreteKeys discrete_keys = last_conditional->discreteKeys(); + + // If not discrete variables, return the eliminated bayes net. + if (discrete_keys.size() == 0) { + return bayesTree; + } + + AlgebraicDecisionTree probPrimeTree = + this->continuousProbPrimes(discrete_keys, bayesTree); + + discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); + + auto updatedBayesTree = + discreteGraph->BaseEliminateable::eliminateMultifrontal(discrete_ordering, + function); + + auto discrete_clique = (*updatedBayesTree)[discrete_ordering.at(0)]; + + // Set the root of the bayes tree as the discrete clique + for (auto node : bayesTree->nodes()) { + auto clique = node.second; + + if (clique->conditional()->parents() == + discrete_clique->conditional()->frontals()) { + updatedBayesTree->addClique(clique, discrete_clique); - // Segregate the continuous and the discrete keys - for (auto &&key : ordering) { - if (std::find(all_continuous_keys.begin(), all_continuous_keys.end(), - key) != all_continuous_keys.end()) { - continuous_ordering.push_back(key); - } else if (std::find(all_discrete_keys.begin(), all_discrete_keys.end(), - key) != all_discrete_keys.end()) { - discrete_ordering.push_back(key); } else { - throw std::runtime_error("Key in ordering not present in factors."); + // Remove the clique from the children of the parents since it will get + // added again in addClique. + auto clique_it = std::find(clique->parent()->children.begin(), + clique->parent()->children.end(), clique); + clique->parent()->children.erase(clique_it); + updatedBayesTree->addClique(clique, clique->parent()); } } + return updatedBayesTree; +} + +/* ************************************************************************ */ +boost::shared_ptr +HybridGaussianFactorGraph::eliminateMultifrontal( + OptionalOrderingType orderingType, const Eliminate &function, + OptionalVariableIndex variableIndex) const { + return BaseEliminateable::eliminateMultifrontal(orderingType, function, + variableIndex); +} + +/* ************************************************************************ */ +boost::shared_ptr +HybridGaussianFactorGraph::eliminateMultifrontal( + const Ordering &ordering, const Eliminate &function, + OptionalVariableIndex variableIndex) const { + // Segregate the continuous and the discrete keys + Ordering continuous_ordering, discrete_ordering; + std::tie(continuous_ordering, discrete_ordering) = + this->separateContinuousDiscreteOrdering(ordering); - return this->eliminateHybridSequential(continuous_ordering, - discrete_ordering); + return this->eliminateHybridMultifrontal( + continuous_ordering, discrete_ordering, function, variableIndex); } } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 88728b6bbe..fb8ebbdc4b 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -231,6 +231,10 @@ class GTSAM_EXPORT HybridGaussianFactorGraph const DiscreteKeys& discrete_keys, const boost::shared_ptr& continuousBayesNet, const std::vector& assignments) const; + DecisionTree continuousDelta( + const DiscreteKeys& discrete_keys, + const boost::shared_ptr& continuousBayesTree, + const std::vector& assignments) const; /** * @brief Compute the unnormalized probabilities of the continuous variables @@ -244,6 +248,12 @@ class GTSAM_EXPORT HybridGaussianFactorGraph AlgebraicDecisionTree continuousProbPrimes( const DiscreteKeys& discrete_keys, const boost::shared_ptr& continuousBayesNet) const; + AlgebraicDecisionTree continuousProbPrimes( + const DiscreteKeys& discrete_keys, + const boost::shared_ptr& continuousBayesTree) const; + + std::pair separateContinuousDiscreteOrdering( + const Ordering& ordering) const; /** * @brief Custom elimination function which computes the correct @@ -269,6 +279,22 @@ class GTSAM_EXPORT HybridGaussianFactorGraph const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; + boost::shared_ptr eliminateHybridMultifrontal( + const boost::optional continuous = boost::none, + const boost::optional discrete = boost::none, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + + boost::shared_ptr eliminateMultifrontal( + OptionalOrderingType orderingType = boost::none, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + + boost::shared_ptr eliminateMultifrontal( + const Ordering& ordering, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + /** * @brief Return a Colamd constrained ordering where the discrete keys are * eliminated after the continuous keys. From 98d31866156d6cd5c5e448734cd760d0c5d1d492 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 9 Nov 2022 20:03:37 -0500 Subject: [PATCH 02/21] add copy constructor for HybridBayesTreeClique --- gtsam/hybrid/HybridBayesTree.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 8af0af9683..2d01aab76e 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -50,9 +50,12 @@ class GTSAM_EXPORT HybridBayesTreeClique typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; HybridBayesTreeClique() {} - virtual ~HybridBayesTreeClique() {} HybridBayesTreeClique(const boost::shared_ptr& conditional) : Base(conditional) {} + ///< Copy constructor + HybridBayesTreeClique(const HybridBayesTreeClique& clique) : Base(clique) {} + + virtual ~HybridBayesTreeClique() {} }; /* ************************************************************************* */ From 7ae4e57d6678a1d8fcff3e74ee39c24b9b156819 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 9 Nov 2022 20:04:21 -0500 Subject: [PATCH 03/21] fix HybridBayesTree::optimize to account for pruned nodes --- gtsam/hybrid/HybridBayesTree.cpp | 31 ++++++++++++++++++++++++------- 1 file changed, 24 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 8fdedab44f..c9c6afa794 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -73,6 +73,8 @@ struct HybridAssignmentData { GaussianBayesTree::sharedNode parentClique_; // The gaussian bayes tree that will be recursively created. GaussianBayesTree* gaussianbayesTree_; + // Flag indicating if all the nodes are valid. Used in optimize(). + bool valid_; /** * @brief Construct a new Hybrid Assignment Data object. @@ -83,10 +85,13 @@ struct HybridAssignmentData { */ HybridAssignmentData(const DiscreteValues& assignment, const GaussianBayesTree::sharedNode& parentClique, - GaussianBayesTree* gbt) + GaussianBayesTree* gbt, bool valid = true) : assignment_(assignment), parentClique_(parentClique), - gaussianbayesTree_(gbt) {} + gaussianbayesTree_(gbt), + valid_(valid) {} + + bool isValid() const { return valid_; } /** * @brief A function used during tree traversal that operates on each node @@ -101,6 +106,7 @@ struct HybridAssignmentData { HybridAssignmentData& parentData) { // Extract the gaussian conditional from the Hybrid clique HybridConditional::shared_ptr hybrid_conditional = node->conditional(); + GaussianConditional::shared_ptr conditional; if (hybrid_conditional->isHybrid()) { conditional = (*hybrid_conditional->asMixture())(parentData.assignment_); @@ -111,15 +117,21 @@ struct HybridAssignmentData { conditional = boost::make_shared(); } - // Create the GaussianClique for the current node - auto clique = boost::make_shared(conditional); - // Add the current clique to the GaussianBayesTree. - parentData.gaussianbayesTree_->addClique(clique, parentData.parentClique_); + GaussianBayesTree::sharedNode clique; + if (conditional) { + // Create the GaussianClique for the current node + clique = boost::make_shared(conditional); + // Add the current clique to the GaussianBayesTree. + parentData.gaussianbayesTree_->addClique(clique, + parentData.parentClique_); + } else { + parentData.valid_ = false; + } // Create new HybridAssignmentData where the current node is the parent // This will be passed down to the children nodes HybridAssignmentData data(parentData.assignment_, clique, - parentData.gaussianbayesTree_); + parentData.gaussianbayesTree_, parentData.valid_); return data; } }; @@ -138,6 +150,9 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { visitorPost); } + if (!rootData.isValid()) { + return VectorValues(); + } VectorValues result = gbt.optimize(); // Return the optimized bayes net result. @@ -151,6 +166,8 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) { DecisionTreeFactor prunedDecisionTree = decisionTree->prune(maxNrLeaves); decisionTree->root_ = prunedDecisionTree.root_; + // this->print(); + // decisionTree->print("", DefaultKeyFormatter); /// Helper struct for pruning the hybrid bayes tree. struct HybridPrunerData { From d54cf484de9ed9c86b387d4776faba13e367627e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 9 Nov 2022 20:04:51 -0500 Subject: [PATCH 04/21] fix creation of updatedBayesTree --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index a0c1b67da9..d07b728b58 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -768,10 +768,13 @@ HybridGaussianFactorGraph::eliminateHybridMultifrontal( auto discrete_clique = (*updatedBayesTree)[discrete_ordering.at(0)]; - // Set the root of the bayes tree as the discrete clique + std::set clique_set; for (auto node : bayesTree->nodes()) { - auto clique = node.second; + clique_set.insert(node.second); + } + // Set the root of the bayes tree as the discrete clique + for (auto clique : clique_set) { if (clique->conditional()->parents() == discrete_clique->conditional()->frontals()) { updatedBayesTree->addClique(clique, discrete_clique); From 318f7384b5b3f61c50b037d311a8285f345a5ba9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 10 Nov 2022 10:53:04 -0500 Subject: [PATCH 05/21] fixup the final tests --- gtsam/hybrid/tests/testHybridBayesTree.cpp | 7 ++ gtsam/hybrid/tests/testHybridEstimation.cpp | 80 +++++++++++++++++++ .../tests/testHybridGaussianFactorGraph.cpp | 3 +- gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 17 ++-- .../hybrid/tests/testHybridNonlinearISAM.cpp | 20 +++-- 5 files changed, 113 insertions(+), 14 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index 5de21322ce..ab0f3c2d94 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -136,6 +136,13 @@ TEST(HybridBayesTree, Optimize) { dfg.push_back( boost::dynamic_pointer_cast(factor->inner())); } + + // Add the probabilities for each branch + DiscreteKeys discrete_keys = {{M(0), 2}, {M(1), 2}, {M(2), 2}}; + vector probs = {0.012519475, 0.041280228, 0.075018647, 0.081663656, + 0.037152205, 0.12248971, 0.07349729, 0.08}; + AlgebraicDecisionTree potentials(discrete_keys, probs); + dfg.emplace_shared(discrete_keys, probs); DiscreteValues expectedMPE = dfg.optimize(); VectorValues expectedValues = hybridBayesNet->optimize(expectedMPE); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 4926212286..ec5d6fb7d2 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -319,6 +320,85 @@ TEST(HybridEstimation, Probability) { // hybrid_values.discrete().print(); } +/****************************************************************************/ +/** + * Test for correctness of different branches of the P'(Continuous | Discrete) + * in the multi-frontal setting. The values should match those of P'(Continuous) + * for each discrete mode. + */ +TEST(HybridEstimation, ProbabilityMultifrontal) { + constexpr size_t K = 4; + std::vector measurements = {0, 1, 2, 2}; + + // This is the correct sequence + // std::vector discrete_seq = {1, 1, 0}; + + double between_sigma = 1.0, measurement_sigma = 0.1; + + std::vector expected_errors, expected_prob_primes; + for (size_t i = 0; i < pow(2, K - 1); i++) { + std::vector discrete_seq = getDiscreteSequence(i); + + GaussianFactorGraph::shared_ptr linear_graph = specificProblem( + K, measurements, discrete_seq, measurement_sigma, between_sigma); + + auto bayes_tree = linear_graph->eliminateMultifrontal(); + + VectorValues values = bayes_tree->optimize(); + + std::cout << i << " " << linear_graph->error(values) << std::endl; + expected_errors.push_back(linear_graph->error(values)); + expected_prob_primes.push_back(linear_graph->probPrime(values)); + } + + Switching switching(K, between_sigma, measurement_sigma, measurements); + auto graph = switching.linearizedFactorGraph; + Ordering ordering = getOrdering(graph, HybridGaussianFactorGraph()); + + AlgebraicDecisionTree expected_probPrimeTree = probPrimeTree(graph); + + // Eliminate continuous + Ordering continuous_ordering(graph.continuousKeys()); + HybridBayesTree::shared_ptr bayesTree; + HybridGaussianFactorGraph::shared_ptr discreteGraph; + std::tie(bayesTree, discreteGraph) = + graph.eliminatePartialMultifrontal(continuous_ordering); + + // Get the last continuous conditional which will have all the discrete keys + Key last_continuous_key = + continuous_ordering.at(continuous_ordering.size() - 1); + auto last_conditional = (*bayesTree)[last_continuous_key]->conditional(); + DiscreteKeys discrete_keys = last_conditional->discreteKeys(); + + // Create a decision tree of all the different VectorValues + AlgebraicDecisionTree probPrimeTree = + graph.continuousProbPrimes(discrete_keys, bayesTree); + + EXPECT(assert_equal(expected_probPrimeTree, probPrimeTree)); + + // Test if the probPrimeTree matches the probability of + // the individual factor graphs + for (size_t i = 0; i < pow(2, K - 1); i++) { + std::vector discrete_seq = getDiscreteSequence(i); + Assignment discrete_assignment; + for (size_t v = 0; v < discrete_seq.size(); v++) { + discrete_assignment[M(v)] = discrete_seq[v]; + } + EXPECT_DOUBLES_EQUAL(expected_prob_primes.at(i), + probPrimeTree(discrete_assignment), 1e-8); + } + + discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); + + // Ordering discrete(graph.discreteKeys()); + // auto discreteBayesTree = discreteGraph->eliminateMultifrontal(discrete); + // // DiscreteBayesTree should have only 1 clique + // bayesTree->addClique((*discreteBayesTree)[discrete.at(0)]); + + // // HybridValues hybrid_values = bayesNet->optimize(); + // // hybrid_values.discrete().print(); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index b56b6b62a0..248d71d5fc 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -182,7 +182,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { boost::make_shared(X(1), I_3x3, Vector3::Ones())})); hfg.add(DecisionTreeFactor(m1, {2, 8})); - hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); + //TODO(Varun) Adding extra discrete variable not connected to continuous variable throws segfault + // hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); HybridBayesTree::shared_ptr result = hfg.eliminateMultifrontal(hfg.getHybridOrdering()); diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 18ce7f10ec..09403853f6 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -165,7 +165,8 @@ TEST(HybridGaussianElimination, IncrementalInference) { discrete_ordering += M(0); discrete_ordering += M(1); HybridBayesTree::shared_ptr discreteBayesTree = - expectedRemainingGraph->eliminateMultifrontal(discrete_ordering); + expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal( + discrete_ordering); DiscreteValues m00; m00[M(0)] = 0, m00[M(1)] = 0; @@ -177,10 +178,10 @@ TEST(HybridGaussianElimination, IncrementalInference) { // Test if the probability values are as expected with regression tests. DiscreteValues assignment; - EXPECT(assert_equal(m00_prob, 0.0619233, 1e-5)); + EXPECT(assert_equal(0.166667, m00_prob, 1e-5)); assignment[M(0)] = 0; assignment[M(1)] = 0; - EXPECT(assert_equal(m00_prob, (*discreteConditional)(assignment), 1e-5)); + EXPECT(assert_equal(0.0619233, (*discreteConditional)(assignment), 1e-5)); assignment[M(0)] = 1; assignment[M(1)] = 0; EXPECT(assert_equal(0.183743, (*discreteConditional)(assignment), 1e-5)); @@ -193,11 +194,15 @@ TEST(HybridGaussianElimination, IncrementalInference) { // Check if the clique conditional generated from incremental elimination // matches that of batch elimination. - auto expectedChordal = expectedRemainingGraph->eliminateMultifrontal(); - auto expectedConditional = dynamic_pointer_cast( - (*expectedChordal)[M(1)]->conditional()->inner()); + auto expectedChordal = + expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal(); auto actualConditional = dynamic_pointer_cast( isam[M(1)]->conditional()->inner()); + // Account for the probability terms from evaluating continuous FGs + DiscreteKeys discrete_keys = {{M(0), 2}, {M(1), 2}}; + vector probs = {0.061923317, 0.20415914, 0.18374323, 0.2}; + auto expectedConditional = + boost::make_shared(discrete_keys, probs); EXPECT(assert_equal(*actualConditional, *expectedConditional, 1e-6)); } diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 3bdb5ed1e0..e7e65b1232 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -153,7 +153,8 @@ TEST(HybridNonlinearISAM, IncrementalInference) { HybridBayesTree::shared_ptr expectedHybridBayesTree; HybridGaussianFactorGraph::shared_ptr expectedRemainingGraph; std::tie(expectedHybridBayesTree, expectedRemainingGraph) = - switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering); + switching.linearizedFactorGraph + .BaseEliminateable::eliminatePartialMultifrontal(ordering); // The densities on X(1) should be the same auto x0_conditional = dynamic_pointer_cast( @@ -182,7 +183,8 @@ TEST(HybridNonlinearISAM, IncrementalInference) { discrete_ordering += M(0); discrete_ordering += M(1); HybridBayesTree::shared_ptr discreteBayesTree = - expectedRemainingGraph->eliminateMultifrontal(discrete_ordering); + expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal( + discrete_ordering); DiscreteValues m00; m00[M(0)] = 0, m00[M(1)] = 0; @@ -195,10 +197,10 @@ TEST(HybridNonlinearISAM, IncrementalInference) { // Test if the probability values are as expected with regression tests. DiscreteValues assignment; - EXPECT(assert_equal(m00_prob, 0.0619233, 1e-5)); + EXPECT(assert_equal(0.166667, m00_prob, 1e-5)); assignment[M(0)] = 0; assignment[M(1)] = 0; - EXPECT(assert_equal(m00_prob, (*discreteConditional)(assignment), 1e-5)); + EXPECT(assert_equal(0.0619233, (*discreteConditional)(assignment), 1e-5)); assignment[M(0)] = 1; assignment[M(1)] = 0; EXPECT(assert_equal(0.183743, (*discreteConditional)(assignment), 1e-5)); @@ -212,10 +214,13 @@ TEST(HybridNonlinearISAM, IncrementalInference) { // Check if the clique conditional generated from incremental elimination // matches that of batch elimination. auto expectedChordal = expectedRemainingGraph->eliminateMultifrontal(); - auto expectedConditional = dynamic_pointer_cast( - (*expectedChordal)[M(1)]->conditional()->inner()); auto actualConditional = dynamic_pointer_cast( bayesTree[M(1)]->conditional()->inner()); + // Account for the probability terms from evaluating continuous FGs + DiscreteKeys discrete_keys = {{M(0), 2}, {M(1), 2}}; + vector probs = {0.061923317, 0.20415914, 0.18374323, 0.2}; + auto expectedConditional = + boost::make_shared(discrete_keys, probs); EXPECT(assert_equal(*actualConditional, *expectedConditional, 1e-6)); } @@ -250,7 +255,8 @@ TEST(HybridNonlinearISAM, Approx_inference) { HybridBayesTree::shared_ptr unprunedHybridBayesTree; HybridGaussianFactorGraph::shared_ptr unprunedRemainingGraph; std::tie(unprunedHybridBayesTree, unprunedRemainingGraph) = - switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering); + switching.linearizedFactorGraph + .BaseEliminateable::eliminatePartialMultifrontal(ordering); size_t maxNrLeaves = 5; incrementalHybrid.update(graph1, initial); From 6e6bbfff4ce6c748c982badcd136fa7e39f231c6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 10 Nov 2022 10:53:17 -0500 Subject: [PATCH 06/21] update docstring for Ordering::+= --- gtsam/inference/Ordering.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 97d5bf110f..c9c6a61765 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -73,7 +73,7 @@ class Ordering: public KeyVector { /** * @brief Append new keys to the ordering as `ordering += keys`. * - * @param key + * @param keys The key vector to append to this ordering. * @return The ordering variable with appended keys. */ This& operator+=(KeyVector& keys); From 5e2cdfdd3bd0eb16e79cedd2fc39cebf064368a2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 12 Nov 2022 23:07:34 -0500 Subject: [PATCH 07/21] make continuousProbPrimes and continuousDeltas as templates --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 112 --------------------- gtsam/hybrid/HybridGaussianFactorGraph.h | 81 ++++++++++++--- 2 files changed, 69 insertions(+), 124 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index d07b728b58..4dc309e7b7 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -528,118 +528,6 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::probPrime( return prob_tree; } -/* ************************************************************************ */ -DecisionTree -HybridGaussianFactorGraph::continuousDelta( - const DiscreteKeys &discrete_keys, - const boost::shared_ptr &continuousBayesNet, - const std::vector &assignments) const { - // Create a decision tree of all the different VectorValues - std::vector vector_values; - for (const DiscreteValues &assignment : assignments) { - VectorValues values = continuousBayesNet->optimize(assignment); - vector_values.push_back(boost::make_shared(values)); - } - DecisionTree delta_tree(discrete_keys, - vector_values); - - return delta_tree; -} - -/* ************************************************************************ */ -DecisionTree -HybridGaussianFactorGraph::continuousDelta( - const DiscreteKeys &discrete_keys, - const boost::shared_ptr &continuousBayesTree, - const std::vector &assignments) const { - // Create a decision tree of all the different VectorValues - std::vector vector_values; - for (const DiscreteValues &assignment : assignments) { - VectorValues values = continuousBayesTree->optimize(assignment); - vector_values.push_back(boost::make_shared(values)); - } - DecisionTree delta_tree(discrete_keys, - vector_values); - - return delta_tree; -} - -/* ************************************************************************ */ -AlgebraicDecisionTree HybridGaussianFactorGraph::continuousProbPrimes( - const DiscreteKeys &orig_discrete_keys, - const boost::shared_ptr &continuousBayesNet) const { - // Generate all possible assignments. - const std::vector assignments = - DiscreteValues::CartesianProduct(orig_discrete_keys); - - // Save a copy of the original discrete key ordering - DiscreteKeys discrete_keys(orig_discrete_keys); - // Reverse discrete keys order for correct tree construction - std::reverse(discrete_keys.begin(), discrete_keys.end()); - - // Create a decision tree of all the different VectorValues - DecisionTree delta_tree = - this->continuousDelta(discrete_keys, continuousBayesNet, assignments); - - // Get the probPrime tree with the correct leaf probabilities - std::vector probPrimes; - for (const DiscreteValues &assignment : assignments) { - VectorValues delta = *delta_tree(assignment); - - // If VectorValues is empty, it means this is a pruned branch. - // Set thr probPrime to 0.0. - if (delta.size() == 0) { - probPrimes.push_back(0.0); - continue; - } - - // Compute the error given the delta and the assignment. - double error = this->error(delta, assignment); - probPrimes.push_back(exp(-error)); - } - - AlgebraicDecisionTree probPrimeTree(discrete_keys, probPrimes); - return probPrimeTree; -} - -/* ************************************************************************ */ -AlgebraicDecisionTree HybridGaussianFactorGraph::continuousProbPrimes( - const DiscreteKeys &orig_discrete_keys, - const boost::shared_ptr &continuousBayesTree) const { - // Generate all possible assignments. - const std::vector assignments = - DiscreteValues::CartesianProduct(orig_discrete_keys); - - // Save a copy of the original discrete key ordering - DiscreteKeys discrete_keys(orig_discrete_keys); - // Reverse discrete keys order for correct tree construction - std::reverse(discrete_keys.begin(), discrete_keys.end()); - - // Create a decision tree of all the different VectorValues - DecisionTree delta_tree = - this->continuousDelta(discrete_keys, continuousBayesTree, assignments); - - // Get the probPrime tree with the correct leaf probabilities - std::vector probPrimes; - for (const DiscreteValues &assignment : assignments) { - VectorValues delta = *delta_tree(assignment); - - // If VectorValues is empty, it means this is a pruned branch. - // Set thr probPrime to 0.0. - if (delta.size() == 0) { - probPrimes.push_back(0.0); - continue; - } - - // Compute the error given the delta and the assignment. - double error = this->error(delta, assignment); - probPrimes.push_back(exp(-error)); - } - - AlgebraicDecisionTree probPrimeTree(discrete_keys, probPrimes); - return probPrimeTree; -} - /* ************************************************************************ */ std::pair HybridGaussianFactorGraph::separateContinuousDiscreteOrdering( diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index fb8ebbdc4b..d4da66400f 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -220,44 +220,89 @@ class GTSAM_EXPORT HybridGaussianFactorGraph * @brief Compute the VectorValues solution for the continuous variables for * each mode. * + * @tparam BAYES Template on the type of Bayes graph, either a bayes net or a + * bayes tree. * @param discrete_keys The discrete keys which form all the modes. - * @param continuousBayesNet The Bayes Net representing the continuous + * @param continuousBayesNet The Bayes Net/Tree representing the continuous * eliminated variables. * @param assignments List of all discrete assignments to create the final * decision tree. * @return DecisionTree */ + template DecisionTree continuousDelta( const DiscreteKeys& discrete_keys, - const boost::shared_ptr& continuousBayesNet, - const std::vector& assignments) const; - DecisionTree continuousDelta( - const DiscreteKeys& discrete_keys, - const boost::shared_ptr& continuousBayesTree, - const std::vector& assignments) const; + const boost::shared_ptr& continuousBayesNet, + const std::vector& assignments) const { + // Create a decision tree of all the different VectorValues + std::vector vector_values; + for (const DiscreteValues& assignment : assignments) { + VectorValues values = continuousBayesNet->optimize(assignment); + vector_values.push_back(boost::make_shared(values)); + } + DecisionTree delta_tree(discrete_keys, + vector_values); + + return delta_tree; + } /** * @brief Compute the unnormalized probabilities of the continuous variables * for each of the modes. * + * @tparam BAYES Template on the type of Bayes graph, either a bayes net or a + * bayes tree. * @param discrete_keys The discrete keys which form all the modes. * @param continuousBayesNet The Bayes Net representing the continuous * eliminated variables. * @return AlgebraicDecisionTree */ + template AlgebraicDecisionTree continuousProbPrimes( const DiscreteKeys& discrete_keys, - const boost::shared_ptr& continuousBayesNet) const; - AlgebraicDecisionTree continuousProbPrimes( - const DiscreteKeys& discrete_keys, - const boost::shared_ptr& continuousBayesTree) const; + const boost::shared_ptr& continuousBayesNet) const { + // Generate all possible assignments. + const std::vector assignments = + DiscreteValues::CartesianProduct(discrete_keys); + + // Save a copy of the original discrete key ordering + DiscreteKeys reversed_discrete_keys(discrete_keys); + // Reverse discrete keys order for correct tree construction + std::reverse(reversed_discrete_keys.begin(), reversed_discrete_keys.end()); + + // Create a decision tree of all the different VectorValues + DecisionTree delta_tree = + this->continuousDelta(reversed_discrete_keys, continuousBayesNet, + assignments); + + // Get the probPrime tree with the correct leaf probabilities + std::vector probPrimes; + for (const DiscreteValues& assignment : assignments) { + VectorValues delta = *delta_tree(assignment); + + // If VectorValues is empty, it means this is a pruned branch. + // Set thr probPrime to 0.0. + if (delta.size() == 0) { + probPrimes.push_back(0.0); + continue; + } + + // Compute the error given the delta and the assignment. + double error = this->error(delta, assignment); + probPrimes.push_back(exp(-error)); + } + + AlgebraicDecisionTree probPrimeTree(reversed_discrete_keys, + probPrimes); + return probPrimeTree; + } std::pair separateContinuousDiscreteOrdering( const Ordering& ordering) const; /** * @brief Custom elimination function which computes the correct - * continuous probabilities. + * continuous probabilities. Returns a bayes net. * * @param continuous Optional ordering for all continuous variables. * @param discrete Optional ordering for all discrete variables. @@ -269,27 +314,39 @@ class GTSAM_EXPORT HybridGaussianFactorGraph const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; + /// Sequential elimination overload for hybrid boost::shared_ptr eliminateSequential( OptionalOrderingType orderingType = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; + /// Sequential elimination overload for hybrid boost::shared_ptr eliminateSequential( const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; + /** + * @brief Custom elimination function which computes the correct + * continuous probabilities. Returns a bayes tree. + * + * @param continuous Optional ordering for all continuous variables. + * @param discrete Optional ordering for all discrete variables. + * @return boost::shared_ptr + */ boost::shared_ptr eliminateHybridMultifrontal( const boost::optional continuous = boost::none, const boost::optional discrete = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; + /// Multifrontal elimination overload for hybrid boost::shared_ptr eliminateMultifrontal( OptionalOrderingType orderingType = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; + /// Multifrontal elimination overload for hybrid boost::shared_ptr eliminateMultifrontal( const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, From 239412956c3632f9da4985cae7583dd3a9c1f31e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 15 Nov 2022 00:50:03 -0500 Subject: [PATCH 08/21] address review comments --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 35 ++++-- gtsam/hybrid/HybridGaussianFactorGraph.h | 6 +- gtsam/hybrid/tests/testHybridBayesTree.cpp | 1 - gtsam/hybrid/tests/testHybridEstimation.cpp | 117 ++++++++++++------ gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 2 +- .../hybrid/tests/testHybridNonlinearISAM.cpp | 2 +- 6 files changed, 107 insertions(+), 56 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 4dc309e7b7..6218303381 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -557,9 +557,9 @@ HybridGaussianFactorGraph::eliminateHybridSequential( const boost::optional continuous, const boost::optional discrete, const Eliminate &function, OptionalVariableIndex variableIndex) const { - Ordering continuous_ordering = + const Ordering continuous_ordering = continuous ? *continuous : Ordering(this->continuousKeys()); - Ordering discrete_ordering = + const Ordering discrete_ordering = discrete ? *discrete : Ordering(this->discreteKeys()); // Eliminate continuous @@ -570,7 +570,8 @@ HybridGaussianFactorGraph::eliminateHybridSequential( function, variableIndex); // Get the last continuous conditional which will have all the discrete keys - auto last_conditional = bayesNet->at(bayesNet->size() - 1); + HybridConditional::shared_ptr last_conditional = + bayesNet->at(bayesNet->size() - 1); DiscreteKeys discrete_keys = last_conditional->discreteKeys(); // If not discrete variables, return the eliminated bayes net. @@ -578,9 +579,11 @@ HybridGaussianFactorGraph::eliminateHybridSequential( return bayesNet; } - AlgebraicDecisionTree probPrimeTree = + // DecisionTree for P'(X|M, Z) for all mode sequences M + const AlgebraicDecisionTree probPrimeTree = this->continuousProbPrimes(discrete_keys, bayesNet); + // Add the model selection factor P(M|Z) discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); // Perform discrete elimination @@ -622,9 +625,9 @@ HybridGaussianFactorGraph::eliminateHybridMultifrontal( const boost::optional continuous, const boost::optional discrete, const Eliminate &function, OptionalVariableIndex variableIndex) const { - Ordering continuous_ordering = + const Ordering continuous_ordering = continuous ? *continuous : Ordering(this->continuousKeys()); - Ordering discrete_ordering = + const Ordering discrete_ordering = discrete ? *discrete : Ordering(this->discreteKeys()); // Eliminate continuous @@ -635,9 +638,9 @@ HybridGaussianFactorGraph::eliminateHybridMultifrontal( function, variableIndex); // Get the last continuous conditional which will have all the discrete - Key last_continuous_key = - continuous_ordering.at(continuous_ordering.size() - 1); - auto last_conditional = (*bayesTree)[last_continuous_key]->conditional(); + const Key last_continuous_key = continuous_ordering.back(); + HybridConditional::shared_ptr last_conditional = + (*bayesTree)[last_continuous_key]->conditional(); DiscreteKeys discrete_keys = last_conditional->discreteKeys(); // If not discrete variables, return the eliminated bayes net. @@ -645,16 +648,24 @@ HybridGaussianFactorGraph::eliminateHybridMultifrontal( return bayesTree; } - AlgebraicDecisionTree probPrimeTree = + // DecisionTree for P'(X|M, Z) for all mode sequences M + const AlgebraicDecisionTree probPrimeTree = this->continuousProbPrimes(discrete_keys, bayesTree); + // Add the model selection factor P(M|Z) discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); - auto updatedBayesTree = + // Eliminate discrete variables to get the discrete bayes tree. + // This bayes tree will be updated with the + // continuous variables as the child nodes. + HybridBayesTree::shared_ptr updatedBayesTree = discreteGraph->BaseEliminateable::eliminateMultifrontal(discrete_ordering, function); - auto discrete_clique = (*updatedBayesTree)[discrete_ordering.at(0)]; + // Get the clique with all the discrete keys. + // There should only be 1 clique. + const HybridBayesTree::sharedClique discrete_clique = + (*updatedBayesTree)[discrete_ordering.at(0)]; std::set clique_set; for (auto node : bayesTree->nodes()) { diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index d4da66400f..47b94070f7 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -217,8 +217,10 @@ class GTSAM_EXPORT HybridGaussianFactorGraph const DiscreteValues& discreteValues) const; /** - * @brief Compute the VectorValues solution for the continuous variables for - * each mode. + * @brief Helper method to compute the VectorValues solution for the + * continuous variables for each discrete mode. + * Used as a helper to compute q(\mu | M, Z) which is used by + * both P(X | M, Z) and P(M | Z). * * @tparam BAYES Template on the type of Bayes graph, either a bayes net or a * bayes tree. diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index ab0f3c2d94..9bc12c31d7 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -141,7 +141,6 @@ TEST(HybridBayesTree, Optimize) { DiscreteKeys discrete_keys = {{M(0), 2}, {M(1), 2}, {M(2), 2}}; vector probs = {0.012519475, 0.041280228, 0.075018647, 0.081663656, 0.037152205, 0.12248971, 0.07349729, 0.08}; - AlgebraicDecisionTree potentials(discrete_keys, probs); dfg.emplace_shared(discrete_keys, probs); DiscreteValues expectedMPE = dfg.optimize(); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index ec5d6fb7d2..431e5769f5 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -79,6 +79,8 @@ TEST(HybridEstimation, Incremental) { // Ground truth discrete seq std::vector discrete_seq = {1, 1, 0, 0, 0, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0, 1, 1, 0, 0, 0}; + // Switching example of robot moving in 1D with given measurements and equal + // mode priors. Switching switching(K, 1.0, 0.1, measurements, "1/1 1/1"); HybridSmoother smoother; HybridNonlinearFactorGraph graph; @@ -136,7 +138,7 @@ TEST(HybridEstimation, Incremental) { * @param between_sigma Noise model sigma for the between factor. * @return GaussianFactorGraph::shared_ptr */ -GaussianFactorGraph::shared_ptr specificProblem( +GaussianFactorGraph::shared_ptr specificModesFactorGraph( size_t K, const std::vector& measurements, const std::vector& discrete_seq, double measurement_sigma = 0.1, double between_sigma = 1.0) { @@ -184,7 +186,7 @@ std::vector getDiscreteSequence(size_t x) { } /** - * @brief Helper method to get the probPrimeTree + * @brief Helper method to get the tree of unnormalized probabilities * as per the new elimination scheme. * * @param graph The HybridGaussianFactorGraph to eliminate. @@ -242,18 +244,15 @@ AlgebraicDecisionTree probPrimeTree( TEST(HybridEstimation, Probability) { constexpr size_t K = 4; std::vector measurements = {0, 1, 2, 2}; - - // This is the correct sequence - // std::vector discrete_seq = {1, 1, 0}; - double between_sigma = 1.0, measurement_sigma = 0.1; std::vector expected_errors, expected_prob_primes; + std::map> discrete_seq_map; for (size_t i = 0; i < pow(2, K - 1); i++) { - std::vector discrete_seq = getDiscreteSequence(i); + discrete_seq_map[i] = getDiscreteSequence(i); - GaussianFactorGraph::shared_ptr linear_graph = specificProblem( - K, measurements, discrete_seq, measurement_sigma, between_sigma); + GaussianFactorGraph::shared_ptr linear_graph = specificModesFactorGraph( + K, measurements, discrete_seq_map[i], measurement_sigma, between_sigma); auto bayes_net = linear_graph->eliminateSequential(); @@ -263,7 +262,10 @@ TEST(HybridEstimation, Probability) { expected_prob_primes.push_back(linear_graph->probPrime(values)); } - Switching switching(K, between_sigma, measurement_sigma, measurements); + // Switching example of robot moving in 1D with given measurements and equal + // mode priors. + Switching switching(K, between_sigma, measurement_sigma, measurements, + "1/1 1/1"); auto graph = switching.linearizedFactorGraph; Ordering ordering = getOrdering(graph, HybridGaussianFactorGraph()); @@ -298,26 +300,30 @@ TEST(HybridEstimation, Probability) { // Test if the probPrimeTree matches the probability of // the individual factor graphs for (size_t i = 0; i < pow(2, K - 1); i++) { - std::vector discrete_seq = getDiscreteSequence(i); Assignment discrete_assignment; - for (size_t v = 0; v < discrete_seq.size(); v++) { - discrete_assignment[M(v)] = discrete_seq[v]; + for (size_t v = 0; v < discrete_seq_map[i].size(); v++) { + discrete_assignment[M(v)] = discrete_seq_map[i][v]; } EXPECT_DOUBLES_EQUAL(expected_prob_primes.at(i), probPrimeTree(discrete_assignment), 1e-8); } - // remainingGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); + discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); + + Ordering discrete(graph.discreteKeys()); + auto discreteBayesNet = + discreteGraph->BaseEliminateable::eliminateSequential(discrete); + bayesNet->add(*discreteBayesNet); - // Ordering discrete(graph.discreteKeys()); - // // remainingGraph->print("remainingGraph"); - // // discrete.print(); - // auto discreteBayesNet = remainingGraph->eliminateSequential(discrete); - // bayesNet->add(*discreteBayesNet); - // // bayesNet->print(); + HybridValues hybrid_values = bayesNet->optimize(); - // HybridValues hybrid_values = bayesNet->optimize(); - // hybrid_values.discrete().print(); + // This is the correct sequence as designed + DiscreteValues discrete_seq; + discrete_seq[M(0)] = 1; + discrete_seq[M(1)] = 1; + discrete_seq[M(2)] = 0; + + EXPECT(assert_equal(discrete_seq, hybrid_values.discrete())); } /****************************************************************************/ @@ -330,31 +336,34 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { constexpr size_t K = 4; std::vector measurements = {0, 1, 2, 2}; - // This is the correct sequence - // std::vector discrete_seq = {1, 1, 0}; - double between_sigma = 1.0, measurement_sigma = 0.1; + // For each discrete mode sequence, create the individual factor graphs and + // optimize each. std::vector expected_errors, expected_prob_primes; + std::map> discrete_seq_map; for (size_t i = 0; i < pow(2, K - 1); i++) { - std::vector discrete_seq = getDiscreteSequence(i); + discrete_seq_map[i] = getDiscreteSequence(i); - GaussianFactorGraph::shared_ptr linear_graph = specificProblem( - K, measurements, discrete_seq, measurement_sigma, between_sigma); + GaussianFactorGraph::shared_ptr linear_graph = specificModesFactorGraph( + K, measurements, discrete_seq_map[i], measurement_sigma, between_sigma); auto bayes_tree = linear_graph->eliminateMultifrontal(); VectorValues values = bayes_tree->optimize(); - std::cout << i << " " << linear_graph->error(values) << std::endl; expected_errors.push_back(linear_graph->error(values)); expected_prob_primes.push_back(linear_graph->probPrime(values)); } - Switching switching(K, between_sigma, measurement_sigma, measurements); + // Switching example of robot moving in 1D with given measurements and equal + // mode priors. + Switching switching(K, between_sigma, measurement_sigma, measurements, + "1/1 1/1"); auto graph = switching.linearizedFactorGraph; Ordering ordering = getOrdering(graph, HybridGaussianFactorGraph()); + // Get the tree of unnormalized probabilities for each mode sequence. AlgebraicDecisionTree expected_probPrimeTree = probPrimeTree(graph); // Eliminate continuous @@ -379,10 +388,9 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { // Test if the probPrimeTree matches the probability of // the individual factor graphs for (size_t i = 0; i < pow(2, K - 1); i++) { - std::vector discrete_seq = getDiscreteSequence(i); Assignment discrete_assignment; - for (size_t v = 0; v < discrete_seq.size(); v++) { - discrete_assignment[M(v)] = discrete_seq[v]; + for (size_t v = 0; v < discrete_seq_map[i].size(); v++) { + discrete_assignment[M(v)] = discrete_seq_map[i][v]; } EXPECT_DOUBLES_EQUAL(expected_prob_primes.at(i), probPrimeTree(discrete_assignment), 1e-8); @@ -390,13 +398,44 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); - // Ordering discrete(graph.discreteKeys()); - // auto discreteBayesTree = discreteGraph->eliminateMultifrontal(discrete); - // // DiscreteBayesTree should have only 1 clique - // bayesTree->addClique((*discreteBayesTree)[discrete.at(0)]); + Ordering discrete(graph.discreteKeys()); + auto discreteBayesTree = + discreteGraph->BaseEliminateable::eliminateMultifrontal(discrete); + + EXPECT_LONGS_EQUAL(1, discreteBayesTree->size()); + // DiscreteBayesTree should have only 1 clique + auto discrete_clique = (*discreteBayesTree)[discrete.at(0)]; + + std::set clique_set; + for (auto node : bayesTree->nodes()) { + clique_set.insert(node.second); + } + + // Set the root of the bayes tree as the discrete clique + for (auto clique : clique_set) { + if (clique->conditional()->parents() == + discrete_clique->conditional()->frontals()) { + discreteBayesTree->addClique(clique, discrete_clique); + + } else { + // Remove the clique from the children of the parents since it will get + // added again in addClique. + auto clique_it = std::find(clique->parent()->children.begin(), + clique->parent()->children.end(), clique); + clique->parent()->children.erase(clique_it); + discreteBayesTree->addClique(clique, clique->parent()); + } + } + + HybridValues hybrid_values = discreteBayesTree->optimize(); + + // This is the correct sequence as designed + DiscreteValues discrete_seq; + discrete_seq[M(0)] = 1; + discrete_seq[M(1)] = 1; + discrete_seq[M(2)] = 0; - // // HybridValues hybrid_values = bayesNet->optimize(); - // // hybrid_values.discrete().print(); + EXPECT(assert_equal(discrete_seq, hybrid_values.discrete())); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 09403853f6..4ba6f88a50 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -176,7 +176,7 @@ TEST(HybridGaussianElimination, IncrementalInference) { auto discreteConditional = isam[M(1)]->conditional()->asDiscreteConditional(); - // Test if the probability values are as expected with regression tests. + // Test the probability values with regression tests. DiscreteValues assignment; EXPECT(assert_equal(0.166667, m00_prob, 1e-5)); assignment[M(0)] = 0; diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index e7e65b1232..46d5c43245 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -195,7 +195,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) { auto discreteConditional = bayesTree[M(1)]->conditional()->asDiscreteConditional(); - // Test if the probability values are as expected with regression tests. + // Test the probability values with regression tests. DiscreteValues assignment; EXPECT(assert_equal(0.166667, m00_prob, 1e-5)); assignment[M(0)] = 0; From 05b2d3169f0a9e72e750c679b4dd92c75b7e0cda Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 3 Dec 2022 06:43:46 +0530 Subject: [PATCH 09/21] better printing --- gtsam/hybrid/GaussianMixture.cpp | 2 +- gtsam/hybrid/HybridFactor.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 4819eda657..0b6fcdff7c 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -105,7 +105,7 @@ bool GaussianMixture::equals(const HybridFactor &lf, double tol) const { /* *******************************************************************************/ void GaussianMixture::print(const std::string &s, const KeyFormatter &formatter) const { - std::cout << s; + std::cout << (s.empty() ? "" : s + "\n"); if (isContinuous()) std::cout << "Continuous "; if (isDiscrete()) std::cout << "Discrete "; if (isHybrid()) std::cout << "Hybrid "; diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index 1216fd9224..b25e97f051 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -81,7 +81,7 @@ bool HybridFactor::equals(const HybridFactor &lf, double tol) const { /* ************************************************************************ */ void HybridFactor::print(const std::string &s, const KeyFormatter &formatter) const { - std::cout << s; + std::cout << (s.empty() ? "" : s + "\n"); if (isContinuous_) std::cout << "Continuous "; if (isDiscrete_) std::cout << "Discrete "; if (isHybrid_) std::cout << "Hybrid "; From 3eaf4cc910fa0cdf34023c4ebd0bf4b37354499b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 3 Dec 2022 17:00:51 +0530 Subject: [PATCH 10/21] move multifrontal optimize test to testHybridBayesTree and update docstrings --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 3 +-- gtsam/hybrid/tests/testHybridBayesNet.cpp | 19 ------------------- gtsam/hybrid/tests/testHybridBayesTree.cpp | 19 +++++++++++++++++++ .../tests/testHybridNonlinearFactorGraph.cpp | 4 ++-- 4 files changed, 22 insertions(+), 23 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 6218303381..1d52a24afe 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -257,7 +257,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // If there are no more continuous parents, then we should create here a // DiscreteFactor, with the error for each discrete choice. if (keysOfSeparator.empty()) { - // TODO(Varun) Use the math from the iMHS_Math-1-indexed document VectorValues empty_values; auto factorProb = [&](const GaussianFactor::shared_ptr &factor) { if (!factor) { @@ -574,7 +573,7 @@ HybridGaussianFactorGraph::eliminateHybridSequential( bayesNet->at(bayesNet->size() - 1); DiscreteKeys discrete_keys = last_conditional->discreteKeys(); - // If not discrete variables, return the eliminated bayes net. + // If no discrete variables, return the eliminated bayes net. if (discrete_keys.size() == 0) { return bayesNet; } diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 8b8ca976b0..d2951ddaa7 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -164,25 +164,6 @@ TEST(HybridBayesNet, Optimize) { EXPECT(assert_equal(expectedValues, delta.continuous(), 1e-5)); } -/* ****************************************************************************/ -// Test bayes net multifrontal optimize -TEST(HybridBayesNet, OptimizeMultifrontal) { - Switching s(4); - - Ordering hybridOrdering = s.linearizedFactorGraph.getHybridOrdering(); - HybridBayesTree::shared_ptr hybridBayesTree = - s.linearizedFactorGraph.eliminateMultifrontal(hybridOrdering); - HybridValues delta = hybridBayesTree->optimize(); - - VectorValues expectedValues; - expectedValues.insert(X(0), -0.999904 * Vector1::Ones()); - expectedValues.insert(X(1), -0.99029 * Vector1::Ones()); - expectedValues.insert(X(2), -1.00971 * Vector1::Ones()); - expectedValues.insert(X(3), -1.0001 * Vector1::Ones()); - - EXPECT(assert_equal(expectedValues, delta.continuous(), 1e-5)); -} - /* ****************************************************************************/ // Test bayes net error TEST(HybridBayesNet, Error) { diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index 9bc12c31d7..3992aa023b 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -32,6 +32,25 @@ using noiseModel::Isotropic; using symbol_shorthand::M; using symbol_shorthand::X; +/* ****************************************************************************/ +// Test multifrontal optimize +TEST(HybridBayesTree, OptimizeMultifrontal) { + Switching s(4); + + Ordering hybridOrdering = s.linearizedFactorGraph.getHybridOrdering(); + HybridBayesTree::shared_ptr hybridBayesTree = + s.linearizedFactorGraph.eliminateMultifrontal(hybridOrdering); + HybridValues delta = hybridBayesTree->optimize(); + + VectorValues expectedValues; + expectedValues.insert(X(0), -0.999904 * Vector1::Ones()); + expectedValues.insert(X(1), -0.99029 * Vector1::Ones()); + expectedValues.insert(X(2), -1.00971 * Vector1::Ones()); + expectedValues.insert(X(3), -1.0001 * Vector1::Ones()); + + EXPECT(assert_equal(expectedValues, delta.continuous(), 1e-5)); +} + /* ****************************************************************************/ // Test for optimizing a HybridBayesTree with a given assignment. TEST(HybridBayesTree, OptimizeAssignment) { diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index f8c61baf7c..e3b7f761a7 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -386,11 +386,11 @@ TEST(HybridFactorGraph, Partial_Elimination) { auto linearizedFactorGraph = self.linearizedFactorGraph; - // Create ordering. + // Create ordering of only continuous variables. Ordering ordering; for (size_t k = 0; k < self.K; k++) ordering += X(k); - // Eliminate partially. + // Eliminate partially i.e. only continuous part. HybridBayesNet::shared_ptr hybridBayesNet; HybridGaussianFactorGraph::shared_ptr remainingFactorGraph; std::tie(hybridBayesNet, remainingFactorGraph) = From cd3cfa0faa5ebcacc05d7ccbdfc21bcdf505d0f9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 3 Dec 2022 17:14:11 +0530 Subject: [PATCH 11/21] moved sequential elimination code to HybridEliminationTree --- gtsam/hybrid/HybridEliminationTree.cpp | 12 ++- gtsam/hybrid/HybridEliminationTree.h | 107 +++++++++++++++++++++ gtsam/hybrid/HybridFactor.cpp | 2 +- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 68 ------------- gtsam/hybrid/HybridGaussianFactorGraph.h | 26 ----- gtsam/hybrid/HybridSmoother.cpp | 2 +- 6 files changed, 119 insertions(+), 98 deletions(-) diff --git a/gtsam/hybrid/HybridEliminationTree.cpp b/gtsam/hybrid/HybridEliminationTree.cpp index c2df2dd600..fe91905713 100644 --- a/gtsam/hybrid/HybridEliminationTree.cpp +++ b/gtsam/hybrid/HybridEliminationTree.cpp @@ -27,12 +27,20 @@ template class EliminationTree; HybridEliminationTree::HybridEliminationTree( const HybridGaussianFactorGraph& factorGraph, const VariableIndex& structure, const Ordering& order) - : Base(factorGraph, structure, order) {} + : Base(factorGraph, structure, order), + graph_(factorGraph), + variable_index_(structure) { + // Segregate the continuous and the discrete keys + std::tie(continuous_ordering_, discrete_ordering_) = + graph_.separateContinuousDiscreteOrdering(order); +} /* ************************************************************************* */ HybridEliminationTree::HybridEliminationTree( const HybridGaussianFactorGraph& factorGraph, const Ordering& order) - : Base(factorGraph, order) {} + : Base(factorGraph, order), + graph_(factorGraph), + variable_index_(VariableIndex(factorGraph)) {} /* ************************************************************************* */ bool HybridEliminationTree::equals(const This& other, double tol) const { diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index b2dd1bd9c8..dfa88bf4e5 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -33,6 +33,12 @@ class GTSAM_EXPORT HybridEliminationTree private: friend class ::EliminationTreeTester; + Ordering continuous_ordering_, discrete_ordering_; + /// Used to store the original factor graph to eliminate + HybridGaussianFactorGraph graph_; + /// Store the provided variable index. + VariableIndex variable_index_; + public: typedef EliminationTree Base; ///< Base class @@ -66,6 +72,107 @@ class GTSAM_EXPORT HybridEliminationTree /** Test whether the tree is equal to another */ bool equals(const This& other, double tol = 1e-9) const; + + /** + * @brief Helper method to eliminate continuous variables. + * + * If no continuous variables exist, return an empty bayes net + * and the original graph. + * + * @param function Elimination function for hybrid elimination. + * @return std::pair, + * boost::shared_ptr > + */ + std::pair, boost::shared_ptr> + eliminateContinuous(Eliminate function) const { + if (continuous_ordering_.size() > 0) { + This continuous_etree(graph_, variable_index_, continuous_ordering_); + return continuous_etree.Base::eliminate(function); + + } else { + BayesNetType::shared_ptr bayesNet = boost::make_shared(); + FactorGraphType::shared_ptr discreteGraph = + boost::make_shared(graph_); + return std::make_pair(bayesNet, discreteGraph); + } + } + + /** + * @brief Helper method to eliminate the discrete variables after the + * continuous variables have been eliminated. + * + * If there are no discrete variables, return an empty bayes net and the + * discreteGraph which is passed in. + * + * @param function Elimination function + * @param discreteGraph The factor graph with the factor ϕ(X | M, Z). + * @return std::pair, + * boost::shared_ptr > + */ + std::pair, boost::shared_ptr> + eliminateDiscrete(Eliminate function, + const FactorGraphType::shared_ptr& discreteGraph) const { + BayesNetType::shared_ptr discreteBayesNet; + FactorGraphType::shared_ptr finalGraph; + if (discrete_ordering_.size() > 0) { + This discrete_etree(*discreteGraph, VariableIndex(*discreteGraph), + discrete_ordering_); + + std::tie(discreteBayesNet, finalGraph) = + discrete_etree.Base::eliminate(function); + + } else { + discreteBayesNet = boost::make_shared(); + finalGraph = discreteGraph; + } + + return std::make_pair(discreteBayesNet, finalGraph); + } + + /** + * @brief Override the EliminationTree eliminate method + * so we can perform hybrid elimination correctly. + * + * @param function + * @return std::pair, + * boost::shared_ptr > + */ + std::pair, boost::shared_ptr> + eliminate(Eliminate function) const { + // Perform continuous elimination + BayesNetType::shared_ptr bayesNet; + FactorGraphType::shared_ptr discreteGraph; + std::tie(bayesNet, discreteGraph) = this->eliminateContinuous(function); + + // If we have eliminated continuous variables + // and have discrete variables to eliminate, + // then compute P(X | M, Z) + if (continuous_ordering_.size() > 0 && discrete_ordering_.size() > 0) { + // Get the last continuous conditional + // which will have all the discrete keys + HybridConditional::shared_ptr last_conditional = + bayesNet->at(bayesNet->size() - 1); + DiscreteKeys discrete_keys = last_conditional->discreteKeys(); + + // DecisionTree for P'(X|M, Z) for all mode sequences M + const AlgebraicDecisionTree probPrimeTree = + graph_.continuousProbPrimes(discrete_keys, bayesNet); + + // Add the model selection factor P(M|Z) + discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); + } + + // Perform discrete elimination + BayesNetType::shared_ptr discreteBayesNet; + FactorGraphType::shared_ptr finalGraph; + std::tie(discreteBayesNet, finalGraph) = + eliminateDiscrete(function, discreteGraph); + + // Add the discrete conditionals to the hybrid conditionals + bayesNet->add(*discreteBayesNet); + + return std::make_pair(bayesNet, finalGraph); + } }; } // namespace gtsam diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index b25e97f051..1216fd9224 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -81,7 +81,7 @@ bool HybridFactor::equals(const HybridFactor &lf, double tol) const { /* ************************************************************************ */ void HybridFactor::print(const std::string &s, const KeyFormatter &formatter) const { - std::cout << (s.empty() ? "" : s + "\n"); + std::cout << s; if (isContinuous_) std::cout << "Continuous "; if (isDiscrete_) std::cout << "Discrete "; if (isHybrid_) std::cout << "Hybrid "; diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 1d52a24afe..1afe4f38af 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -550,74 +550,6 @@ HybridGaussianFactorGraph::separateContinuousDiscreteOrdering( return std::make_pair(continuous_ordering, discrete_ordering); } -/* ************************************************************************ */ -boost::shared_ptr -HybridGaussianFactorGraph::eliminateHybridSequential( - const boost::optional continuous, - const boost::optional discrete, const Eliminate &function, - OptionalVariableIndex variableIndex) const { - const Ordering continuous_ordering = - continuous ? *continuous : Ordering(this->continuousKeys()); - const Ordering discrete_ordering = - discrete ? *discrete : Ordering(this->discreteKeys()); - - // Eliminate continuous - HybridBayesNet::shared_ptr bayesNet; - HybridGaussianFactorGraph::shared_ptr discreteGraph; - std::tie(bayesNet, discreteGraph) = - BaseEliminateable::eliminatePartialSequential(continuous_ordering, - function, variableIndex); - - // Get the last continuous conditional which will have all the discrete keys - HybridConditional::shared_ptr last_conditional = - bayesNet->at(bayesNet->size() - 1); - DiscreteKeys discrete_keys = last_conditional->discreteKeys(); - - // If no discrete variables, return the eliminated bayes net. - if (discrete_keys.size() == 0) { - return bayesNet; - } - - // DecisionTree for P'(X|M, Z) for all mode sequences M - const AlgebraicDecisionTree probPrimeTree = - this->continuousProbPrimes(discrete_keys, bayesNet); - - // Add the model selection factor P(M|Z) - discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); - - // Perform discrete elimination - HybridBayesNet::shared_ptr discreteBayesNet = - discreteGraph->BaseEliminateable::eliminateSequential( - discrete_ordering, function, variableIndex); - - bayesNet->add(*discreteBayesNet); - - return bayesNet; -} - -/* ************************************************************************ */ -boost::shared_ptr -HybridGaussianFactorGraph::eliminateSequential( - OptionalOrderingType orderingType, const Eliminate &function, - OptionalVariableIndex variableIndex) const { - return BaseEliminateable::eliminateSequential(orderingType, function, - variableIndex); -} - -/* ************************************************************************ */ -boost::shared_ptr -HybridGaussianFactorGraph::eliminateSequential( - const Ordering &ordering, const Eliminate &function, - OptionalVariableIndex variableIndex) const { - // Segregate the continuous and the discrete keys - Ordering continuous_ordering, discrete_ordering; - std::tie(continuous_ordering, discrete_ordering) = - this->separateContinuousDiscreteOrdering(ordering); - - return this->eliminateHybridSequential(continuous_ordering, discrete_ordering, - function, variableIndex); -} - /* ************************************************************************ */ boost::shared_ptr HybridGaussianFactorGraph::eliminateHybridMultifrontal( diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 47b94070f7..a0d80b1547 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -302,32 +302,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph std::pair separateContinuousDiscreteOrdering( const Ordering& ordering) const; - /** - * @brief Custom elimination function which computes the correct - * continuous probabilities. Returns a bayes net. - * - * @param continuous Optional ordering for all continuous variables. - * @param discrete Optional ordering for all discrete variables. - * @return boost::shared_ptr - */ - boost::shared_ptr eliminateHybridSequential( - const boost::optional continuous = boost::none, - const boost::optional discrete = boost::none, - const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; - - /// Sequential elimination overload for hybrid - boost::shared_ptr eliminateSequential( - OptionalOrderingType orderingType = boost::none, - const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; - - /// Sequential elimination overload for hybrid - boost::shared_ptr eliminateSequential( - const Ordering& ordering, - const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; - /** * @brief Custom elimination function which computes the correct * continuous probabilities. Returns a bayes tree. diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index 12f6949abf..7400053bfc 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -32,7 +32,7 @@ void HybridSmoother::update(HybridGaussianFactorGraph graph, addConditionals(graph, hybridBayesNet_, ordering); // Eliminate. - auto bayesNetFragment = graph.eliminateHybridSequential(); + auto bayesNetFragment = graph.eliminateSequential(); /// Prune if (maxNrLeaves) { From 15fffeb18adf952424789cf6835123bc97b2eb1b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 4 Dec 2022 14:30:01 +0530 Subject: [PATCH 12/21] add getters to HybridEliminationTree --- gtsam/hybrid/HybridEliminationTree.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index dfa88bf4e5..65d614ca3c 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -173,6 +173,13 @@ class GTSAM_EXPORT HybridEliminationTree return std::make_pair(bayesNet, finalGraph); } + + Ordering continuousOrdering() const { return continuous_ordering_; } + Ordering discreteOrdering() const { return discrete_ordering_; } + + /// Store the provided variable index. + VariableIndex variableIndex() const { return variable_index_; } + HybridGaussianFactorGraph graph() const { return graph_; } }; } // namespace gtsam From addbe2a5a57bfab3851a51a7193d92f3e2110bfa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 4 Dec 2022 14:55:17 +0530 Subject: [PATCH 13/21] override eliminate in HybridJunctionTree --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 93 ------------ gtsam/hybrid/HybridGaussianFactorGraph.h | 26 +--- gtsam/hybrid/HybridJunctionTree.cpp | 139 +++++++++++++++++- gtsam/hybrid/HybridJunctionTree.h | 67 ++++++++- .../tests/testHybridGaussianFactorGraph.cpp | 7 +- 5 files changed, 209 insertions(+), 123 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 1afe4f38af..c430fac2c6 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -550,98 +550,5 @@ HybridGaussianFactorGraph::separateContinuousDiscreteOrdering( return std::make_pair(continuous_ordering, discrete_ordering); } -/* ************************************************************************ */ -boost::shared_ptr -HybridGaussianFactorGraph::eliminateHybridMultifrontal( - const boost::optional continuous, - const boost::optional discrete, const Eliminate &function, - OptionalVariableIndex variableIndex) const { - const Ordering continuous_ordering = - continuous ? *continuous : Ordering(this->continuousKeys()); - const Ordering discrete_ordering = - discrete ? *discrete : Ordering(this->discreteKeys()); - - // Eliminate continuous - HybridBayesTree::shared_ptr bayesTree; - HybridGaussianFactorGraph::shared_ptr discreteGraph; - std::tie(bayesTree, discreteGraph) = - BaseEliminateable::eliminatePartialMultifrontal(continuous_ordering, - function, variableIndex); - - // Get the last continuous conditional which will have all the discrete - const Key last_continuous_key = continuous_ordering.back(); - HybridConditional::shared_ptr last_conditional = - (*bayesTree)[last_continuous_key]->conditional(); - DiscreteKeys discrete_keys = last_conditional->discreteKeys(); - - // If not discrete variables, return the eliminated bayes net. - if (discrete_keys.size() == 0) { - return bayesTree; - } - - // DecisionTree for P'(X|M, Z) for all mode sequences M - const AlgebraicDecisionTree probPrimeTree = - this->continuousProbPrimes(discrete_keys, bayesTree); - - // Add the model selection factor P(M|Z) - discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); - - // Eliminate discrete variables to get the discrete bayes tree. - // This bayes tree will be updated with the - // continuous variables as the child nodes. - HybridBayesTree::shared_ptr updatedBayesTree = - discreteGraph->BaseEliminateable::eliminateMultifrontal(discrete_ordering, - function); - - // Get the clique with all the discrete keys. - // There should only be 1 clique. - const HybridBayesTree::sharedClique discrete_clique = - (*updatedBayesTree)[discrete_ordering.at(0)]; - - std::set clique_set; - for (auto node : bayesTree->nodes()) { - clique_set.insert(node.second); - } - - // Set the root of the bayes tree as the discrete clique - for (auto clique : clique_set) { - if (clique->conditional()->parents() == - discrete_clique->conditional()->frontals()) { - updatedBayesTree->addClique(clique, discrete_clique); - - } else { - // Remove the clique from the children of the parents since it will get - // added again in addClique. - auto clique_it = std::find(clique->parent()->children.begin(), - clique->parent()->children.end(), clique); - clique->parent()->children.erase(clique_it); - updatedBayesTree->addClique(clique, clique->parent()); - } - } - return updatedBayesTree; -} - -/* ************************************************************************ */ -boost::shared_ptr -HybridGaussianFactorGraph::eliminateMultifrontal( - OptionalOrderingType orderingType, const Eliminate &function, - OptionalVariableIndex variableIndex) const { - return BaseEliminateable::eliminateMultifrontal(orderingType, function, - variableIndex); -} - -/* ************************************************************************ */ -boost::shared_ptr -HybridGaussianFactorGraph::eliminateMultifrontal( - const Ordering &ordering, const Eliminate &function, - OptionalVariableIndex variableIndex) const { - // Segregate the continuous and the discrete keys - Ordering continuous_ordering, discrete_ordering; - std::tie(continuous_ordering, discrete_ordering) = - this->separateContinuousDiscreteOrdering(ordering); - - return this->eliminateHybridMultifrontal( - continuous_ordering, discrete_ordering, function, variableIndex); -} } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index a0d80b1547..210ce50e93 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -302,31 +302,7 @@ class GTSAM_EXPORT HybridGaussianFactorGraph std::pair separateContinuousDiscreteOrdering( const Ordering& ordering) const; - /** - * @brief Custom elimination function which computes the correct - * continuous probabilities. Returns a bayes tree. - * - * @param continuous Optional ordering for all continuous variables. - * @param discrete Optional ordering for all discrete variables. - * @return boost::shared_ptr - */ - boost::shared_ptr eliminateHybridMultifrontal( - const boost::optional continuous = boost::none, - const boost::optional discrete = boost::none, - const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; - - /// Multifrontal elimination overload for hybrid - boost::shared_ptr eliminateMultifrontal( - OptionalOrderingType orderingType = boost::none, - const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; - - /// Multifrontal elimination overload for hybrid - boost::shared_ptr eliminateMultifrontal( - const Ordering& ordering, - const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + /** * @brief Return a Colamd constrained ordering where the discrete keys are diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 422c200a43..f233d4bef9 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -142,7 +142,8 @@ struct HybridConstructorTraversalData { /* ************************************************************************* */ HybridJunctionTree::HybridJunctionTree( - const HybridEliminationTree& eliminationTree) { + const HybridEliminationTree& eliminationTree) + : etree_(eliminationTree) { gttic(JunctionTree_FromEliminationTree); // Here we rely on the BayesNet having been produced by this elimination tree, // such that the conditionals are arranged in DFS post-order. We traverse the @@ -169,4 +170,140 @@ HybridJunctionTree::HybridJunctionTree( Base::remainingFactors_ = eliminationTree.remainingFactors(); } +/* ************************************************************************* */ +std::pair, + boost::shared_ptr> +HybridJunctionTree::eliminateContinuous( + const Eliminate& function, const HybridGaussianFactorGraph& graph, + const Ordering& continuous_ordering) const { + HybridBayesTree::shared_ptr continuousBayesTree; + HybridGaussianFactorGraph::shared_ptr discreteGraph; + + if (continuous_ordering.size() > 0) { + HybridEliminationTree continuous_etree(graph, etree_.variableIndex(), + continuous_ordering); + + This continuous_junction_tree(continuous_etree); + std::tie(continuousBayesTree, discreteGraph) = + continuous_junction_tree.Base::eliminate(function); + + } else { + continuousBayesTree = boost::make_shared(); + discreteGraph = boost::make_shared(graph); + } + + return std::make_pair(continuousBayesTree, discreteGraph); +} +/* ************************************************************************* */ +std::pair, + boost::shared_ptr> +HybridJunctionTree::eliminateDiscrete( + const Eliminate& function, + const HybridBayesTree::shared_ptr& continuousBayesTree, + const HybridGaussianFactorGraph::shared_ptr& discreteGraph, + const Ordering& discrete_ordering) const { + HybridBayesTree::shared_ptr updatedBayesTree; + HybridGaussianFactorGraph::shared_ptr finalGraph; + if (discrete_ordering.size() > 0) { + HybridEliminationTree discrete_etree( + *discreteGraph, VariableIndex(*discreteGraph), discrete_ordering); + + This discrete_junction_tree(discrete_etree); + + std::tie(updatedBayesTree, finalGraph) = + discrete_junction_tree.Base::eliminate(function); + + // Get the clique with all the discrete keys. + // There should only be 1 clique. + const HybridBayesTree::sharedClique discrete_clique = + (*updatedBayesTree)[discrete_ordering.at(0)]; + + std::set clique_set; + for (auto node : continuousBayesTree->nodes()) { + clique_set.insert(node.second); + } + + // Set the root of the bayes tree as the discrete clique + for (auto clique : clique_set) { + if (clique->conditional()->parents() == + discrete_clique->conditional()->frontals()) { + updatedBayesTree->addClique(clique, discrete_clique); + + } else { + if (clique->parent()) { + // Remove the clique from the children of the parents since it will + // get added again in addClique. + auto clique_it = std::find(clique->parent()->children.begin(), + clique->parent()->children.end(), clique); + clique->parent()->children.erase(clique_it); + updatedBayesTree->addClique(clique, clique->parent()); + } else { + updatedBayesTree->addClique(clique); + } + } + } + } else { + updatedBayesTree = continuousBayesTree; + finalGraph = discreteGraph; + } + + return std::make_pair(updatedBayesTree, finalGraph); +} + +/* ************************************************************************* */ +boost::shared_ptr HybridJunctionTree::addProbPrimes( + const HybridGaussianFactorGraph& graph, + const HybridBayesTree::shared_ptr& continuousBayesTree, + const HybridGaussianFactorGraph::shared_ptr& discreteGraph, + const Ordering& continuous_ordering, + const Ordering& discrete_ordering) const { + // If we have eliminated continuous variables + // and have discrete variables to eliminate, + // then compute P(X | M, Z) + if (continuous_ordering.size() > 0 && discrete_ordering.size() > 0) { + // Collect all the discrete keys + DiscreteKeys discrete_keys; + for (auto node : continuousBayesTree->nodes()) { + auto node_dkeys = node.second->conditional()->discreteKeys(); + discrete_keys.insert(discrete_keys.end(), node_dkeys.begin(), + node_dkeys.end()); + } + // Remove duplicates and convert back to DiscreteKeys + std::set dkeys_set(discrete_keys.begin(), discrete_keys.end()); + discrete_keys = DiscreteKeys(dkeys_set.begin(), dkeys_set.end()); + + // DecisionTree for P'(X|M, Z) for all mode sequences M + const AlgebraicDecisionTree probPrimeTree = + graph.continuousProbPrimes(discrete_keys, continuousBayesTree); + + // Add the model selection factor P(M|Z) + discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); + } + + return discreteGraph; +} + +/* ************************************************************************* */ +std::pair +HybridJunctionTree::eliminate(const Eliminate& function) const { + Ordering continuous_ordering = etree_.continuousOrdering(); + Ordering discrete_ordering = etree_.discreteOrdering(); + + FactorGraphType graph = etree_.graph(); + + // Eliminate continuous + BayesTreeType::shared_ptr continuousBayesTree; + FactorGraphType::shared_ptr discreteGraph; + std::tie(continuousBayesTree, discreteGraph) = + this->eliminateContinuous(function, graph, continuous_ordering); + + FactorGraphType::shared_ptr updatedDiscreteGraph = + this->addProbPrimes(graph, continuousBayesTree, discreteGraph, + continuous_ordering, discrete_ordering); + + // Eliminate discrete variables to get the discrete bayes tree. + return this->eliminateDiscrete(function, continuousBayesTree, + updatedDiscreteGraph, discrete_ordering); +} + } // namespace gtsam diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h index 4b0c369a82..2dc13d5e38 100644 --- a/gtsam/hybrid/HybridJunctionTree.h +++ b/gtsam/hybrid/HybridJunctionTree.h @@ -51,10 +51,15 @@ class HybridEliminationTree; */ class GTSAM_EXPORT HybridJunctionTree : public JunctionTree { + /// Record the elimination tree for use in hybrid elimination. + HybridEliminationTree etree_; + /// Store the provided variable index. + VariableIndex variable_index_; + public: typedef JunctionTree Base; ///< Base class - typedef HybridJunctionTree This; ///< This class + typedef HybridJunctionTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class /** @@ -67,6 +72,66 @@ class GTSAM_EXPORT HybridJunctionTree * @return The elimination tree */ HybridJunctionTree(const HybridEliminationTree& eliminationTree); + + /** + * @brief + * + * @param function + * @param graph + * @param continuous_ordering + * @return std::pair, + * boost::shared_ptr> + */ + std::pair, + boost::shared_ptr> + eliminateContinuous(const Eliminate& function, + const HybridGaussianFactorGraph& graph, + const Ordering& continuous_ordering) const; + + /** + * @brief + * + * @param function + * @param continuousBayesTree + * @param discreteGraph + * @param discrete_ordering + * @return std::pair, + * boost::shared_ptr> + */ + std::pair, + boost::shared_ptr> + eliminateDiscrete(const Eliminate& function, + const HybridBayesTree::shared_ptr& continuousBayesTree, + const HybridGaussianFactorGraph::shared_ptr& discreteGraph, + const Ordering& discrete_ordering) const; + + /** + * @brief + * + * @param graph + * @param continuousBayesTree + * @param discreteGraph + * @param continuous_ordering + * @param discrete_ordering + * @return boost::shared_ptr + */ + boost::shared_ptr addProbPrimes( + const HybridGaussianFactorGraph& graph, + const HybridBayesTree::shared_ptr& continuousBayesTree, + const HybridGaussianFactorGraph::shared_ptr& discreteGraph, + const Ordering& continuous_ordering, + const Ordering& discrete_ordering) const; + + /** + * @brief + * + * @param function + * @return std::pair, + * boost::shared_ptr> + */ + std::pair, + boost::shared_ptr> + eliminate(const Eliminate& function) const; }; } // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 248d71d5fc..6288bcd93f 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -182,8 +182,9 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { boost::make_shared(X(1), I_3x3, Vector3::Ones())})); hfg.add(DecisionTreeFactor(m1, {2, 8})); - //TODO(Varun) Adding extra discrete variable not connected to continuous variable throws segfault - // hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); + // TODO(Varun) Adding extra discrete variable not connected to continuous + // variable throws segfault + // hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); HybridBayesTree::shared_ptr result = hfg.eliminateMultifrontal(hfg.getHybridOrdering()); @@ -276,7 +277,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full); // 9 cliques in the bayes tree and 0 remaining variables to eliminate. - EXPECT_LONGS_EQUAL(9, hbt->size()); + EXPECT_LONGS_EQUAL(7, hbt->size()); EXPECT_LONGS_EQUAL(0, remaining->size()); /* From ae0b3e3902d929bd47bec4579634c2825986d5ce Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 4 Dec 2022 18:21:22 +0530 Subject: [PATCH 14/21] split up the eliminate method to constituent parts --- gtsam/hybrid/HybridEliminationTree.cpp | 88 ++++++++++++++++++++++++ gtsam/hybrid/HybridEliminationTree.h | 95 ++++++-------------------- 2 files changed, 107 insertions(+), 76 deletions(-) diff --git a/gtsam/hybrid/HybridEliminationTree.cpp b/gtsam/hybrid/HybridEliminationTree.cpp index fe91905713..e541059197 100644 --- a/gtsam/hybrid/HybridEliminationTree.cpp +++ b/gtsam/hybrid/HybridEliminationTree.cpp @@ -47,4 +47,92 @@ bool HybridEliminationTree::equals(const This& other, double tol) const { return Base::equals(other, tol); } +/* ************************************************************************* */ +std::pair, + boost::shared_ptr> +HybridEliminationTree::eliminateContinuous(Eliminate function) const { + if (continuous_ordering_.size() > 0) { + This continuous_etree(graph_, variable_index_, continuous_ordering_); + return continuous_etree.Base::eliminate(function); + + } else { + HybridBayesNet::shared_ptr bayesNet = boost::make_shared(); + HybridGaussianFactorGraph::shared_ptr discreteGraph = + boost::make_shared(graph_); + return std::make_pair(bayesNet, discreteGraph); + } +} + +/* ************************************************************************* */ +boost::shared_ptr +HybridEliminationTree::addProbPrimes( + const HybridBayesNet::shared_ptr& continuousBayesNet, + const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const { + if (continuous_ordering_.size() > 0 && discrete_ordering_.size() > 0) { + // Get the last continuous conditional + // which will have all the discrete keys + HybridConditional::shared_ptr last_conditional = + continuousBayesNet->at(continuousBayesNet->size() - 1); + DiscreteKeys discrete_keys = last_conditional->discreteKeys(); + + // DecisionTree for P'(X|M, Z) for all mode sequences M + const AlgebraicDecisionTree probPrimeTree = + graph_.continuousProbPrimes(discrete_keys, continuousBayesNet); + + // Add the model selection factor P(M|Z) + discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); + } + return discreteGraph; +} + +/* ************************************************************************* */ +std::pair, + boost::shared_ptr> +HybridEliminationTree::eliminateDiscrete( + Eliminate function, + const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const { + HybridBayesNet::shared_ptr discreteBayesNet; + HybridGaussianFactorGraph::shared_ptr finalGraph; + if (discrete_ordering_.size() > 0) { + This discrete_etree(*discreteGraph, VariableIndex(*discreteGraph), + discrete_ordering_); + + std::tie(discreteBayesNet, finalGraph) = + discrete_etree.Base::eliminate(function); + + } else { + discreteBayesNet = boost::make_shared(); + finalGraph = discreteGraph; + } + + return std::make_pair(discreteBayesNet, finalGraph); +} + +/* ************************************************************************* */ +std::pair, + boost::shared_ptr> +HybridEliminationTree::eliminate(Eliminate function) const { + // Perform continuous elimination + HybridBayesNet::shared_ptr bayesNet; + HybridGaussianFactorGraph::shared_ptr discreteGraph; + std::tie(bayesNet, discreteGraph) = this->eliminateContinuous(function); + + // If we have eliminated continuous variables + // and have discrete variables to eliminate, + // then compute P(X | M, Z) + HybridGaussianFactorGraph::shared_ptr updatedDiscreteGraph = + addProbPrimes(bayesNet, discreteGraph); + + // Perform discrete elimination + HybridBayesNet::shared_ptr discreteBayesNet; + HybridGaussianFactorGraph::shared_ptr finalGraph; + std::tie(discreteBayesNet, finalGraph) = + eliminateDiscrete(function, updatedDiscreteGraph); + + // Add the discrete conditionals to the hybrid conditionals + bayesNet->add(*discreteBayesNet); + + return std::make_pair(bayesNet, finalGraph); +} + } // namespace gtsam diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index 65d614ca3c..9186e04a8a 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -80,22 +80,12 @@ class GTSAM_EXPORT HybridEliminationTree * and the original graph. * * @param function Elimination function for hybrid elimination. - * @return std::pair, - * boost::shared_ptr > + * @return std::pair, + * boost::shared_ptr > */ - std::pair, boost::shared_ptr> - eliminateContinuous(Eliminate function) const { - if (continuous_ordering_.size() > 0) { - This continuous_etree(graph_, variable_index_, continuous_ordering_); - return continuous_etree.Base::eliminate(function); - - } else { - BayesNetType::shared_ptr bayesNet = boost::make_shared(); - FactorGraphType::shared_ptr discreteGraph = - boost::make_shared(graph_); - return std::make_pair(bayesNet, discreteGraph); - } - } + std::pair, + boost::shared_ptr> + eliminateContinuous(Eliminate function) const; /** * @brief Helper method to eliminate the discrete variables after the @@ -104,75 +94,28 @@ class GTSAM_EXPORT HybridEliminationTree * If there are no discrete variables, return an empty bayes net and the * discreteGraph which is passed in. * - * @param function Elimination function + * @param function Hybrid elimination function * @param discreteGraph The factor graph with the factor ϕ(X | M, Z). - * @return std::pair, - * boost::shared_ptr > + * @return std::pair, + * boost::shared_ptr > */ - std::pair, boost::shared_ptr> - eliminateDiscrete(Eliminate function, - const FactorGraphType::shared_ptr& discreteGraph) const { - BayesNetType::shared_ptr discreteBayesNet; - FactorGraphType::shared_ptr finalGraph; - if (discrete_ordering_.size() > 0) { - This discrete_etree(*discreteGraph, VariableIndex(*discreteGraph), - discrete_ordering_); - - std::tie(discreteBayesNet, finalGraph) = - discrete_etree.Base::eliminate(function); - - } else { - discreteBayesNet = boost::make_shared(); - finalGraph = discreteGraph; - } - - return std::make_pair(discreteBayesNet, finalGraph); - } + std::pair, + boost::shared_ptr> + eliminateDiscrete( + Eliminate function, + const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const; /** * @brief Override the EliminationTree eliminate method * so we can perform hybrid elimination correctly. * - * @param function - * @return std::pair, - * boost::shared_ptr > + * @param function Hybrid elimination function + * @return std::pair, + * boost::shared_ptr > */ - std::pair, boost::shared_ptr> - eliminate(Eliminate function) const { - // Perform continuous elimination - BayesNetType::shared_ptr bayesNet; - FactorGraphType::shared_ptr discreteGraph; - std::tie(bayesNet, discreteGraph) = this->eliminateContinuous(function); - - // If we have eliminated continuous variables - // and have discrete variables to eliminate, - // then compute P(X | M, Z) - if (continuous_ordering_.size() > 0 && discrete_ordering_.size() > 0) { - // Get the last continuous conditional - // which will have all the discrete keys - HybridConditional::shared_ptr last_conditional = - bayesNet->at(bayesNet->size() - 1); - DiscreteKeys discrete_keys = last_conditional->discreteKeys(); - - // DecisionTree for P'(X|M, Z) for all mode sequences M - const AlgebraicDecisionTree probPrimeTree = - graph_.continuousProbPrimes(discrete_keys, bayesNet); - - // Add the model selection factor P(M|Z) - discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); - } - - // Perform discrete elimination - BayesNetType::shared_ptr discreteBayesNet; - FactorGraphType::shared_ptr finalGraph; - std::tie(discreteBayesNet, finalGraph) = - eliminateDiscrete(function, discreteGraph); - - // Add the discrete conditionals to the hybrid conditionals - bayesNet->add(*discreteBayesNet); - - return std::make_pair(bayesNet, finalGraph); - } + std::pair, + boost::shared_ptr> + eliminate(Eliminate function) const; Ordering continuousOrdering() const { return continuous_ordering_; } Ordering discreteOrdering() const { return discrete_ordering_; } From bed56e06932c8ab6b5875e6ae6b9026befdfe785 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 4 Dec 2022 18:21:57 +0530 Subject: [PATCH 15/21] mark helper methods as protected and add docstrings --- gtsam/hybrid/HybridEliminationTree.h | 18 ++++++++ gtsam/hybrid/HybridJunctionTree.cpp | 13 +++--- gtsam/hybrid/HybridJunctionTree.h | 63 +++++++++++++++------------- 3 files changed, 59 insertions(+), 35 deletions(-) diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index 9186e04a8a..9b68540266 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -73,6 +73,7 @@ class GTSAM_EXPORT HybridEliminationTree /** Test whether the tree is equal to another */ bool equals(const This& other, double tol = 1e-9) const; + protected: /** * @brief Helper method to eliminate continuous variables. * @@ -87,6 +88,22 @@ class GTSAM_EXPORT HybridEliminationTree boost::shared_ptr> eliminateContinuous(Eliminate function) const; + /** + * @brief Compute the unnormalized probability P'(X | M, Z) + * for the factor graph in each leaf of the discrete tree. + * The discrete decision tree formed as a result is added to the + * `discreteGraph` for discrete elimination. + * + * @param continuousBayesNet The bayes nets corresponding to + * the eliminated continuous variables. + * @param discreteGraph Factor graph consisting of factors + * on discrete variables only. + * @return boost::shared_ptr + */ + boost::shared_ptr addProbPrimes( + const HybridBayesNet::shared_ptr& continuousBayesNet, + const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const; + /** * @brief Helper method to eliminate the discrete variables after the * continuous variables have been eliminated. @@ -105,6 +122,7 @@ class GTSAM_EXPORT HybridEliminationTree Eliminate function, const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const; + public: /** * @brief Override the EliminationTree eliminate method * so we can perform hybrid elimination correctly. diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index f233d4bef9..43b043e304 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -252,11 +252,11 @@ HybridJunctionTree::eliminateDiscrete( /* ************************************************************************* */ boost::shared_ptr HybridJunctionTree::addProbPrimes( - const HybridGaussianFactorGraph& graph, const HybridBayesTree::shared_ptr& continuousBayesTree, - const HybridGaussianFactorGraph::shared_ptr& discreteGraph, - const Ordering& continuous_ordering, - const Ordering& discrete_ordering) const { + const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const { + Ordering continuous_ordering = etree_.continuousOrdering(); + Ordering discrete_ordering = etree_.discreteOrdering(); + // If we have eliminated continuous variables // and have discrete variables to eliminate, // then compute P(X | M, Z) @@ -272,6 +272,8 @@ boost::shared_ptr HybridJunctionTree::addProbPrimes( std::set dkeys_set(discrete_keys.begin(), discrete_keys.end()); discrete_keys = DiscreteKeys(dkeys_set.begin(), dkeys_set.end()); + FactorGraphType graph = etree_.graph(); + // DecisionTree for P'(X|M, Z) for all mode sequences M const AlgebraicDecisionTree probPrimeTree = graph.continuousProbPrimes(discrete_keys, continuousBayesTree); @@ -298,8 +300,7 @@ HybridJunctionTree::eliminate(const Eliminate& function) const { this->eliminateContinuous(function, graph, continuous_ordering); FactorGraphType::shared_ptr updatedDiscreteGraph = - this->addProbPrimes(graph, continuousBayesTree, discreteGraph, - continuous_ordering, discrete_ordering); + this->addProbPrimes(continuousBayesTree, discreteGraph); // Eliminate discrete variables to get the discrete bayes tree. return this->eliminateDiscrete(function, continuousBayesTree, diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h index 2dc13d5e38..d0473c33d8 100644 --- a/gtsam/hybrid/HybridJunctionTree.h +++ b/gtsam/hybrid/HybridJunctionTree.h @@ -73,14 +73,15 @@ class GTSAM_EXPORT HybridJunctionTree */ HybridJunctionTree(const HybridEliminationTree& eliminationTree); + protected: /** - * @brief - * - * @param function - * @param graph - * @param continuous_ordering + * @brief Eliminate all the continuous variables from the factor graph. + * + * @param function The hybrid elimination function. + * @param graph The factor graph to eliminate. + * @param continuous_ordering The ordering of continuous variables. * @return std::pair, - * boost::shared_ptr> + * boost::shared_ptr> */ std::pair, boost::shared_ptr> @@ -89,14 +90,17 @@ class GTSAM_EXPORT HybridJunctionTree const Ordering& continuous_ordering) const; /** - * @brief - * - * @param function - * @param continuousBayesTree - * @param discreteGraph - * @param discrete_ordering + * @brief Eliminate all the discrete variables in the hybrid factor graph. + * + * @param function The hybrid elimination function. + * @param continuousBayesTree The bayes tree corresponding to + * the eliminated continuous variables. + * @param discreteGraph Factor graph of factors containing + * only discrete variables. + * @param discrete_ordering The elimination ordering for + * the discrete variables. * @return std::pair, - * boost::shared_ptr> + * boost::shared_ptr> */ std::pair, boost::shared_ptr> @@ -106,28 +110,29 @@ class GTSAM_EXPORT HybridJunctionTree const Ordering& discrete_ordering) const; /** - * @brief - * - * @param graph - * @param continuousBayesTree - * @param discreteGraph - * @param continuous_ordering - * @param discrete_ordering - * @return boost::shared_ptr + * @brief Compute the unnormalized probability P'(X | M, Z) + * for the factor graph in each leaf of the discrete tree. + * The discrete decision tree formed as a result is added to the + * `discreteGraph` for discrete elimination. + * + * @param continuousBayesTree The bayes tree corresponding to + * the eliminated continuous variables. + * @param discreteGraph Factor graph consisting of factors + * on discrete variables only. + * @return boost::shared_ptr */ boost::shared_ptr addProbPrimes( - const HybridGaussianFactorGraph& graph, const HybridBayesTree::shared_ptr& continuousBayesTree, - const HybridGaussianFactorGraph::shared_ptr& discreteGraph, - const Ordering& continuous_ordering, - const Ordering& discrete_ordering) const; + const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const; + public: /** - * @brief - * - * @param function + * @brief Override the eliminate method so we can + * perform hybrid elimination correctly. + * + * @param function The hybrid elimination function. * @return std::pair, - * boost::shared_ptr> + * boost::shared_ptr> */ std::pair, boost::shared_ptr> From 5fc114fad27ba308e5819e6c59dd4402200dd9bb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 4 Dec 2022 18:24:16 +0530 Subject: [PATCH 16/21] more unit tests --- gtsam/hybrid/tests/testHybridEstimation.cpp | 25 +++++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 431e5769f5..85be0ab31c 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -70,6 +70,28 @@ Ordering getOrdering(HybridGaussianFactorGraph& factors, return ordering; } +TEST(HybridEstimation, Full) { + size_t K = 3; + std::vector measurements = {0, 1, 2}; + // Ground truth discrete seq + std::vector discrete_seq = {1, 1, 0}; + // Switching example of robot moving in 1D + // with given measurements and equal mode priors. + Switching switching(K, 1.0, 0.1, measurements, "1/1 1/1"); + HybridGaussianFactorGraph graph = switching.linearizedFactorGraph; + + Ordering hybridOrdering; + hybridOrdering += X(0); + hybridOrdering += X(1); + hybridOrdering += X(2); + hybridOrdering += M(0); + hybridOrdering += M(1); + HybridBayesNet::shared_ptr bayesNet = + graph.eliminateSequential(hybridOrdering); + + EXPECT_LONGS_EQUAL(5, bayesNet->size()); +} + /****************************************************************************/ // Test approximate inference with an additional pruning step. TEST(HybridEstimation, Incremental) { @@ -311,8 +333,7 @@ TEST(HybridEstimation, Probability) { discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); Ordering discrete(graph.discreteKeys()); - auto discreteBayesNet = - discreteGraph->BaseEliminateable::eliminateSequential(discrete); + auto discreteBayesNet = discreteGraph->eliminateSequential(); bayesNet->add(*discreteBayesNet); HybridValues hybrid_values = bayesNet->optimize(); From 22e4a733e0bf6dfa2b27947b679da27472753747 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 4 Dec 2022 18:36:36 +0530 Subject: [PATCH 17/21] Add details about the role of the HybridEliminationTree in hybrid elimination --- gtsam/hybrid/HybridEliminationTree.h | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index 9b68540266..4802c2f89f 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -24,7 +24,22 @@ namespace gtsam { /** - * Elimination Tree type for Hybrid + * Elimination Tree type for Hybrid Factor Graphs. + * + * The elimination tree helps in elimination by specifying the parents to which + * the "combined factor" after elimination should be assigned to. + * + * In the case of hybrid elimination, we use the elimination tree to store the + * intermediate results. + * Concretely, we wish to estimate the unnormalized probability P'(X | M, Z) for + * each mode assignment M. + * P'(X | M, Z) can be computed by estimating X* which maximizes the continuous + * factor graph given the discrete modes, and then computing the unnormalized + * probability as P(X=X* | M, Z). + * + * This is done by eliminating the (continuous) factor graph present at each + * leaf of the discrete tree formed for the discrete sequence and using the + * inferred X* to compute the `probPrime`. * * @ingroup hybrid */ From 0596b2f543a7327d4e89d2999c578883756956ae Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 10 Dec 2022 09:46:26 +0530 Subject: [PATCH 18/21] remove unrequired code --- gtsam/hybrid/HybridEliminationTree.cpp | 100 +----------------- gtsam/hybrid/HybridEliminationTree.h | 90 ---------------- gtsam/hybrid/HybridJunctionTree.cpp | 140 +------------------------ gtsam/hybrid/HybridJunctionTree.h | 69 ------------ 4 files changed, 3 insertions(+), 396 deletions(-) diff --git a/gtsam/hybrid/HybridEliminationTree.cpp b/gtsam/hybrid/HybridEliminationTree.cpp index e541059197..c2df2dd600 100644 --- a/gtsam/hybrid/HybridEliminationTree.cpp +++ b/gtsam/hybrid/HybridEliminationTree.cpp @@ -27,112 +27,16 @@ template class EliminationTree; HybridEliminationTree::HybridEliminationTree( const HybridGaussianFactorGraph& factorGraph, const VariableIndex& structure, const Ordering& order) - : Base(factorGraph, structure, order), - graph_(factorGraph), - variable_index_(structure) { - // Segregate the continuous and the discrete keys - std::tie(continuous_ordering_, discrete_ordering_) = - graph_.separateContinuousDiscreteOrdering(order); -} + : Base(factorGraph, structure, order) {} /* ************************************************************************* */ HybridEliminationTree::HybridEliminationTree( const HybridGaussianFactorGraph& factorGraph, const Ordering& order) - : Base(factorGraph, order), - graph_(factorGraph), - variable_index_(VariableIndex(factorGraph)) {} + : Base(factorGraph, order) {} /* ************************************************************************* */ bool HybridEliminationTree::equals(const This& other, double tol) const { return Base::equals(other, tol); } -/* ************************************************************************* */ -std::pair, - boost::shared_ptr> -HybridEliminationTree::eliminateContinuous(Eliminate function) const { - if (continuous_ordering_.size() > 0) { - This continuous_etree(graph_, variable_index_, continuous_ordering_); - return continuous_etree.Base::eliminate(function); - - } else { - HybridBayesNet::shared_ptr bayesNet = boost::make_shared(); - HybridGaussianFactorGraph::shared_ptr discreteGraph = - boost::make_shared(graph_); - return std::make_pair(bayesNet, discreteGraph); - } -} - -/* ************************************************************************* */ -boost::shared_ptr -HybridEliminationTree::addProbPrimes( - const HybridBayesNet::shared_ptr& continuousBayesNet, - const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const { - if (continuous_ordering_.size() > 0 && discrete_ordering_.size() > 0) { - // Get the last continuous conditional - // which will have all the discrete keys - HybridConditional::shared_ptr last_conditional = - continuousBayesNet->at(continuousBayesNet->size() - 1); - DiscreteKeys discrete_keys = last_conditional->discreteKeys(); - - // DecisionTree for P'(X|M, Z) for all mode sequences M - const AlgebraicDecisionTree probPrimeTree = - graph_.continuousProbPrimes(discrete_keys, continuousBayesNet); - - // Add the model selection factor P(M|Z) - discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); - } - return discreteGraph; -} - -/* ************************************************************************* */ -std::pair, - boost::shared_ptr> -HybridEliminationTree::eliminateDiscrete( - Eliminate function, - const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const { - HybridBayesNet::shared_ptr discreteBayesNet; - HybridGaussianFactorGraph::shared_ptr finalGraph; - if (discrete_ordering_.size() > 0) { - This discrete_etree(*discreteGraph, VariableIndex(*discreteGraph), - discrete_ordering_); - - std::tie(discreteBayesNet, finalGraph) = - discrete_etree.Base::eliminate(function); - - } else { - discreteBayesNet = boost::make_shared(); - finalGraph = discreteGraph; - } - - return std::make_pair(discreteBayesNet, finalGraph); -} - -/* ************************************************************************* */ -std::pair, - boost::shared_ptr> -HybridEliminationTree::eliminate(Eliminate function) const { - // Perform continuous elimination - HybridBayesNet::shared_ptr bayesNet; - HybridGaussianFactorGraph::shared_ptr discreteGraph; - std::tie(bayesNet, discreteGraph) = this->eliminateContinuous(function); - - // If we have eliminated continuous variables - // and have discrete variables to eliminate, - // then compute P(X | M, Z) - HybridGaussianFactorGraph::shared_ptr updatedDiscreteGraph = - addProbPrimes(bayesNet, discreteGraph); - - // Perform discrete elimination - HybridBayesNet::shared_ptr discreteBayesNet; - HybridGaussianFactorGraph::shared_ptr finalGraph; - std::tie(discreteBayesNet, finalGraph) = - eliminateDiscrete(function, updatedDiscreteGraph); - - // Add the discrete conditionals to the hybrid conditionals - bayesNet->add(*discreteBayesNet); - - return std::make_pair(bayesNet, finalGraph); -} - } // namespace gtsam diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index 4802c2f89f..941fefa5a5 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -26,21 +26,6 @@ namespace gtsam { /** * Elimination Tree type for Hybrid Factor Graphs. * - * The elimination tree helps in elimination by specifying the parents to which - * the "combined factor" after elimination should be assigned to. - * - * In the case of hybrid elimination, we use the elimination tree to store the - * intermediate results. - * Concretely, we wish to estimate the unnormalized probability P'(X | M, Z) for - * each mode assignment M. - * P'(X | M, Z) can be computed by estimating X* which maximizes the continuous - * factor graph given the discrete modes, and then computing the unnormalized - * probability as P(X=X* | M, Z). - * - * This is done by eliminating the (continuous) factor graph present at each - * leaf of the discrete tree formed for the discrete sequence and using the - * inferred X* to compute the `probPrime`. - * * @ingroup hybrid */ class GTSAM_EXPORT HybridEliminationTree @@ -48,12 +33,6 @@ class GTSAM_EXPORT HybridEliminationTree private: friend class ::EliminationTreeTester; - Ordering continuous_ordering_, discrete_ordering_; - /// Used to store the original factor graph to eliminate - HybridGaussianFactorGraph graph_; - /// Store the provided variable index. - VariableIndex variable_index_; - public: typedef EliminationTree Base; ///< Base class @@ -87,75 +66,6 @@ class GTSAM_EXPORT HybridEliminationTree /** Test whether the tree is equal to another */ bool equals(const This& other, double tol = 1e-9) const; - - protected: - /** - * @brief Helper method to eliminate continuous variables. - * - * If no continuous variables exist, return an empty bayes net - * and the original graph. - * - * @param function Elimination function for hybrid elimination. - * @return std::pair, - * boost::shared_ptr > - */ - std::pair, - boost::shared_ptr> - eliminateContinuous(Eliminate function) const; - - /** - * @brief Compute the unnormalized probability P'(X | M, Z) - * for the factor graph in each leaf of the discrete tree. - * The discrete decision tree formed as a result is added to the - * `discreteGraph` for discrete elimination. - * - * @param continuousBayesNet The bayes nets corresponding to - * the eliminated continuous variables. - * @param discreteGraph Factor graph consisting of factors - * on discrete variables only. - * @return boost::shared_ptr - */ - boost::shared_ptr addProbPrimes( - const HybridBayesNet::shared_ptr& continuousBayesNet, - const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const; - - /** - * @brief Helper method to eliminate the discrete variables after the - * continuous variables have been eliminated. - * - * If there are no discrete variables, return an empty bayes net and the - * discreteGraph which is passed in. - * - * @param function Hybrid elimination function - * @param discreteGraph The factor graph with the factor ϕ(X | M, Z). - * @return std::pair, - * boost::shared_ptr > - */ - std::pair, - boost::shared_ptr> - eliminateDiscrete( - Eliminate function, - const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const; - - public: - /** - * @brief Override the EliminationTree eliminate method - * so we can perform hybrid elimination correctly. - * - * @param function Hybrid elimination function - * @return std::pair, - * boost::shared_ptr > - */ - std::pair, - boost::shared_ptr> - eliminate(Eliminate function) const; - - Ordering continuousOrdering() const { return continuous_ordering_; } - Ordering discreteOrdering() const { return discrete_ordering_; } - - /// Store the provided variable index. - VariableIndex variableIndex() const { return variable_index_; } - HybridGaussianFactorGraph graph() const { return graph_; } }; } // namespace gtsam diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 43b043e304..422c200a43 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -142,8 +142,7 @@ struct HybridConstructorTraversalData { /* ************************************************************************* */ HybridJunctionTree::HybridJunctionTree( - const HybridEliminationTree& eliminationTree) - : etree_(eliminationTree) { + const HybridEliminationTree& eliminationTree) { gttic(JunctionTree_FromEliminationTree); // Here we rely on the BayesNet having been produced by this elimination tree, // such that the conditionals are arranged in DFS post-order. We traverse the @@ -170,141 +169,4 @@ HybridJunctionTree::HybridJunctionTree( Base::remainingFactors_ = eliminationTree.remainingFactors(); } -/* ************************************************************************* */ -std::pair, - boost::shared_ptr> -HybridJunctionTree::eliminateContinuous( - const Eliminate& function, const HybridGaussianFactorGraph& graph, - const Ordering& continuous_ordering) const { - HybridBayesTree::shared_ptr continuousBayesTree; - HybridGaussianFactorGraph::shared_ptr discreteGraph; - - if (continuous_ordering.size() > 0) { - HybridEliminationTree continuous_etree(graph, etree_.variableIndex(), - continuous_ordering); - - This continuous_junction_tree(continuous_etree); - std::tie(continuousBayesTree, discreteGraph) = - continuous_junction_tree.Base::eliminate(function); - - } else { - continuousBayesTree = boost::make_shared(); - discreteGraph = boost::make_shared(graph); - } - - return std::make_pair(continuousBayesTree, discreteGraph); -} -/* ************************************************************************* */ -std::pair, - boost::shared_ptr> -HybridJunctionTree::eliminateDiscrete( - const Eliminate& function, - const HybridBayesTree::shared_ptr& continuousBayesTree, - const HybridGaussianFactorGraph::shared_ptr& discreteGraph, - const Ordering& discrete_ordering) const { - HybridBayesTree::shared_ptr updatedBayesTree; - HybridGaussianFactorGraph::shared_ptr finalGraph; - if (discrete_ordering.size() > 0) { - HybridEliminationTree discrete_etree( - *discreteGraph, VariableIndex(*discreteGraph), discrete_ordering); - - This discrete_junction_tree(discrete_etree); - - std::tie(updatedBayesTree, finalGraph) = - discrete_junction_tree.Base::eliminate(function); - - // Get the clique with all the discrete keys. - // There should only be 1 clique. - const HybridBayesTree::sharedClique discrete_clique = - (*updatedBayesTree)[discrete_ordering.at(0)]; - - std::set clique_set; - for (auto node : continuousBayesTree->nodes()) { - clique_set.insert(node.second); - } - - // Set the root of the bayes tree as the discrete clique - for (auto clique : clique_set) { - if (clique->conditional()->parents() == - discrete_clique->conditional()->frontals()) { - updatedBayesTree->addClique(clique, discrete_clique); - - } else { - if (clique->parent()) { - // Remove the clique from the children of the parents since it will - // get added again in addClique. - auto clique_it = std::find(clique->parent()->children.begin(), - clique->parent()->children.end(), clique); - clique->parent()->children.erase(clique_it); - updatedBayesTree->addClique(clique, clique->parent()); - } else { - updatedBayesTree->addClique(clique); - } - } - } - } else { - updatedBayesTree = continuousBayesTree; - finalGraph = discreteGraph; - } - - return std::make_pair(updatedBayesTree, finalGraph); -} - -/* ************************************************************************* */ -boost::shared_ptr HybridJunctionTree::addProbPrimes( - const HybridBayesTree::shared_ptr& continuousBayesTree, - const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const { - Ordering continuous_ordering = etree_.continuousOrdering(); - Ordering discrete_ordering = etree_.discreteOrdering(); - - // If we have eliminated continuous variables - // and have discrete variables to eliminate, - // then compute P(X | M, Z) - if (continuous_ordering.size() > 0 && discrete_ordering.size() > 0) { - // Collect all the discrete keys - DiscreteKeys discrete_keys; - for (auto node : continuousBayesTree->nodes()) { - auto node_dkeys = node.second->conditional()->discreteKeys(); - discrete_keys.insert(discrete_keys.end(), node_dkeys.begin(), - node_dkeys.end()); - } - // Remove duplicates and convert back to DiscreteKeys - std::set dkeys_set(discrete_keys.begin(), discrete_keys.end()); - discrete_keys = DiscreteKeys(dkeys_set.begin(), dkeys_set.end()); - - FactorGraphType graph = etree_.graph(); - - // DecisionTree for P'(X|M, Z) for all mode sequences M - const AlgebraicDecisionTree probPrimeTree = - graph.continuousProbPrimes(discrete_keys, continuousBayesTree); - - // Add the model selection factor P(M|Z) - discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); - } - - return discreteGraph; -} - -/* ************************************************************************* */ -std::pair -HybridJunctionTree::eliminate(const Eliminate& function) const { - Ordering continuous_ordering = etree_.continuousOrdering(); - Ordering discrete_ordering = etree_.discreteOrdering(); - - FactorGraphType graph = etree_.graph(); - - // Eliminate continuous - BayesTreeType::shared_ptr continuousBayesTree; - FactorGraphType::shared_ptr discreteGraph; - std::tie(continuousBayesTree, discreteGraph) = - this->eliminateContinuous(function, graph, continuous_ordering); - - FactorGraphType::shared_ptr updatedDiscreteGraph = - this->addProbPrimes(continuousBayesTree, discreteGraph); - - // Eliminate discrete variables to get the discrete bayes tree. - return this->eliminateDiscrete(function, continuousBayesTree, - updatedDiscreteGraph, discrete_ordering); -} - } // namespace gtsam diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h index d0473c33d8..29fa24809e 100644 --- a/gtsam/hybrid/HybridJunctionTree.h +++ b/gtsam/hybrid/HybridJunctionTree.h @@ -51,10 +51,6 @@ class HybridEliminationTree; */ class GTSAM_EXPORT HybridJunctionTree : public JunctionTree { - /// Record the elimination tree for use in hybrid elimination. - HybridEliminationTree etree_; - /// Store the provided variable index. - VariableIndex variable_index_; public: typedef JunctionTree @@ -72,71 +68,6 @@ class GTSAM_EXPORT HybridJunctionTree * @return The elimination tree */ HybridJunctionTree(const HybridEliminationTree& eliminationTree); - - protected: - /** - * @brief Eliminate all the continuous variables from the factor graph. - * - * @param function The hybrid elimination function. - * @param graph The factor graph to eliminate. - * @param continuous_ordering The ordering of continuous variables. - * @return std::pair, - * boost::shared_ptr> - */ - std::pair, - boost::shared_ptr> - eliminateContinuous(const Eliminate& function, - const HybridGaussianFactorGraph& graph, - const Ordering& continuous_ordering) const; - - /** - * @brief Eliminate all the discrete variables in the hybrid factor graph. - * - * @param function The hybrid elimination function. - * @param continuousBayesTree The bayes tree corresponding to - * the eliminated continuous variables. - * @param discreteGraph Factor graph of factors containing - * only discrete variables. - * @param discrete_ordering The elimination ordering for - * the discrete variables. - * @return std::pair, - * boost::shared_ptr> - */ - std::pair, - boost::shared_ptr> - eliminateDiscrete(const Eliminate& function, - const HybridBayesTree::shared_ptr& continuousBayesTree, - const HybridGaussianFactorGraph::shared_ptr& discreteGraph, - const Ordering& discrete_ordering) const; - - /** - * @brief Compute the unnormalized probability P'(X | M, Z) - * for the factor graph in each leaf of the discrete tree. - * The discrete decision tree formed as a result is added to the - * `discreteGraph` for discrete elimination. - * - * @param continuousBayesTree The bayes tree corresponding to - * the eliminated continuous variables. - * @param discreteGraph Factor graph consisting of factors - * on discrete variables only. - * @return boost::shared_ptr - */ - boost::shared_ptr addProbPrimes( - const HybridBayesTree::shared_ptr& continuousBayesTree, - const HybridGaussianFactorGraph::shared_ptr& discreteGraph) const; - - public: - /** - * @brief Override the eliminate method so we can - * perform hybrid elimination correctly. - * - * @param function The hybrid elimination function. - * @return std::pair, - * boost::shared_ptr> - */ - std::pair, - boost::shared_ptr> - eliminate(const Eliminate& function) const; }; } // namespace gtsam From 62bc9f20a3b36fbcb1840b9d600d68239f549063 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 10 Dec 2022 10:35:46 +0530 Subject: [PATCH 19/21] update hybrid elimination and corresponding tests --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 12 +++-- gtsam/hybrid/HybridSmoother.cpp | 2 +- gtsam/hybrid/tests/testHybridEstimation.cpp | 49 +++++-------------- .../tests/testHybridGaussianFactorGraph.cpp | 2 +- gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 2 +- .../tests/testHybridNonlinearFactorGraph.cpp | 11 +---- .../hybrid/tests/testHybridNonlinearISAM.cpp | 2 +- 7 files changed, 25 insertions(+), 55 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index c430fac2c6..de237b0491 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -172,8 +172,13 @@ discreteElimination(const HybridGaussianFactorGraph &factors, } } + // std::cout << "Eliminate For MPE" << std::endl; auto result = EliminateForMPE(dfg, frontalKeys); - + // std::cout << "discrete elimination done!" << std::endl; + // dfg.print(); + // std::cout << "\n\n\n" << std::endl; + // result.first->print(); + // result.second->print(); return {boost::make_shared(result.first), boost::make_shared(result.second)}; } @@ -262,7 +267,9 @@ hybridElimination(const HybridGaussianFactorGraph &factors, if (!factor) { return 0.0; // If nullptr, return 0.0 probability } else { - return 1.0; + double error = + 0.5 * std::abs(factor->augmentedInformation().determinant()); + return std::exp(-error); } }; DecisionTree fdt(separatorFactors, factorProb); @@ -550,5 +557,4 @@ HybridGaussianFactorGraph::separateContinuousDiscreteOrdering( return std::make_pair(continuous_ordering, discrete_ordering); } - } // namespace gtsam diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index 7400053bfc..07a7a4e77a 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -32,7 +32,7 @@ void HybridSmoother::update(HybridGaussianFactorGraph graph, addConditionals(graph, hybridBayesNet_, ordering); // Eliminate. - auto bayesNetFragment = graph.eliminateSequential(); + auto bayesNetFragment = graph.eliminateSequential(ordering); /// Prune if (maxNrLeaves) { diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 85be0ab31c..39c5eea157 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -15,6 +15,7 @@ * @author Varun Agrawal */ +#include #include #include #include @@ -280,8 +281,10 @@ TEST(HybridEstimation, Probability) { VectorValues values = bayes_net->optimize(); - expected_errors.push_back(linear_graph->error(values)); - expected_prob_primes.push_back(linear_graph->probPrime(values)); + double error = linear_graph->error(values); + expected_errors.push_back(error); + double prob_prime = linear_graph->probPrime(values); + expected_prob_primes.push_back(prob_prime); } // Switching example of robot moving in 1D with given measurements and equal @@ -291,51 +294,21 @@ TEST(HybridEstimation, Probability) { auto graph = switching.linearizedFactorGraph; Ordering ordering = getOrdering(graph, HybridGaussianFactorGraph()); - AlgebraicDecisionTree expected_probPrimeTree = probPrimeTree(graph); - - // Eliminate continuous - Ordering continuous_ordering(graph.continuousKeys()); - HybridBayesNet::shared_ptr bayesNet; - HybridGaussianFactorGraph::shared_ptr discreteGraph; - std::tie(bayesNet, discreteGraph) = - graph.eliminatePartialSequential(continuous_ordering); - - // Get the last continuous conditional which will have all the discrete keys - auto last_conditional = bayesNet->at(bayesNet->size() - 1); - DiscreteKeys discrete_keys = last_conditional->discreteKeys(); - - const std::vector assignments = - DiscreteValues::CartesianProduct(discrete_keys); - - // Reverse discrete keys order for correct tree construction - std::reverse(discrete_keys.begin(), discrete_keys.end()); - - // Create a decision tree of all the different VectorValues - DecisionTree delta_tree = - graph.continuousDelta(discrete_keys, bayesNet, assignments); - - AlgebraicDecisionTree probPrimeTree = - graph.continuousProbPrimes(discrete_keys, bayesNet); - - EXPECT(assert_equal(expected_probPrimeTree, probPrimeTree)); + HybridBayesNet::shared_ptr bayesNet = graph.eliminateSequential(ordering); + auto discreteConditional = bayesNet->atDiscrete(bayesNet->size() - 3); // Test if the probPrimeTree matches the probability of // the individual factor graphs for (size_t i = 0; i < pow(2, K - 1); i++) { - Assignment discrete_assignment; + DiscreteValues discrete_assignment; for (size_t v = 0; v < discrete_seq_map[i].size(); v++) { discrete_assignment[M(v)] = discrete_seq_map[i][v]; } - EXPECT_DOUBLES_EQUAL(expected_prob_primes.at(i), - probPrimeTree(discrete_assignment), 1e-8); + double discrete_transition_prob = 0.25; + EXPECT_DOUBLES_EQUAL(expected_prob_primes.at(i) * discrete_transition_prob, + (*discreteConditional)(discrete_assignment), 1e-8); } - discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); - - Ordering discrete(graph.discreteKeys()); - auto discreteBayesNet = discreteGraph->eliminateSequential(); - bayesNet->add(*discreteBayesNet); - HybridValues hybrid_values = bayesNet->optimize(); // This is the correct sequence as designed diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 6288bcd93f..8954f2503c 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -277,7 +277,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full); // 9 cliques in the bayes tree and 0 remaining variables to eliminate. - EXPECT_LONGS_EQUAL(7, hbt->size()); + EXPECT_LONGS_EQUAL(9, hbt->size()); EXPECT_LONGS_EQUAL(0, remaining->size()); /* diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 4ba6f88a50..8e79021327 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -178,7 +178,7 @@ TEST(HybridGaussianElimination, IncrementalInference) { // Test the probability values with regression tests. DiscreteValues assignment; - EXPECT(assert_equal(0.166667, m00_prob, 1e-5)); + EXPECT(assert_equal(0.0619233, m00_prob, 1e-5)); assignment[M(0)] = 0; assignment[M(1)] = 0; EXPECT(assert_equal(0.0619233, (*discreteConditional)(assignment), 1e-5)); diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index e3b7f761a7..a0a6f7d955 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -372,8 +372,7 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { dynamic_pointer_cast(hybridDiscreteFactor->inner()); CHECK(discreteFactor); EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size()); - // All leaves should be probability 1 since this is not P*(X|M,Z) - EXPECT(discreteFactor->root_->isLeaf()); + EXPECT(discreteFactor->root_->isLeaf() == false); // TODO(Varun) Test emplace_discrete } @@ -441,14 +440,6 @@ TEST(HybridFactorGraph, Full_Elimination) { discrete_fg.push_back(df->inner()); } - // Get the probabilit P*(X | M, Z) - DiscreteKeys discrete_keys = - remainingFactorGraph_partial->at(2)->discreteKeys(); - AlgebraicDecisionTree probPrimeTree = - linearizedFactorGraph.continuousProbPrimes(discrete_keys, - hybridBayesNet_partial); - discrete_fg.add(DecisionTreeFactor(discrete_keys, probPrimeTree)); - ordering.clear(); for (size_t k = 0; k < self.K - 1; k++) ordering += M(k); discreteBayesNet = diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 46d5c43245..2a1932ac76 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -197,7 +197,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) { // Test the probability values with regression tests. DiscreteValues assignment; - EXPECT(assert_equal(0.166667, m00_prob, 1e-5)); + EXPECT(assert_equal(0.0619233, m00_prob, 1e-5)); assignment[M(0)] = 0; assignment[M(1)] = 0; EXPECT(assert_equal(0.0619233, (*discreteConditional)(assignment), 1e-5)); From 6beffeb0c1d0eb4d098aef7ac88d198e9b3bd9c6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 10 Dec 2022 12:46:48 +0530 Subject: [PATCH 20/21] remove commented out code --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index de237b0491..5c18a94b55 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -172,13 +172,8 @@ discreteElimination(const HybridGaussianFactorGraph &factors, } } - // std::cout << "Eliminate For MPE" << std::endl; auto result = EliminateForMPE(dfg, frontalKeys); - // std::cout << "discrete elimination done!" << std::endl; - // dfg.print(); - // std::cout << "\n\n\n" << std::endl; - // result.first->print(); - // result.second->print(); + return {boost::make_shared(result.first), boost::make_shared(result.second)}; } From 812bf52c11ed39597ceb4fa21e2ae1fab20b2938 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Dec 2022 15:08:24 +0530 Subject: [PATCH 21/21] minor cleanup --- gtsam/hybrid/HybridBayesTree.cpp | 4 +--- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 3 +-- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index c9c6afa794..b706fb7456 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -14,7 +14,7 @@ * @brief Hybrid Bayes Tree, the result of eliminating a * HybridJunctionTree * @date Mar 11, 2022 - * @author Fan Jiang + * @author Fan Jiang, Varun Agrawal */ #include @@ -166,8 +166,6 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) { DecisionTreeFactor prunedDecisionTree = decisionTree->prune(maxNrLeaves); decisionTree->root_ = prunedDecisionTree.root_; - // this->print(); - // decisionTree->print("", DefaultKeyFormatter); /// Helper struct for pruning the hybrid bayes tree. struct HybridPrunerData { diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 5c18a94b55..c8707a586a 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -92,7 +92,6 @@ GaussianMixtureFactor::Sum sumFrontals( if (auto cgmf = boost::dynamic_pointer_cast(f)) { sum = cgmf->add(sum); } - if (auto gm = boost::dynamic_pointer_cast(f)) { sum = gm->asMixture()->add(sum); } @@ -189,7 +188,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, DiscreteKeys discreteSeparator(discreteSeparatorSet.begin(), discreteSeparatorSet.end()); - // sum out frontals, this is the factor on the separator + // sum out frontals, this is the factor 𝜏 on the separator GaussianMixtureFactor::Sum sum = sumFrontals(factors); // If a tree leaf contains nullptr,