From 76e838b8d05349cdbae42cc40fdc1878c4c5743c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 25 Dec 2022 18:19:02 -0500 Subject: [PATCH 1/5] Implement printing rather than calling factor graph version --- gtsam/inference/BayesNet-inst.h | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/gtsam/inference/BayesNet-inst.h b/gtsam/inference/BayesNet-inst.h index e792b5c03..f43b4025e 100644 --- a/gtsam/inference/BayesNet-inst.h +++ b/gtsam/inference/BayesNet-inst.h @@ -31,7 +31,14 @@ namespace gtsam { template void BayesNet::print(const std::string& s, const KeyFormatter& formatter) const { - Base::print(s, formatter); + std::cout << (s.empty() ? "" : s + " ") << std::endl; + std::cout << "size: " << this->size() << std::endl; + for (size_t i = 0; i < this->size(); i++) { + const auto& conditional = this->at(i); + std::stringstream ss; + ss << "conditional " << i << ": "; + if (conditional) conditional->print(ss.str(), formatter); + } } /* ************************************************************************* */ From 4ad482aa70913719b71a706a36de2579946763c3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 25 Dec 2022 19:20:55 -0500 Subject: [PATCH 2/5] Small comments --- gtsam/hybrid/HybridBayesNet.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index f5c11c6e1..2cb60475c 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -99,6 +99,7 @@ std::function &, double)> prunerFunc( } /* ************************************************************************* */ +// TODO(dellaert): what is this non-const method used for? Abolish it? void HybridBayesNet::updateDiscreteConditionals( const DecisionTreeFactor::shared_ptr &prunedDecisionTree) { KeyVector prunedTreeKeys = prunedDecisionTree->keys(); @@ -150,9 +151,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) { // Go through all the conditionals in the // Bayes Net and prune them as per decisionTree. - for (size_t i = 0; i < this->size(); i++) { - HybridConditional::shared_ptr conditional = this->at(i); - + for (auto &&conditional : *this) { if (conditional->isHybrid()) { GaussianMixture::shared_ptr gaussianMixture = conditional->asMixture(); From a7573e8e6f9f75aa1f49e2d7cfed18282a30fe67 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 25 Dec 2022 19:21:34 -0500 Subject: [PATCH 3/5] Refactor elimination setup to not use C declaration style --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 63 +++++++++++----------- 1 file changed, 33 insertions(+), 30 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 39c3c2965..a2777bfc0 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -344,18 +344,20 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, // However this is also the case with iSAM2, so no pressure :) // PREPROCESS: Identify the nature of the current elimination - std::unordered_map mapFromKeyToDiscreteKey; - std::set discreteSeparatorSet; - std::set discreteFrontals; + // First, identify the separator keys, i.e. all keys that are not frontal. KeySet separatorKeys; - KeySet allContinuousKeys; - KeySet continuousFrontals; - KeySet continuousSeparator; - - // This initializes separatorKeys and mapFromKeyToDiscreteKey for (auto &&factor : factors) { separatorKeys.insert(factor->begin(), factor->end()); + } + // remove frontals from separator + for (auto &k : frontalKeys) { + separatorKeys.erase(k); + } + + // Build a map from keys to DiscreteKeys + std::unordered_map mapFromKeyToDiscreteKey; + for (auto &&factor : factors) { if (!factor->isContinuous()) { for (auto &k : factor->discreteKeys()) { mapFromKeyToDiscreteKey[k.first] = k; @@ -363,49 +365,50 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, } } - // remove frontals from separator - for (auto &k : frontalKeys) { - separatorKeys.erase(k); - } - - // Fill in discrete frontals and continuous frontals for the end result + // Fill in discrete frontals and continuous frontals. + std::set discreteFrontals; + KeySet continuousFrontals; for (auto &k : frontalKeys) { if (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) { discreteFrontals.insert(mapFromKeyToDiscreteKey.at(k)); } else { continuousFrontals.insert(k); - allContinuousKeys.insert(k); } } - // Fill in discrete frontals and continuous frontals for the end result + // Fill in discrete discrete separator keys and continuous separator keys. + std::set discreteSeparatorSet; + KeySet continuousSeparator; for (auto &k : separatorKeys) { if (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) { discreteSeparatorSet.insert(mapFromKeyToDiscreteKey.at(k)); } else { continuousSeparator.insert(k); - allContinuousKeys.insert(k); } } + // Check if we have any continuous keys: + const bool discrete_only = + continuousFrontals.empty() && continuousSeparator.empty(); + // NOTE: We should really defer the product here because of pruning - // Case 1: we are only dealing with continuous - if (mapFromKeyToDiscreteKey.empty() && !allContinuousKeys.empty()) { - return continuousElimination(factors, frontalKeys); - } - - // Case 2: we are only dealing with discrete - if (allContinuousKeys.empty()) { + if (discrete_only) { + // Case 1: we are only dealing with discrete return discreteElimination(factors, frontalKeys); - } - + } else { + // Case 2: we are only dealing with continuous + if (mapFromKeyToDiscreteKey.empty()) { + return continuousElimination(factors, frontalKeys); + } else { + // Case 3: We are now in the hybrid land! #ifdef HYBRID_TIMING - tictoc_reset_(); + tictoc_reset_(); #endif - // Case 3: We are now in the hybrid land! - return hybridElimination(factors, frontalKeys, continuousSeparator, - discreteSeparatorSet); + return hybridElimination(factors, frontalKeys, continuousSeparator, + discreteSeparatorSet); + } + } } /* ************************************************************************ */ From 8a319c5a49e4df071eaa4ae76491e4996d0a0f02 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 25 Dec 2022 19:31:54 -0500 Subject: [PATCH 4/5] Separated out NFG setup and added test. --- gtsam/hybrid/tests/testHybridEstimation.cpp | 87 +++++++++++++++------ 1 file changed, 61 insertions(+), 26 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index e4a45bf4d..9b3d192ee 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -432,8 +432,65 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { EXPECT(assert_equal(discrete_seq, hybrid_values.discrete())); } -/****************************************************************************/ -/** +/********************************************************************************* + // Create a hybrid nonlinear factor graph f(x0, x1, m0; z0, z1) + ********************************************************************************/ +static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() { + HybridNonlinearFactorGraph nfg; + + constexpr double sigma = 0.5; // measurement noise + const auto noise_model = noiseModel::Isotropic::Sigma(1, sigma); + + // Add "measurement" factors: + nfg.emplace_nonlinear>(X(0), 0.0, noise_model); + nfg.emplace_nonlinear>(X(1), 1.0, noise_model); + + // Add mixture factor: + DiscreteKey m(M(0), 2); + const auto zero_motion = + boost::make_shared>(X(0), X(1), 0, noise_model); + const auto one_motion = + boost::make_shared>(X(0), X(1), 1, noise_model); + nfg.emplace_hybrid( + KeyVector{X(0), X(1)}, DiscreteKeys{m}, + std::vector{zero_motion, one_motion}); + + return nfg; +} + +/********************************************************************************* + // Create a hybrid nonlinear factor graph f(x0, x1, m0; z0, z1) + ********************************************************************************/ +static HybridGaussianFactorGraph::shared_ptr createHybridGaussianFactorGraph() { + HybridNonlinearFactorGraph nfg = createHybridNonlinearFactorGraph(); + + Values initial; + double z0 = 0.0, z1 = 1.0; + initial.insert(X(0), z0); + initial.insert(X(1), z1); + return nfg.linearize(initial); +} + +/********************************************************************************* + * Do hybrid elimination and do regression test on discrete conditional. + ********************************************************************************/ +TEST(HybridEstimation, eliminateSequentialRegression) { + // 1. Create the factor graph from the nonlinear factor graph. + HybridGaussianFactorGraph::shared_ptr fg = createHybridGaussianFactorGraph(); + + // 2. Eliminate into BN + const Ordering ordering = fg->getHybridOrdering(); + HybridBayesNet::shared_ptr bn = fg->eliminateSequential(ordering); + // GTSAM_PRINT(*bn); + + // TODO(dellaert): dc should be discrete conditional on m0, but it is an unnormalized factor? + // DiscreteKey m(M(0), 2); + // DiscreteConditional expected(m % "0.51341712/1"); + // auto dc = bn->back()->asDiscreteConditional(); + // EXPECT(assert_equal(expected, *dc, 1e-9)); +} + +/********************************************************************************* * Test for correctness via sampling. * * Compute the conditional P(x0, m0, x1| z0, z1) @@ -442,32 +499,10 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { * 2. Eliminate the factor graph into a Bayes Net `BN`. * 3. Sample from the Bayes Net. * 4. Check that the ratio `BN(x)/FG(x) = constant` for all samples `x`. - */ + ********************************************************************************/ TEST(HybridEstimation, CorrectnessViaSampling) { - HybridNonlinearFactorGraph nfg; - - // First we create a hybrid nonlinear factor graph - // which represents f(x0, x1, m0; z0, z1). - // We linearize and eliminate this to get - // the required Factor Graph FG and Bayes Net BN. - const auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0); - const auto zero_motion = - boost::make_shared>(X(0), X(1), 0, noise_model); - const auto one_motion = - boost::make_shared>(X(0), X(1), 1, noise_model); - - nfg.emplace_nonlinear>(X(0), 0.0, noise_model); - nfg.emplace_hybrid( - KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, - std::vector{zero_motion, one_motion}); - - Values initial; - double z0 = 0.0, z1 = 1.0; - initial.insert(X(0), z0); - initial.insert(X(1), z1); - // 1. Create the factor graph from the nonlinear factor graph. - HybridGaussianFactorGraph::shared_ptr fg = nfg.linearize(initial); + HybridGaussianFactorGraph::shared_ptr fg = createHybridGaussianFactorGraph(); // 2. Eliminate into BN const Ordering ordering = fg->getHybridOrdering(); From db17a04c59d041cf86e1ad40d6c65823f50fdeac Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 25 Dec 2022 22:30:12 -0500 Subject: [PATCH 5/5] Fix print test --- gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index a0a6f7d95..a4de4a1ae 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -587,7 +587,7 @@ factor 6: Discrete [m1 m0] // Expected output for hybridBayesNet. string expected_hybridBayesNet = R"( size: 3 -factor 0: Hybrid P( x0 | x1 m0) +conditional 0: Hybrid P( x0 | x1 m0) Discrete Keys = (m0, 2), Choice(m0) 0 Leaf p(x0 | x1) @@ -602,7 +602,7 @@ factor 0: Hybrid P( x0 | x1 m0) d = [ -9.95037 ] No noise model -factor 1: Hybrid P( x1 | x2 m0 m1) +conditional 1: Hybrid P( x1 | x2 m0 m1) Discrete Keys = (m0, 2), (m1, 2), Choice(m1) 0 Choice(m0) @@ -631,7 +631,7 @@ factor 1: Hybrid P( x1 | x2 m0 m1) d = [ -10 ] No noise model -factor 2: Hybrid P( x2 | m0 m1) +conditional 2: Hybrid P( x2 | m0 m1) Discrete Keys = (m0, 2), (m1, 2), Choice(m1) 0 Choice(m0)