From f3e84004bfa94f57024d35c784693902d009d180 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jan 2024 14:59:47 -0500 Subject: [PATCH 01/20] helper functions for computing leaf error and normalization constants used for model selection --- gtsam/hybrid/HybridBayesNet.cpp | 85 ++++++++++++++++++++------------- gtsam/hybrid/HybridBayesNet.h | 37 +++++++++++++- 2 files changed, 87 insertions(+), 35 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 23ee722150..eeacc929b8 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -227,24 +227,6 @@ GaussianBayesNet HybridBayesNet::choose( return gbn; } -/* ************************************************************************ */ -static GaussianBayesNetTree addGaussian( - const GaussianBayesNetTree &gfgTree, - const GaussianConditional::shared_ptr &factor) { - // If the decision tree is not initialized, then initialize it. - if (gfgTree.empty()) { - GaussianBayesNet result{factor}; - return GaussianBayesNetTree(result); - } else { - auto add = [&factor](const GaussianBayesNet &graph) { - auto result = graph; - result.push_back(factor); - return result; - }; - return gfgTree.apply(add); - } -} - /* ************************************************************************ */ GaussianBayesNetValTree HybridBayesNet::assembleTree() const { GaussianBayesNetTree result; @@ -320,25 +302,12 @@ AlgebraicDecisionTree HybridBayesNet::modelSelection() const { AlgebraicDecisionTree errorTree = trees.second; // Only compute logNormalizationConstant - AlgebraicDecisionTree log_norm_constants = DecisionTree( - bnTree, [](const std::pair &gbnAndValue) { - GaussianBayesNet gbn = gbnAndValue.first; - if (gbn.size() == 0) { - return 0.0; - } - return gbn.logNormalizationConstant(); - }); + AlgebraicDecisionTree log_norm_constants = + computeLogNormConstants(bnTree); // Compute model selection term (with help from ADT methods) AlgebraicDecisionTree modelSelectionTerm = - (errorTree + log_norm_constants) * -1; - - double max_log = modelSelectionTerm.max(); - modelSelectionTerm = DecisionTree( - modelSelectionTerm, - [&max_log](const double &x) { return std::exp(x - max_log); }); - modelSelectionTerm = modelSelectionTerm.normalize(modelSelectionTerm.sum()); - + computeModelSelectionTerm(errorTree, log_norm_constants); return modelSelectionTerm; } @@ -530,4 +499,52 @@ HybridGaussianFactorGraph HybridBayesNet::toFactorGraph( return fg; } +/* ************************************************************************ */ +GaussianBayesNetTree addGaussian( + const GaussianBayesNetTree &gbnTree, + const GaussianConditional::shared_ptr &factor) { + // If the decision tree is not initialized, then initialize it. + if (gbnTree.empty()) { + GaussianBayesNet result{factor}; + return GaussianBayesNetTree(result); + } else { + auto add = [&factor](const GaussianBayesNet &graph) { + auto result = graph; + result.push_back(factor); + return result; + }; + return gbnTree.apply(add); + } +} + +/* ************************************************************************* */ +AlgebraicDecisionTree computeLogNormConstants( + const GaussianBayesNetValTree &bnTree) { + AlgebraicDecisionTree log_norm_constants = DecisionTree( + bnTree, [](const std::pair &gbnAndValue) { + GaussianBayesNet gbn = gbnAndValue.first; + if (gbn.size() == 0) { + return 0.0; + } + return gbn.logNormalizationConstant(); + }); + return log_norm_constants; +} + +/* ************************************************************************* */ +AlgebraicDecisionTree computeModelSelectionTerm( + const AlgebraicDecisionTree &errorTree, + const AlgebraicDecisionTree &log_norm_constants) { + AlgebraicDecisionTree modelSelectionTerm = + (errorTree + log_norm_constants) * -1; + + double max_log = modelSelectionTerm.max(); + modelSelectionTerm = DecisionTree( + modelSelectionTerm, + [&max_log](const double &x) { return std::exp(x - max_log); }); + modelSelectionTerm = modelSelectionTerm.normalize(modelSelectionTerm.sum()); + + return modelSelectionTerm; +} + } // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index fe9010b1f2..d4fd3db718 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -141,7 +141,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { This can be computed by multiplying all the exponentiated errors of each of the conditionals. - + Return a tree where each leaf value is L(M_i;Z). */ AlgebraicDecisionTree modelSelection() const; @@ -280,4 +280,39 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { template <> struct traits : public Testable {}; +/** + * @brief Add a Gaussian conditional to each node of the GaussianBayesNetTree + * + * @param gbnTree + * @param factor + * @return GaussianBayesNetTree + */ +GaussianBayesNetTree addGaussian(const GaussianBayesNetTree &gbnTree, + const GaussianConditional::shared_ptr &factor); + +/** + * @brief Compute the (logarithmic) normalization constant for each Bayes + * network in the tree. + * + * @param bnTree A tree of Bayes networks in each leaf. The tree encodes a + * discrete assignment yielding the Bayes net. + * @return AlgebraicDecisionTree + */ +AlgebraicDecisionTree computeLogNormConstants( + const GaussianBayesNetValTree &bnTree); + +/** + * @brief Compute the model selection term L(M; Z, X) given the error + * and log normalization constants. + * + * Perform normalization to handle underflow issues. + * + * @param errorTree + * @param log_norm_constants + * @return AlgebraicDecisionTree + */ +AlgebraicDecisionTree computeModelSelectionTerm( + const AlgebraicDecisionTree &errorTree, + const AlgebraicDecisionTree &log_norm_constants); + } // namespace gtsam From e7cb7b2dcdc730ffd5a7309e69f03121ac41339b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jan 2024 15:00:35 -0500 Subject: [PATCH 02/20] add constructor for HybridFactorGraph which takes a container of factors --- gtsam/hybrid/HybridFactorGraph.h | 6 +++++- gtsam/hybrid/HybridGaussianFactorGraph.h | 5 +++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index 8b59fd4f93..8881a1d358 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -38,7 +38,7 @@ using SharedFactor = std::shared_ptr; class GTSAM_EXPORT HybridFactorGraph : public FactorGraph { public: using Base = FactorGraph; - using This = HybridFactorGraph; ///< this class + using This = HybridFactorGraph; ///< this class using shared_ptr = std::shared_ptr; ///< shared_ptr to This using Values = gtsam::Values; ///< backwards compatibility @@ -59,6 +59,10 @@ class GTSAM_EXPORT HybridFactorGraph : public FactorGraph { template HybridFactorGraph(const FactorGraph& graph) : Base(graph) {} + /** Construct from container of factors (shared_ptr or plain objects) */ + template + explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {} + /// @} /// @name Extra methods to inspect discrete/continuous keys. /// @{ diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 1708ff64be..8a86a7335c 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -126,6 +126,11 @@ class GTSAM_EXPORT HybridGaussianFactorGraph /// @brief Default constructor. HybridGaussianFactorGraph() = default; + /** Construct from container of factors (shared_ptr or plain objects) */ + template + explicit HybridGaussianFactorGraph(const CONTAINER& factors) + : Base(factors) {} + /** * Implicit copy/downcast constructor to override explicit template container * constructor. In BayesTree this is used for: From 4b2a22eaa564e4593cc1c28f12629f35e218598e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jan 2024 15:01:31 -0500 Subject: [PATCH 03/20] model selection for HybridBayesTree --- gtsam/hybrid/HybridBayesTree.cpp | 105 +++++++++++++++++++++++++++++-- gtsam/hybrid/HybridBayesTree.h | 45 +++++++++++++ 2 files changed, 146 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index ae8fa03781..cdd30d3980 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -38,19 +38,116 @@ bool HybridBayesTree::equals(const This& other, double tol) const { return Base::equals(other, tol); } +GaussianBayesNetTree& HybridBayesTree::addCliqueToTree( + const sharedClique& clique, GaussianBayesNetTree& result) const { + // Perform bottom-up inclusion + for (sharedClique child : clique->children) { + result = addCliqueToTree(child, result); + } + + auto f = clique->conditional(); + + if (auto hc = std::dynamic_pointer_cast(f)) { + if (auto gm = hc->asMixture()) { + result = gm->add(result); + } else if (auto g = hc->asGaussian()) { + result = addGaussian(result, g); + } else { + // Has to be discrete, which we don't add. + } + } + return result; +} + +/* ************************************************************************ */ +GaussianBayesNetValTree HybridBayesTree::assembleTree() const { + GaussianBayesNetTree result; + for (auto&& root : roots_) { + result = addCliqueToTree(root, result); + } + + GaussianBayesNetValTree resultTree(result, [](const GaussianBayesNet& gbn) { + return std::make_pair(gbn, 0.0); + }); + return resultTree; +} + +/* ************************************************************************* */ +AlgebraicDecisionTree HybridBayesTree::modelSelection() const { + /* + To perform model selection, we need: + q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) + + If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) + thus, q * sqrt((2*pi)^n*det(Sigma)) = q/k = exp(log(q/k)) + = exp(log(q) - log(k)) = exp(-error - log(k)) + = exp(-(error + log(k))), + where error is computed at the corresponding MAP point, gbt.error(mu). + + So we compute (error + log(k)) and exponentiate later + */ + + GaussianBayesNetValTree bnTree = assembleTree(); + + GaussianBayesNetValTree bn_error = bnTree.apply( + [this](const Assignment& assignment, + const std::pair& gbnAndValue) { + // Compute the X* of each assignment + VectorValues mu = gbnAndValue.first.optimize(); + + // mu is empty if gbn had nullptrs + if (mu.size() == 0) { + return std::make_pair(gbnAndValue.first, + std::numeric_limits::max()); + } + + // Compute the error for X* and the assignment + double error = + this->error(HybridValues(mu, DiscreteValues(assignment))); + + return std::make_pair(gbnAndValue.first, error); + }); + + auto trees = unzip(bn_error); + AlgebraicDecisionTree errorTree = trees.second; + + // Only compute logNormalizationConstant + AlgebraicDecisionTree log_norm_constants = + computeLogNormConstants(bnTree); + + // Compute model selection term (with help from ADT methods) + AlgebraicDecisionTree modelSelectionTerm = + computeModelSelectionTerm(errorTree, log_norm_constants); + + return modelSelectionTerm; +} + /* ************************************************************************* */ HybridValues HybridBayesTree::optimize() const { - DiscreteBayesNet dbn; + DiscreteFactorGraph discrete_fg; DiscreteValues mpe; + // Compute model selection term + AlgebraicDecisionTree modelSelectionTerm = modelSelection(); + auto root = roots_.at(0); // Access the clique and get the underlying hybrid conditional HybridConditional::shared_ptr root_conditional = root->conditional(); - // The root should be discrete only, we compute the MPE + // Get the set of all discrete keys involved in model selection + std::set discreteKeySet; + + // The root should be discrete only, we compute the MPE if (root_conditional->isDiscrete()) { - dbn.push_back(root_conditional->asDiscrete()); - mpe = DiscreteFactorGraph(dbn).optimize(); + discrete_fg.push_back(root_conditional->asDiscrete()); + + // Only add model_selection if we have discrete keys + if (discreteKeySet.size() > 0) { + discrete_fg.push_back(DecisionTreeFactor( + DiscreteKeys(discreteKeySet.begin(), discreteKeySet.end()), + modelSelectionTerm)); + } + mpe = discrete_fg.optimize(); } else { throw std::runtime_error( "HybridBayesTree root is not discrete-only. Please check elimination " diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index f91e16cbf1..8327b7f31c 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -84,6 +84,51 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree { */ GaussianBayesTree choose(const DiscreteValues& assignment) const; + /** Error for all conditionals. */ + double error(const HybridValues& values) const { + return HybridGaussianFactorGraph(*this).error(values); + } + + /** + * @brief Helper function to add a clique of hybrid conditionals to the passed + * in GaussianBayesNetTree. Operates recursively on the clique in a bottom-up + * fashion, adding the children first. + * + * @param clique The + * @param result + * @return GaussianBayesNetTree& + */ + GaussianBayesNetTree& addCliqueToTree(const sharedClique& clique, + GaussianBayesNetTree& result) const; + + /** + * @brief Assemble a DecisionTree of (GaussianBayesTree, double) leaves for + * each discrete assignment. + * The included double value is used to make + * constructing the model selection term cleaner and more efficient. + * + * @return GaussianBayesNetValTree + */ + GaussianBayesNetValTree assembleTree() const; + + /* + Compute L(M;Z), the likelihood of the discrete model M + given the measurements Z. + This is called the model selection term. + + To do so, we perform the integration of L(M;Z) ∝ L(X;M,Z)P(X|M). + + By Bayes' rule, P(X|M,Z) ∝ L(X;M,Z)P(X|M), + hence L(X;M,Z)P(X|M) is the unnormalized probabilty of + the joint Gaussian distribution. + + This can be computed by multiplying all the exponentiated errors + of each of the conditionals. + + Return a tree where each leaf value is L(M_i;Z). + */ + AlgebraicDecisionTree modelSelection() const; + /** * @brief Optimize the hybrid Bayes tree by computing the MPE for the current * set of discrete variables and using it to compute the best continuous From 6d7dc57599b21e9dc6f3989324df67790d9982a1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 17 Feb 2024 23:10:37 -0500 Subject: [PATCH 04/20] additional clarifying comments --- gtsam/hybrid/HybridBayesNet.cpp | 2 +- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index eeacc929b8..d343decc8f 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -291,7 +291,7 @@ AlgebraicDecisionTree HybridBayesNet::modelSelection() const { std::numeric_limits::max()); } - // Compute the error for X* and the assignment + // Compute the error at the MLE point X* for the current assignment double error = this->error(HybridValues(mu, DiscreteValues(assignment))); diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 42402076ea..be94f42eee 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -295,8 +295,9 @@ static std::shared_ptr createDiscreteFactor( if (!factor) return 1.0; // TODO(dellaert): not loving this. // Logspace version of: - // exp(-factor->error(kEmpty)) / conditional->normalizationConstant(); - return -factor->error(kEmpty) - conditional->logNormalizationConstant(); + // exp(-factor->error(kEmpty)) * conditional->normalizationConstant(); + // We take negative of the logNormalizationConstant (1/k) to get k + return -factor->error(kEmpty) + (-conditional->logNormalizationConstant()); }; AlgebraicDecisionTree logProbabilities( @@ -324,6 +325,7 @@ static std::shared_ptr createGaussianMixtureFactor( if (factor) { auto hf = std::dynamic_pointer_cast(factor); if (!hf) throw std::runtime_error("Expected HessianFactor!"); + // Add 2.0 term since the constant term will be premultiplied by 0.5 hf->constantTerm() += 2.0 * conditional->logNormalizationConstant(); } return factor; From f12a24e00c666f4f275cf9e942a37f27761bdf6c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 19 Feb 2024 11:33:13 -0500 Subject: [PATCH 05/20] fix comment --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index be94f42eee..ea8bd0b056 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -296,7 +296,7 @@ static std::shared_ptr createDiscreteFactor( // Logspace version of: // exp(-factor->error(kEmpty)) * conditional->normalizationConstant(); - // We take negative of the logNormalizationConstant (1/k) to get k + // We take negative of the logNormalizationConstant `log(1/k)` to get `log(k)`. return -factor->error(kEmpty) + (-conditional->logNormalizationConstant()); }; From d19ebe0fdfbaadc61d52aaea40dfdbf0ce6ecae2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 19 Feb 2024 19:14:59 -0500 Subject: [PATCH 06/20] fix issue with Boost collisions --- gtsam/base/std_optional_serialization.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 5c250eab43..ac0c16c877 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -56,6 +56,8 @@ namespace std { template<> struct is_trivially_move_constructible& t, const unsigned int version) { } // namespace serialization } // namespace boost #endif +#endif From 8372d8490cc4161c09138c03d973a272af3e5fab Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Feb 2024 14:11:47 -0500 Subject: [PATCH 07/20] better printing --- gtsam/hybrid/GaussianMixture.cpp | 3 +-- gtsam/linear/GaussianConditional.cpp | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 7f42e69863..a6f816706d 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -173,9 +173,8 @@ void GaussianMixture::print(const std::string &s, for (auto &dk : discreteKeys()) { std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), "; } + std::cout << "\n logNormalizationConstant: " << logConstant_ << std::endl; std::cout << "\n"; - std::cout << " logNormalizationConstant: " << logConstant_ << "\n" - << std::endl; conditionals_.print( "", [&](Key k) { return formatter(k); }, [&](const GaussianConditional::shared_ptr &gf) -> std::string { diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index f986eed02f..585bbdb331 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -121,7 +121,7 @@ namespace gtsam { const auto mean = solve({}); // solve for mean. mean.print(" mean", formatter); } - cout << " logNormalizationConstant: " << logNormalizationConstant() << std::endl; + cout << " logNormalizationConstant: " << logNormalizationConstant() << endl; if (model_) model_->print(" Noise model: "); else From f62805f8b38b5bfc24986c535310cbc34d7a241d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 23 Feb 2024 12:49:51 -0500 Subject: [PATCH 08/20] add method to select underlying continuous Gaussian graph given discrete assignment --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 24 ++++++++- gtsam/hybrid/HybridGaussianFactorGraph.h | 4 ++ .../tests/testHybridGaussianFactorGraph.cpp | 52 +++++++++++++++++++ 3 files changed, 79 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index ea8bd0b056..32cdddec63 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -296,7 +296,8 @@ static std::shared_ptr createDiscreteFactor( // Logspace version of: // exp(-factor->error(kEmpty)) * conditional->normalizationConstant(); - // We take negative of the logNormalizationConstant `log(1/k)` to get `log(k)`. + // We take negative of the logNormalizationConstant `log(1/k)` + // to get `log(k)`. return -factor->error(kEmpty) + (-conditional->logNormalizationConstant()); }; @@ -326,6 +327,7 @@ static std::shared_ptr createGaussianMixtureFactor( auto hf = std::dynamic_pointer_cast(factor); if (!hf) throw std::runtime_error("Expected HessianFactor!"); // Add 2.0 term since the constant term will be premultiplied by 0.5 + // as per the Hessian definition hf->constantTerm() += 2.0 * conditional->logNormalizationConstant(); } return factor; @@ -563,4 +565,24 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::probPrime( return prob_tree; } +/* ************************************************************************ */ +GaussianFactorGraph HybridGaussianFactorGraph::operator()( + const DiscreteValues &assignment) const { + GaussianFactorGraph gfg; + for (auto &&f : *this) { + if (auto gf = std::dynamic_pointer_cast(f)) { + gfg.push_back(gf); + } else if (auto gc = std::dynamic_pointer_cast(f)) { + gfg.push_back(gf); + } else if (auto gmf = std::dynamic_pointer_cast(f)) { + gfg.push_back((*gmf)(assignment)); + } else if (auto gm = dynamic_pointer_cast(f)) { + gfg.push_back((*gm)(assignment)); + } else { + continue; + } + } + return gfg; +} + } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 8a86a7335c..415464ecd4 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -210,6 +210,10 @@ class GTSAM_EXPORT HybridGaussianFactorGraph GaussianFactorGraphTree assembleGraphTree() const; /// @} + + /// Get the GaussianFactorGraph at a given discrete assignment. + GaussianFactorGraph operator()(const DiscreteValues& assignment) const; + }; } // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 5be2f27426..b97dcef72e 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -490,6 +490,58 @@ TEST(HybridGaussianFactorGraph, SwitchingTwoVar) { } } +/* ****************************************************************************/ +// Select a particular continuous factor graph given a discrete assignment +TEST(HybridGaussianFactorGraph, DiscreteSelection) { + Switching s(3); + + HybridGaussianFactorGraph graph = s.linearizedFactorGraph; + + DiscreteValues dv00{{M(0), 0}, {M(1), 0}}; + GaussianFactorGraph continuous_00 = graph(dv00); + GaussianFactorGraph expected_00; + expected_00.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10))); + expected_00.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-1))); + expected_00.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-1))); + expected_00.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10))); + expected_00.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10))); + + EXPECT(assert_equal(expected_00, continuous_00)); + + DiscreteValues dv01{{M(0), 0}, {M(1), 1}}; + GaussianFactorGraph continuous_01 = graph(dv01); + GaussianFactorGraph expected_01; + expected_01.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10))); + expected_01.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-1))); + expected_01.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-0))); + expected_01.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10))); + expected_01.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10))); + + EXPECT(assert_equal(expected_01, continuous_01)); + + DiscreteValues dv10{{M(0), 1}, {M(1), 0}}; + GaussianFactorGraph continuous_10 = graph(dv10); + GaussianFactorGraph expected_10; + expected_10.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10))); + expected_10.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-0))); + expected_10.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-1))); + expected_10.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10))); + expected_10.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10))); + + EXPECT(assert_equal(expected_10, continuous_10)); + + DiscreteValues dv11{{M(0), 1}, {M(1), 1}}; + GaussianFactorGraph continuous_11 = graph(dv11); + GaussianFactorGraph expected_11; + expected_11.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10))); + expected_11.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-0))); + expected_11.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-0))); + expected_11.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10))); + expected_11.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10))); + + EXPECT(assert_equal(expected_11, continuous_11)); +} + /* ************************************************************************* */ TEST(HybridGaussianFactorGraph, optimize) { HybridGaussianFactorGraph hfg; From 6e8e2579da7e48557ec388aa4905bac2a4470196 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 6 Mar 2024 16:01:30 -0500 Subject: [PATCH 09/20] update model selection code and docs to match the math --- gtsam/hybrid/HybridBayesNet.cpp | 41 +++++++++----------------------- gtsam/hybrid/HybridBayesNet.h | 41 +++++++++----------------------- gtsam/hybrid/HybridBayesTree.cpp | 14 ++++++----- 3 files changed, 30 insertions(+), 66 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index d343decc8f..4c41e952b0 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -265,16 +265,10 @@ GaussianBayesNetValTree HybridBayesNet::assembleTree() const { /* ************************************************************************* */ AlgebraicDecisionTree HybridBayesNet::modelSelection() const { /* - To perform model selection, we need: - q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) - - If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) - thus, q * sqrt((2*pi)^n*det(Sigma)) = q/k = exp(log(q/k)) - = exp(log(q) - log(k)) = exp(-error - log(k)) - = exp(-(error + log(k))), + To perform model selection, we need: q(mu; M, Z) = exp(-error) where error is computed at the corresponding MAP point, gbn.error(mu). - So we compute (error + log(k)) and exponentiate later + So we compute (-error) and exponentiate later */ GaussianBayesNetValTree bnTree = assembleTree(); @@ -301,13 +295,16 @@ AlgebraicDecisionTree HybridBayesNet::modelSelection() const { auto trees = unzip(bn_error); AlgebraicDecisionTree errorTree = trees.second; - // Only compute logNormalizationConstant - AlgebraicDecisionTree log_norm_constants = - computeLogNormConstants(bnTree); - // Compute model selection term (with help from ADT methods) - AlgebraicDecisionTree modelSelectionTerm = - computeModelSelectionTerm(errorTree, log_norm_constants); + AlgebraicDecisionTree modelSelectionTerm = errorTree * -1; + + // Exponentiate using our scheme + double max_log = modelSelectionTerm.max(); + modelSelectionTerm = DecisionTree( + modelSelectionTerm, + [&max_log](const double &x) { return std::exp(x - max_log); }); + modelSelectionTerm = modelSelectionTerm.normalize(modelSelectionTerm.sum()); + return modelSelectionTerm; } @@ -531,20 +528,4 @@ AlgebraicDecisionTree computeLogNormConstants( return log_norm_constants; } -/* ************************************************************************* */ -AlgebraicDecisionTree computeModelSelectionTerm( - const AlgebraicDecisionTree &errorTree, - const AlgebraicDecisionTree &log_norm_constants) { - AlgebraicDecisionTree modelSelectionTerm = - (errorTree + log_norm_constants) * -1; - - double max_log = modelSelectionTerm.max(); - modelSelectionTerm = DecisionTree( - modelSelectionTerm, - [&max_log](const double &x) { return std::exp(x - max_log); }); - modelSelectionTerm = modelSelectionTerm.normalize(modelSelectionTerm.sum()); - - return modelSelectionTerm; -} - } // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index d4fd3db718..c678c418c0 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -128,22 +128,17 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { */ GaussianBayesNetValTree assembleTree() const; - /* - Compute L(M;Z), the likelihood of the discrete model M - given the measurements Z. - This is called the model selection term. - - To do so, we perform the integration of L(M;Z) ∝ L(X;M,Z)P(X|M). - - By Bayes' rule, P(X|M,Z) ∝ L(X;M,Z)P(X|M), - hence L(X;M,Z)P(X|M) is the unnormalized probabilty of - the joint Gaussian distribution. - - This can be computed by multiplying all the exponentiated errors - of each of the conditionals. - - Return a tree where each leaf value is L(M_i;Z). - */ + /** + * @brief Compute the model selection term q(μ_X; M, Z) + * given the error for each discrete assignment. + * + * The q(μ) terms are obtained as a result of elimination + * as part of the separator factor. + * + * Perform normalization to handle underflow issues. + * + * @return AlgebraicDecisionTree + */ AlgebraicDecisionTree modelSelection() const; /** @@ -301,18 +296,4 @@ GaussianBayesNetTree addGaussian(const GaussianBayesNetTree &gbnTree, AlgebraicDecisionTree computeLogNormConstants( const GaussianBayesNetValTree &bnTree); -/** - * @brief Compute the model selection term L(M; Z, X) given the error - * and log normalization constants. - * - * Perform normalization to handle underflow issues. - * - * @param errorTree - * @param log_norm_constants - * @return AlgebraicDecisionTree - */ -AlgebraicDecisionTree computeModelSelectionTerm( - const AlgebraicDecisionTree &errorTree, - const AlgebraicDecisionTree &log_norm_constants); - } // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index cdd30d3980..394c88928a 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -111,13 +111,15 @@ AlgebraicDecisionTree HybridBayesTree::modelSelection() const { auto trees = unzip(bn_error); AlgebraicDecisionTree errorTree = trees.second; - // Only compute logNormalizationConstant - AlgebraicDecisionTree log_norm_constants = - computeLogNormConstants(bnTree); - // Compute model selection term (with help from ADT methods) - AlgebraicDecisionTree modelSelectionTerm = - computeModelSelectionTerm(errorTree, log_norm_constants); + AlgebraicDecisionTree modelSelectionTerm = errorTree * -1; + + // Exponentiate using our scheme + double max_log = modelSelectionTerm.max(); + modelSelectionTerm = DecisionTree( + modelSelectionTerm, + [&max_log](const double& x) { return std::exp(x - max_log); }); + modelSelectionTerm = modelSelectionTerm.normalize(modelSelectionTerm.sum()); return modelSelectionTerm; } From f69891895a1338004e330796a414e3bc4e873441 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 6 Mar 2024 16:12:08 -0500 Subject: [PATCH 10/20] additional test based on Frank's colab notebook --- gtsam/hybrid/tests/testMixtureFactor.cpp | 76 ++++++++++++++++++++++++ 1 file changed, 76 insertions(+) diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 006a3b026c..dd2108d2f3 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -229,6 +229,82 @@ TEST(MixtureFactor, DifferentCovariances) { EXPECT(assert_equal(expected_values, actual_values)); } +/* ************************************************************************* */ +// Test components with differing means and covariances +TEST(MixtureFactor, DifferentMeansAndCovariances) { + DiscreteKey m1(M(1), 2); + + Values values; + double x1 = 0.0, x2 = 7.0; + values.insert(X(1), x1); + + double between = 0.0; + + auto model0 = noiseModel::Isotropic::Sigma(1, 1e2); + auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2); + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); + + auto f0 = + std::make_shared>(X(1), X(2), between, model0); + auto f1 = + std::make_shared>(X(1), X(2), between, model1); + std::vector factors{f0, f1}; + + // Create via toFactorGraph + using symbol_shorthand::Z; + Matrix H0_1, H0_2, H1_1, H1_2; + Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2); + std::vector> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/}, + // + {X(1), H0_1 /*Sp1*/}, + {X(2), H0_2 /*Tp2*/}}; + + Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2); + std::vector> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/}, + // + {X(1), H1_1 /*Sp1*/}, + {X(2), H1_2 /*Tp2*/}}; + auto gm = new gtsam::GaussianMixture( + {Z(1)}, {X(1), X(2)}, {m1}, + {std::make_shared(terms0, 1, -d0, model0), + std::make_shared(terms1, 1, -d1, model1)}); + gtsam::HybridBayesNet bn; + bn.emplace_back(gm); + + gtsam::VectorValues measurements; + measurements.insert(Z(1), gtsam::Z_1x1); + // Create FG with single GaussianMixtureFactor + HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements); + + // Linearized prior factor on X1 + auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); + mixture_fg.push_back(prior); + + // bn.print("BayesNet:"); + // mixture_fg.print("\n\n"); + + VectorValues vv{{X(1), x1 * I_1x1}, {X(2), x2 * I_1x1}}; + // std::cout << "FG error for m1=0: " + // << mixture_fg.error(HybridValues(vv, DiscreteValues{{m1.first, 0}})) + // << std::endl; + // std::cout << "FG error for m1=1: " + // << mixture_fg.error(HybridValues(vv, DiscreteValues{{m1.first, 1}})) + // << std::endl; + + auto hbn = mixture_fg.eliminateSequential(); + + HybridValues actual_values = hbn->optimize(); + + VectorValues cv; + cv.insert(X(1), Vector1(0.0)); + cv.insert(X(2), Vector1(-7.0)); + DiscreteValues dv; + dv.insert({M(1), 1}); + HybridValues expected_values(cv, dv); + + EXPECT(assert_equal(expected_values, actual_values)); +} + /* ************************************************************************* */ int main() { TestResult tr; From c5d6cd5466c01f0a985406cd377b852724c52606 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Apr 2024 16:13:55 +0530 Subject: [PATCH 11/20] wrap DiscreteConditional which takes a vector of doubles --- gtsam/discrete/discrete.i | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index a1731f8e56..0deeb80331 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -104,6 +104,9 @@ virtual class DiscreteConditional : gtsam::DecisionTreeFactor { DiscreteConditional(const gtsam::DecisionTreeFactor& joint, const gtsam::DecisionTreeFactor& marginal, const gtsam::Ordering& orderedKeys); + DiscreteConditional(const gtsam::DiscreteKey& key, + const gtsam::DiscreteKeys& parents, + const std::vector& table); // Standard interface double logNormalizationConstant() const; From c05d17dedfc4d4f943efeb8396f5fb947dac8f71 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Apr 2024 16:14:19 +0530 Subject: [PATCH 12/20] minor formatting in linear.i --- gtsam/linear/linear.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 6862646aeb..8bd3dcb889 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -489,7 +489,7 @@ virtual class GaussianConditional : gtsam::JacobianFactor { GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(const vector> terms, + GaussianConditional(const std::vector> terms, size_t nrFrontals, Vector d, const gtsam::noiseModel::Diagonal* sigmas); @@ -751,4 +751,4 @@ class KalmanFilter { Vector z, Matrix Q); }; -} \ No newline at end of file +} From bd241f61ecc35560f0f5b958639fcc3515a121b8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Apr 2024 16:14:46 +0530 Subject: [PATCH 13/20] wrap logProbability and evaluate methods of GaussianMixtureFactor --- gtsam/hybrid/hybrid.i | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index c1d57715ec..85c34e5755 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -92,7 +92,10 @@ class GaussianMixture : gtsam::HybridFactor { const std::vector& conditionalsList); - gtsam::GaussianMixtureFactor* likelihood(const gtsam::VectorValues &frontals) const; + gtsam::GaussianMixtureFactor* likelihood( + const gtsam::VectorValues& frontals) const; + double logProbability(const gtsam::HybridValues& values) const; + double evaluate(const gtsam::HybridValues& values) const; void print(string s = "GaussianMixture\n", const gtsam::KeyFormatter& keyFormatter = From 8dd2bed6c100b9c576b8fa8b2c7d9aa7bb622e1d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Apr 2024 16:15:21 +0530 Subject: [PATCH 14/20] don't check for 0 continuous keys when printing GaussianMixtureFactor as it is possible to have non-zero discrete keys --- gtsam/hybrid/GaussianMixtureFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 4e138acfa8..fd7cec0669 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -66,7 +66,7 @@ void GaussianMixtureFactor::print(const std::string &s, [&](const sharedFactor &gf) -> std::string { RedirectCout rd; std::cout << ":\n"; - if (gf && !gf->empty()) { + if (gf) { gf->print("", formatter); return rd.str(); } else { From 8333c25dab31ad83aaa4d89fc62d140bf2abc004 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Apr 2024 16:15:54 +0530 Subject: [PATCH 15/20] Fix slam module README --- gtsam/slam/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/README.md b/gtsam/slam/README.md index ae5edfdac0..018126a663 100644 --- a/gtsam/slam/README.md +++ b/gtsam/slam/README.md @@ -63,6 +63,6 @@ A RegularJacobianFactor that uses some badly documented reduction on the Jacobia A RegularJacobianFactor that eliminates a point using sequential elimination. -### JacobianFactorQR +### JacobianFactorSVD -A RegularJacobianFactor that uses the "Nullspace Trick" by Mourikis et al. See the documentation in the file, which *is* well documented. \ No newline at end of file +A RegularJacobianFactor that uses the "Nullspace Trick" by Mourikis et al. See the documentation in the file, which *is* well documented. From 0b1c3688c43f45808ee20e0e02aabaf9673cfc20 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Aug 2024 14:50:54 -0400 Subject: [PATCH 16/20] remove model selection from hybrid bayes tree --- gtsam/hybrid/HybridBayesNet.cpp | 33 -------- gtsam/hybrid/HybridBayesNet.h | 11 --- gtsam/hybrid/HybridBayesTree.cpp | 99 ------------------------ gtsam/hybrid/HybridBayesTree.h | 40 ---------- gtsam/hybrid/tests/testMixtureFactor.cpp | 39 +++------- 5 files changed, 9 insertions(+), 213 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 2bced6c0d3..33c7c91da0 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -378,37 +378,4 @@ HybridGaussianFactorGraph HybridBayesNet::toFactorGraph( } return fg; } - -/* ************************************************************************ */ -GaussianBayesNetTree addGaussian( - const GaussianBayesNetTree &gbnTree, - const GaussianConditional::shared_ptr &factor) { - // If the decision tree is not initialized, then initialize it. - if (gbnTree.empty()) { - GaussianBayesNet result{factor}; - return GaussianBayesNetTree(result); - } else { - auto add = [&factor](const GaussianBayesNet &graph) { - auto result = graph; - result.push_back(factor); - return result; - }; - return gbnTree.apply(add); - } -} - -/* ************************************************************************* */ -AlgebraicDecisionTree computeLogNormConstants( - const GaussianBayesNetValTree &bnTree) { - AlgebraicDecisionTree log_norm_constants = DecisionTree( - bnTree, [](const std::pair &gbnAndValue) { - GaussianBayesNet gbn = gbnAndValue.first; - if (gbn.size() == 0) { - return 0.0; - } - return gbn.logNormalizationConstant(); - }); - return log_norm_constants; -} - } // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 6f7b9aae72..55eaf6b5ec 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -262,15 +262,4 @@ struct traits : public Testable {}; GaussianBayesNetTree addGaussian(const GaussianBayesNetTree &gbnTree, const GaussianConditional::shared_ptr &factor); -/** - * @brief Compute the (logarithmic) normalization constant for each Bayes - * network in the tree. - * - * @param bnTree A tree of Bayes networks in each leaf. The tree encodes a - * discrete assignment yielding the Bayes net. - * @return AlgebraicDecisionTree - */ -AlgebraicDecisionTree computeLogNormConstants( - const GaussianBayesNetValTree &bnTree); - } // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 394c88928a..f08eff01bf 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -38,117 +38,18 @@ bool HybridBayesTree::equals(const This& other, double tol) const { return Base::equals(other, tol); } -GaussianBayesNetTree& HybridBayesTree::addCliqueToTree( - const sharedClique& clique, GaussianBayesNetTree& result) const { - // Perform bottom-up inclusion - for (sharedClique child : clique->children) { - result = addCliqueToTree(child, result); - } - - auto f = clique->conditional(); - - if (auto hc = std::dynamic_pointer_cast(f)) { - if (auto gm = hc->asMixture()) { - result = gm->add(result); - } else if (auto g = hc->asGaussian()) { - result = addGaussian(result, g); - } else { - // Has to be discrete, which we don't add. - } - } - return result; -} - -/* ************************************************************************ */ -GaussianBayesNetValTree HybridBayesTree::assembleTree() const { - GaussianBayesNetTree result; - for (auto&& root : roots_) { - result = addCliqueToTree(root, result); - } - - GaussianBayesNetValTree resultTree(result, [](const GaussianBayesNet& gbn) { - return std::make_pair(gbn, 0.0); - }); - return resultTree; -} - -/* ************************************************************************* */ -AlgebraicDecisionTree HybridBayesTree::modelSelection() const { - /* - To perform model selection, we need: - q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) - - If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) - thus, q * sqrt((2*pi)^n*det(Sigma)) = q/k = exp(log(q/k)) - = exp(log(q) - log(k)) = exp(-error - log(k)) - = exp(-(error + log(k))), - where error is computed at the corresponding MAP point, gbt.error(mu). - - So we compute (error + log(k)) and exponentiate later - */ - - GaussianBayesNetValTree bnTree = assembleTree(); - - GaussianBayesNetValTree bn_error = bnTree.apply( - [this](const Assignment& assignment, - const std::pair& gbnAndValue) { - // Compute the X* of each assignment - VectorValues mu = gbnAndValue.first.optimize(); - - // mu is empty if gbn had nullptrs - if (mu.size() == 0) { - return std::make_pair(gbnAndValue.first, - std::numeric_limits::max()); - } - - // Compute the error for X* and the assignment - double error = - this->error(HybridValues(mu, DiscreteValues(assignment))); - - return std::make_pair(gbnAndValue.first, error); - }); - - auto trees = unzip(bn_error); - AlgebraicDecisionTree errorTree = trees.second; - - // Compute model selection term (with help from ADT methods) - AlgebraicDecisionTree modelSelectionTerm = errorTree * -1; - - // Exponentiate using our scheme - double max_log = modelSelectionTerm.max(); - modelSelectionTerm = DecisionTree( - modelSelectionTerm, - [&max_log](const double& x) { return std::exp(x - max_log); }); - modelSelectionTerm = modelSelectionTerm.normalize(modelSelectionTerm.sum()); - - return modelSelectionTerm; -} - /* ************************************************************************* */ HybridValues HybridBayesTree::optimize() const { DiscreteFactorGraph discrete_fg; DiscreteValues mpe; - // Compute model selection term - AlgebraicDecisionTree modelSelectionTerm = modelSelection(); - auto root = roots_.at(0); // Access the clique and get the underlying hybrid conditional HybridConditional::shared_ptr root_conditional = root->conditional(); - // Get the set of all discrete keys involved in model selection - std::set discreteKeySet; - // The root should be discrete only, we compute the MPE if (root_conditional->isDiscrete()) { discrete_fg.push_back(root_conditional->asDiscrete()); - - // Only add model_selection if we have discrete keys - if (discreteKeySet.size() > 0) { - discrete_fg.push_back(DecisionTreeFactor( - DiscreteKeys(discreteKeySet.begin(), discreteKeySet.end()), - modelSelectionTerm)); - } mpe = discrete_fg.optimize(); } else { throw std::runtime_error( diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 8327b7f31c..af8eb32288 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -89,46 +89,6 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree { return HybridGaussianFactorGraph(*this).error(values); } - /** - * @brief Helper function to add a clique of hybrid conditionals to the passed - * in GaussianBayesNetTree. Operates recursively on the clique in a bottom-up - * fashion, adding the children first. - * - * @param clique The - * @param result - * @return GaussianBayesNetTree& - */ - GaussianBayesNetTree& addCliqueToTree(const sharedClique& clique, - GaussianBayesNetTree& result) const; - - /** - * @brief Assemble a DecisionTree of (GaussianBayesTree, double) leaves for - * each discrete assignment. - * The included double value is used to make - * constructing the model selection term cleaner and more efficient. - * - * @return GaussianBayesNetValTree - */ - GaussianBayesNetValTree assembleTree() const; - - /* - Compute L(M;Z), the likelihood of the discrete model M - given the measurements Z. - This is called the model selection term. - - To do so, we perform the integration of L(M;Z) ∝ L(X;M,Z)P(X|M). - - By Bayes' rule, P(X|M,Z) ∝ L(X;M,Z)P(X|M), - hence L(X;M,Z)P(X|M) is the unnormalized probabilty of - the joint Gaussian distribution. - - This can be computed by multiplying all the exponentiated errors - of each of the conditionals. - - Return a tree where each leaf value is L(M_i;Z). - */ - AlgebraicDecisionTree modelSelection() const; - /** * @brief Optimize the hybrid Bayes tree by computing the MPE for the current * set of discrete variables and using it to compute the best continuous diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 9de2772143..1bd4f5b884 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -252,30 +252,16 @@ TEST(MixtureFactor, DifferentCovariances) { // Check that we get different error values at the MLE point μ. AlgebraicDecisionTree errorTree = hbn->errorTree(cv); - auto cond0 = hbn->at(0)->asMixture(); - auto cond1 = hbn->at(1)->asMixture(); - auto discrete_cond = hbn->at(2)->asDiscrete(); HybridValues hv0(cv, DiscreteValues{{M(1), 0}}); HybridValues hv1(cv, DiscreteValues{{M(1), 1}}); - AlgebraicDecisionTree expectedErrorTree( - m1, - cond0->error(hv0) // cond0(0)->logNormalizationConstant() - // - cond0(1)->logNormalizationConstant - + cond1->error(hv0) + discrete_cond->error(DiscreteValues{{M(1), 0}}), - cond0->error(hv1) // cond1(0)->logNormalizationConstant() - // - cond1(1)->logNormalizationConstant - + cond1->error(hv1) + - discrete_cond->error(DiscreteValues{{M(1), 0}})); - EXPECT(assert_equal(expectedErrorTree, errorTree)); - - DiscreteValues dv; - dv.insert({M(1), 1}); - HybridValues expected_values(cv, dv); - - HybridValues actual_values = hbn->optimize(); - EXPECT(assert_equal(expected_values, actual_values)); + auto cond0 = hbn->at(0)->asMixture(); + auto cond1 = hbn->at(1)->asMixture(); + auto discrete_cond = hbn->at(2)->asDiscrete(); + AlgebraicDecisionTree expectedErrorTree(m1, 9.90348755254, + 0.69314718056); + EXPECT(assert_equal(expectedErrorTree, errorTree)); } /* ************************************************************************* */ @@ -329,16 +315,7 @@ TEST(MixtureFactor, DifferentMeansAndCovariances) { auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); mixture_fg.push_back(prior); - // bn.print("BayesNet:"); - // mixture_fg.print("\n\n"); - VectorValues vv{{X(1), x1 * I_1x1}, {X(2), x2 * I_1x1}}; - // std::cout << "FG error for m1=0: " - // << mixture_fg.error(HybridValues(vv, DiscreteValues{{m1.first, 0}})) - // << std::endl; - // std::cout << "FG error for m1=1: " - // << mixture_fg.error(HybridValues(vv, DiscreteValues{{m1.first, 1}})) - // << std::endl; auto hbn = mixture_fg.eliminateSequential(); @@ -347,8 +324,10 @@ TEST(MixtureFactor, DifferentMeansAndCovariances) { VectorValues cv; cv.insert(X(1), Vector1(0.0)); cv.insert(X(2), Vector1(-7.0)); + + // The first value is chosen as the tiebreaker DiscreteValues dv; - dv.insert({M(1), 1}); + dv.insert({M(1), 0}); HybridValues expected_values(cv, dv); EXPECT(assert_equal(expected_values, actual_values)); From 9d27ce8186c1023e9e84f0476d14b6a63953ff59 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Aug 2024 05:29:02 -0400 Subject: [PATCH 17/20] remove unused code in HybridBayesNet --- gtsam/hybrid/HybridBayesNet.cpp | 1 + gtsam/hybrid/HybridBayesNet.h | 10 ---------- 2 files changed, 1 insertion(+), 10 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 362174514c..b029675552 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -377,4 +377,5 @@ HybridGaussianFactorGraph HybridBayesNet::toFactorGraph( } return fg; } + } // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 55eaf6b5ec..032cd55b97 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -252,14 +252,4 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { template <> struct traits : public Testable {}; -/** - * @brief Add a Gaussian conditional to each node of the GaussianBayesNetTree - * - * @param gbnTree - * @param factor - * @return GaussianBayesNetTree - */ -GaussianBayesNetTree addGaussian(const GaussianBayesNetTree &gbnTree, - const GaussianConditional::shared_ptr &factor); - } // namespace gtsam From 0145a93d66db0ad1a64fd25f954eaee05592ea1d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Aug 2024 05:32:47 -0400 Subject: [PATCH 18/20] more code cleanup --- gtsam/hybrid/HybridBayesTree.cpp | 11 +- gtsam/hybrid/HybridBayesTree.h | 4 +- gtsam/hybrid/tests/testMixtureFactor.cpp | 215 ----------------------- 3 files changed, 8 insertions(+), 222 deletions(-) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index f08eff01bf..da2645b5a6 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -136,8 +136,7 @@ struct HybridAssignmentData { } }; -/* ************************************************************************* - */ +/* ************************************************************************* */ GaussianBayesTree HybridBayesTree::choose( const DiscreteValues& assignment) const { GaussianBayesTree gbt; @@ -157,8 +156,12 @@ GaussianBayesTree HybridBayesTree::choose( return gbt; } -/* ************************************************************************* - */ +/* ************************************************************************* */ +double HybridBayesTree::error(const HybridValues& values) const { + return HybridGaussianFactorGraph(*this).error(values); +} + +/* ************************************************************************* */ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { GaussianBayesTree gbt = this->choose(assignment); // If empty GaussianBayesTree, means a clique is pruned hence invalid diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index af8eb32288..ab2d8a73d2 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -85,9 +85,7 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree { GaussianBayesTree choose(const DiscreteValues& assignment) const; /** Error for all conditionals. */ - double error(const HybridValues& values) const { - return HybridGaussianFactorGraph(*this).error(values); - } + double error(const HybridValues& values) const; /** * @brief Optimize the hybrid Bayes tree by computing the MPE for the current diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 52f56f62ea..0b2564403a 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -115,221 +115,6 @@ TEST(MixtureFactor, Dim) { EXPECT_LONGS_EQUAL(1, mixtureFactor.dim()); } -/* ************************************************************************* */ -// Test components with differing means -TEST(MixtureFactor, DifferentMeans) { - DiscreteKey m1(M(1), 2), m2(M(2), 2); - - Values values; - double x1 = 0.0, x2 = 1.75, x3 = 2.60; - values.insert(X(1), x1); - values.insert(X(2), x2); - values.insert(X(3), x3); - - auto model0 = noiseModel::Isotropic::Sigma(1, 1e-0); - auto model1 = noiseModel::Isotropic::Sigma(1, 1e-0); - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-0); - - auto f0 = std::make_shared>(X(1), X(2), 0.0, model0); - auto f1 = std::make_shared>(X(1), X(2), 2.0, model1); - std::vector factors{f0, f1}; - - MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); - HybridNonlinearFactorGraph hnfg; - hnfg.push_back(mixtureFactor); - - f0 = std::make_shared>(X(2), X(3), 0.0, model0); - f1 = std::make_shared>(X(2), X(3), 2.0, model1); - std::vector factors23{f0, f1}; - hnfg.push_back(MixtureFactor({X(2), X(3)}, {m2}, factors23)); - - auto prior = PriorFactor(X(1), x1, prior_noise); - hnfg.push_back(prior); - - hnfg.emplace_shared>(X(2), 2.0, prior_noise); - - auto hgfg = hnfg.linearize(values); - auto bn = hgfg->eliminateSequential(); - HybridValues actual = bn->optimize(); - - HybridValues expected( - VectorValues{ - {X(1), Vector1(0.0)}, {X(2), Vector1(0.25)}, {X(3), Vector1(-0.6)}}, - DiscreteValues{{M(1), 1}, {M(2), 0}}); - - EXPECT(assert_equal(expected, actual)); - - { - DiscreteValues dv{{M(1), 0}, {M(2), 0}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9); - } - { - DiscreteValues dv{{M(1), 0}, {M(2), 1}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9); - } - { - DiscreteValues dv{{M(1), 1}, {M(2), 0}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9); - } - { - DiscreteValues dv{{M(1), 1}, {M(2), 1}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9); - } -} - -/* ************************************************************************* */ -// Test components with differing covariances -TEST(MixtureFactor, DifferentCovariances) { - DiscreteKey m1(M(1), 2); - - Values values; - double x1 = 1.0, x2 = 1.0; - values.insert(X(1), x1); - values.insert(X(2), x2); - - double between = 0.0; - - auto model0 = noiseModel::Isotropic::Sigma(1, 1e2); - auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2); - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); - - auto f0 = - std::make_shared>(X(1), X(2), between, model0); - auto f1 = - std::make_shared>(X(1), X(2), between, model1); - std::vector factors{f0, f1}; - - // Create via toFactorGraph - using symbol_shorthand::Z; - Matrix H0_1, H0_2, H1_1, H1_2; - Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2); - std::vector> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/}, - // - {X(1), H0_1 /*Sp1*/}, - {X(2), H0_2 /*Tp2*/}}; - - Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2); - std::vector> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/}, - // - {X(1), H1_1 /*Sp1*/}, - {X(2), H1_2 /*Tp2*/}}; - auto gm = new gtsam::GaussianMixture( - {Z(1)}, {X(1), X(2)}, {m1}, - {std::make_shared(terms0, 1, -d0, model0), - std::make_shared(terms1, 1, -d1, model1)}); - gtsam::HybridBayesNet bn; - bn.emplace_back(gm); - - gtsam::VectorValues measurements; - measurements.insert(Z(1), gtsam::Z_1x1); - // Create FG with single GaussianMixtureFactor - HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements); - - // Linearized prior factor on X1 - auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); - mixture_fg.push_back(prior); - - auto hbn = mixture_fg.eliminateSequential(); - - VectorValues cv; - cv.insert(X(1), Vector1(0.0)); - cv.insert(X(2), Vector1(0.0)); - - // Check that we get different error values at the MLE point μ. - AlgebraicDecisionTree errorTree = hbn->errorTree(cv); - - HybridValues hv0(cv, DiscreteValues{{M(1), 0}}); - HybridValues hv1(cv, DiscreteValues{{M(1), 1}}); - - auto cond0 = hbn->at(0)->asMixture(); - auto cond1 = hbn->at(1)->asMixture(); - auto discrete_cond = hbn->at(2)->asDiscrete(); - AlgebraicDecisionTree expectedErrorTree(m1, 9.90348755254, - 0.69314718056); - EXPECT(assert_equal(expectedErrorTree, errorTree)); -} - -/* ************************************************************************* */ -// Test components with differing means and covariances -TEST(MixtureFactor, DifferentMeansAndCovariances) { - DiscreteKey m1(M(1), 2); - - Values values; - double x1 = 0.0, x2 = 7.0; - values.insert(X(1), x1); - - double between = 0.0; - - auto model0 = noiseModel::Isotropic::Sigma(1, 1e2); - auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2); - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); - - auto f0 = - std::make_shared>(X(1), X(2), between, model0); - auto f1 = - std::make_shared>(X(1), X(2), between, model1); - std::vector factors{f0, f1}; - - // Create via toFactorGraph - using symbol_shorthand::Z; - Matrix H0_1, H0_2, H1_1, H1_2; - Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2); - std::vector> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/}, - // - {X(1), H0_1 /*Sp1*/}, - {X(2), H0_2 /*Tp2*/}}; - - Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2); - std::vector> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/}, - // - {X(1), H1_1 /*Sp1*/}, - {X(2), H1_2 /*Tp2*/}}; - auto gm = new gtsam::GaussianMixture( - {Z(1)}, {X(1), X(2)}, {m1}, - {std::make_shared(terms0, 1, -d0, model0), - std::make_shared(terms1, 1, -d1, model1)}); - gtsam::HybridBayesNet bn; - bn.emplace_back(gm); - - gtsam::VectorValues measurements; - measurements.insert(Z(1), gtsam::Z_1x1); - // Create FG with single GaussianMixtureFactor - HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements); - - // Linearized prior factor on X1 - auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); - mixture_fg.push_back(prior); - - VectorValues vv{{X(1), x1 * I_1x1}, {X(2), x2 * I_1x1}}; - - auto hbn = mixture_fg.eliminateSequential(); - - HybridValues actual_values = hbn->optimize(); - - VectorValues cv; - cv.insert(X(1), Vector1(0.0)); - cv.insert(X(2), Vector1(-7.0)); - - // The first value is chosen as the tiebreaker - DiscreteValues dv; - dv.insert({M(1), 0}); - HybridValues expected_values(cv, dv); - - EXPECT(assert_equal(expected_values, actual_values)); -} - /* ************************************************************************* */ int main() { TestResult tr; From 019dad976eae0bb9dcff152dea1bc64030415325 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Aug 2024 06:33:28 -0400 Subject: [PATCH 19/20] fix test --- gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 751e84d915..1dcd0bbb17 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -675,6 +675,8 @@ factor 6: P( m1 | m0 ): size: 3 conditional 0: Hybrid P( x0 | x1 m0) Discrete Keys = (m0, 2), + logNormalizationConstant: 1.38862 + Choice(m0) 0 Leaf p(x0 | x1) R = [ 10.0499 ] @@ -692,6 +694,8 @@ conditional 0: Hybrid P( x0 | x1 m0) conditional 1: Hybrid P( x1 | x2 m0 m1) Discrete Keys = (m0, 2), (m1, 2), + logNormalizationConstant: 1.3935 + Choice(m1) 0 Choice(m0) 0 0 Leaf p(x1 | x2) @@ -725,6 +729,8 @@ conditional 1: Hybrid P( x1 | x2 m0 m1) conditional 2: Hybrid P( x2 | m0 m1) Discrete Keys = (m0, 2), (m1, 2), + logNormalizationConstant: 1.38857 + Choice(m1) 0 Choice(m0) 0 0 Leaf p(x2) From 1725deff1bd4b696467cad06308444f301506bf3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 01:50:57 -0400 Subject: [PATCH 20/20] better GaussianMixture printing --- gtsam/hybrid/GaussianMixture.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 729842485b..888872f819 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -144,8 +144,9 @@ void GaussianMixture::print(const std::string &s, for (auto &dk : discreteKeys()) { std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), "; } - std::cout << "\n logNormalizationConstant: " << logConstant_ << std::endl; - std::cout << "\n"; + std::cout << std::endl + << " logNormalizationConstant: " << logConstant_ << std::endl + << std::endl; conditionals_.print( "", [&](Key k) { return formatter(k); }, [&](const GaussianConditional::shared_ptr &gf) -> std::string {