From 4343b3aadca7ecad7d40a972349d0ea3a5114d22 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 13 Sep 2024 15:20:27 -0400 Subject: [PATCH 01/30] update HybridNonlinearFactor to accept a tree of nonlinear factors and arbitrary scalars --- gtsam/hybrid/HybridNonlinearFactor.h | 66 +++++++++++-------- gtsam/hybrid/tests/Switching.h | 5 +- gtsam/hybrid/tests/testHybridBayesNet.cpp | 3 +- gtsam/hybrid/tests/testHybridEstimation.cpp | 13 ++-- gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 7 +- .../tests/testHybridNonlinearFactor.cpp | 6 +- .../tests/testHybridNonlinearFactorGraph.cpp | 9 ++- .../hybrid/tests/testHybridNonlinearISAM.cpp | 7 +- 8 files changed, 68 insertions(+), 48 deletions(-) diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index f5d9cd356a..2c2810f528 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -53,9 +53,9 @@ class HybridNonlinearFactor : public HybridFactor { /** * @brief typedef for DecisionTree which has Keys as node labels and - * NonlinearFactor as leaf nodes. + * pairs of NonlinearFactor & an arbitrary scalar as leaf nodes. */ - using Factors = DecisionTree; + using Factors = DecisionTree>; private: /// Decision tree of Gaussian factors indexed by discrete keys. @@ -90,25 +90,27 @@ class HybridNonlinearFactor : public HybridFactor { * Will be typecast to NonlinearFactor shared pointers. * @param keys Vector of keys for continuous factors. * @param discreteKeys Vector of discrete keys. - * @param factors Vector of nonlinear factors. + * @param factors Vector of nonlinear factor and scalar pairs. * @param normalized Flag indicating if the factor error is already * normalized. */ template - HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys, - const std::vector>& factors, - bool normalized = false) + HybridNonlinearFactor( + const KeyVector& keys, const DiscreteKeys& discreteKeys, + const std::vector, double>>& factors, + bool normalized = false) : Base(keys, discreteKeys), normalized_(normalized) { - std::vector nonlinear_factors; + std::vector> + nonlinear_factors; KeySet continuous_keys_set(keys.begin(), keys.end()); KeySet factor_keys_set; - for (auto&& f : factors) { + for (auto&& [f, val] : factors) { // Insert all factor continuous keys in the continuous keys set. std::copy(f->keys().begin(), f->keys().end(), std::inserter(factor_keys_set, factor_keys_set.end())); if (auto nf = std::dynamic_pointer_cast(f)) { - nonlinear_factors.push_back(nf); + nonlinear_factors.push_back(std::make_pair(nf, val)); } else { throw std::runtime_error( "Factors passed into HybridNonlinearFactor need to be nonlinear!"); @@ -133,9 +135,11 @@ class HybridNonlinearFactor : public HybridFactor { */ AlgebraicDecisionTree errorTree(const Values& continuousValues) const { // functor to convert from sharedFactor to double error value. - auto errorFunc = [continuousValues](const sharedFactor& factor) { - return factor->error(continuousValues); - }; + auto errorFunc = + [continuousValues](const std::pair& f) { + auto [factor, val] = f; + return factor->error(continuousValues) + val; + }; DecisionTree result(factors_, errorFunc); return result; } @@ -150,12 +154,10 @@ class HybridNonlinearFactor : public HybridFactor { double error(const Values& continuousValues, const DiscreteValues& discreteValues) const { // Retrieve the factor corresponding to the assignment in discreteValues. - auto factor = factors_(discreteValues); + auto [factor, val] = factors_(discreteValues); // Compute the error for the selected factor const double factorError = factor->error(continuousValues); - if (normalized_) return factorError; - return factorError + this->nonlinearFactorLogNormalizingConstant( - factor, continuousValues); + return factorError + val; } /** @@ -175,7 +177,7 @@ class HybridNonlinearFactor : public HybridFactor { */ size_t dim() const { const auto assignments = DiscreteValues::CartesianProduct(discreteKeys_); - auto factor = factors_(assignments.at(0)); + auto [factor, val] = factors_(assignments.at(0)); return factor->dim(); } @@ -189,9 +191,11 @@ class HybridNonlinearFactor : public HybridFactor { std::cout << (s.empty() ? "" : s + " "); Base::print("", keyFormatter); std::cout << "\nHybridNonlinearFactor\n"; - auto valueFormatter = [](const sharedFactor& v) { - if (v) { - return "Nonlinear factor on " + std::to_string(v->size()) + " keys"; + auto valueFormatter = [](const std::pair& v) { + auto [factor, val] = v; + if (factor) { + return "Nonlinear factor on " + std::to_string(factor->size()) + + " keys"; } else { return std::string("nullptr"); } @@ -211,8 +215,10 @@ class HybridNonlinearFactor : public HybridFactor { static_cast(other)); // Ensure that this HybridNonlinearFactor and `f` have the same `factors_`. - auto compare = [tol](const sharedFactor& a, const sharedFactor& b) { - return traits::Equals(*a, *b, tol); + auto compare = [tol](const std::pair& a, + const std::pair& b) { + return traits::Equals(*a.first, *b.first, tol) && + (a.second == b.second); }; if (!factors_.equals(f.factors_, compare)) return false; @@ -230,7 +236,7 @@ class HybridNonlinearFactor : public HybridFactor { GaussianFactor::shared_ptr linearize( const Values& continuousValues, const DiscreteValues& discreteValues) const { - auto factor = factors_(discreteValues); + auto [factor, val] = factors_(discreteValues); return factor->linearize(continuousValues); } @@ -238,12 +244,14 @@ class HybridNonlinearFactor : public HybridFactor { std::shared_ptr linearize( const Values& continuousValues) const { // functional to linearize each factor in the decision tree - auto linearizeDT = [continuousValues](const sharedFactor& factor) { - return factor->linearize(continuousValues); - }; - - DecisionTree linearized_factors( - factors_, linearizeDT); + auto linearizeDT = + [continuousValues](const std::pair& f) { + auto [factor, val] = f; + return {factor->linearize(continuousValues), val}; + }; + + DecisionTree> + linearized_factors(factors_, linearizeDT); return std::make_shared( continuousKeys_, discreteKeys_, linearized_factors); diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index f08d29fd5c..851a506983 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -159,9 +159,10 @@ struct Switching { for (size_t k = 0; k < K - 1; k++) { KeyVector keys = {X(k), X(k + 1)}; auto motion_models = motionModels(k, between_sigma); - std::vector components; + std::vector> components; for (auto &&f : motion_models) { - components.push_back(std::dynamic_pointer_cast(f)); + components.push_back( + {std::dynamic_pointer_cast(f), 0.0}); } nonlinearFactorGraph.emplace_shared( keys, DiscreteKeys{modes[k]}, components); diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index b35468302d..bf3301d2fe 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -387,7 +387,8 @@ TEST(HybridBayesNet, Sampling) { std::make_shared>(X(0), X(1), 0, noise_model); auto one_motion = std::make_shared>(X(0), X(1), 1, noise_model); - std::vector factors = {zero_motion, one_motion}; + std::vector> factors = { + {zero_motion, 0.0}, {one_motion, 0.0}}; nfg.emplace_shared>(X(0), 0.0, noise_model); nfg.emplace_shared( KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 308daca9aa..9e64b73da0 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -435,9 +435,10 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() { std::make_shared>(X(0), X(1), 0, noise_model); const auto one_motion = std::make_shared>(X(0), X(1), 1, noise_model); - nfg.emplace_shared( - KeyVector{X(0), X(1)}, DiscreteKeys{m}, - std::vector{zero_motion, one_motion}); + std::vector> components = { + {zero_motion, 0.0}, {one_motion, 0.0}}; + nfg.emplace_shared(KeyVector{X(0), X(1)}, + DiscreteKeys{m}, components); return nfg; } @@ -589,7 +590,8 @@ TEST(HybridEstimation, ModeSelection) { model1 = std::make_shared( X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight)); - std::vector components = {model0, model1}; + std::vector> components = { + {model0, 0.0}, {model1, 0.0}}; KeyVector keys = {X(0), X(1)}; HybridNonlinearFactor mf(keys, modes, components); @@ -680,7 +682,8 @@ TEST(HybridEstimation, ModeSelection2) { model1 = std::make_shared>( X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight)); - std::vector components = {model0, model1}; + std::vector> components = { + {model0, 0.0}, {model1, 0.0}}; KeyVector keys = {X(0), X(1)}; HybridNonlinearFactor mf(keys, modes, components); diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index d0a4a79af5..cf983254a2 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -420,7 +420,8 @@ TEST(HybridGaussianISAM, NonTrivial) { noise_model), moving = std::make_shared(W(0), W(1), odometry, noise_model); - std::vector components = {moving, still}; + std::vector> components = { + {moving, 0.0}, {still, 0.0}}; auto mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); fg.push_back(mixtureFactor); @@ -460,7 +461,7 @@ TEST(HybridGaussianISAM, NonTrivial) { noise_model); moving = std::make_shared(W(1), W(2), odometry, noise_model); - components = {moving, still}; + components = {{moving, 0.0}, {still, 0.0}}; mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); fg.push_back(mixtureFactor); @@ -503,7 +504,7 @@ TEST(HybridGaussianISAM, NonTrivial) { noise_model); moving = std::make_shared(W(2), W(3), odometry, noise_model); - components = {moving, still}; + components = {{moving, 0.0}, {still, 0.0}}; mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); fg.push_back(mixtureFactor); diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp index 71d5108a05..fa31fbe7db 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp @@ -58,7 +58,8 @@ TEST(HybridNonlinearFactor, Printing) { std::make_shared>(X(1), X(2), between0, model); auto f1 = std::make_shared>(X(1), X(2), between1, model); - std::vector factors{f0, f1}; + std::vector> factors{ + {f0, 0.0}, {f1, 0.0}}; HybridNonlinearFactor mixtureFactor({X(1), X(2)}, {m1}, factors); @@ -86,7 +87,8 @@ static HybridNonlinearFactor getHybridNonlinearFactor() { std::make_shared>(X(1), X(2), between0, model); auto f1 = std::make_shared>(X(1), X(2), between1, model); - std::vector factors{f0, f1}; + std::vector> factors{ + {f0, 0.0}, {f1, 0.0}}; return HybridNonlinearFactor({X(1), X(2)}, {m1}, factors); } diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index fd3494bea3..947c24677c 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -131,7 +131,8 @@ TEST(HybridGaussianFactorGraph, Resize) { auto still = std::make_shared(X(0), X(1), 0.0, noise_model), moving = std::make_shared(X(0), X(1), 1.0, noise_model); - std::vector components = {still, moving}; + std::vector> components = { + {still, 0.0}, {moving, 0.0}}; auto dcFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); nhfg.push_back(dcFactor); @@ -162,7 +163,8 @@ TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) { auto still = std::make_shared(X(0), X(1), 0.0, noise_model), moving = std::make_shared(X(0), X(1), 1.0, noise_model); - std::vector components = {still, moving}; + std::vector> components = { + {still, 0.0}, {moving, 0.0}}; // Check for exception when number of continuous keys are under-specified. KeyVector contKeys = {X(0)}; @@ -801,7 +803,8 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { noise_model), moving = std::make_shared(X(0), X(1), odometry, noise_model); - std::vector motion_models = {still, moving}; + std::vector> motion_models = + {{still, 0.0}, {moving, 0.0}}; fg.emplace_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models); diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index f4054c11ab..7519162ebe 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -439,7 +439,8 @@ TEST(HybridNonlinearISAM, NonTrivial) { noise_model), moving = std::make_shared(W(0), W(1), odometry, noise_model); - std::vector components = {moving, still}; + std::vector> components = { + {moving, 0.0}, {still, 0.0}}; auto mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); fg.push_back(mixtureFactor); @@ -479,7 +480,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { noise_model); moving = std::make_shared(W(1), W(2), odometry, noise_model); - components = {moving, still}; + components = {{moving, 0.0}, {still, 0.0}}; mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); fg.push_back(mixtureFactor); @@ -522,7 +523,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { noise_model); moving = std::make_shared(W(2), W(3), odometry, noise_model); - components = {moving, still}; + components = {{moving, 0.0}, {still, 0.0}}; mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); fg.push_back(mixtureFactor); From 0bbf16bf4b9677f4153d253a8d5b59f470a7d25e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 13 Sep 2024 17:37:29 -0400 Subject: [PATCH 02/30] HybridGaussianFactor tests passing --- gtsam/hybrid/HybridGaussianConditional.cpp | 17 ++++------ gtsam/hybrid/HybridGaussianFactor.cpp | 30 ++++++++++------ gtsam/hybrid/HybridGaussianFactor.h | 19 ++++++----- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 34 +++++++++++-------- gtsam/hybrid/HybridNonlinearFactor.h | 9 ++--- .../hybrid/tests/testHybridGaussianFactor.cpp | 14 +++++--- 6 files changed, 71 insertions(+), 52 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 42e7021556..093d50fbf0 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -220,22 +220,19 @@ std::shared_ptr HybridGaussianConditional::likelihood( const DiscreteKeys discreteParentKeys = discreteKeys(); const KeyVector continuousParentKeys = continuousParents(); const HybridGaussianFactor::Factors likelihoods( - conditionals_, [&](const GaussianConditional::shared_ptr &conditional) { - const auto likelihood_m = conditional->likelihood(given); + conditionals_, + [&](const GaussianConditional::shared_ptr &conditional) + -> std::pair { + auto likelihood_m = conditional->likelihood(given); const double Cgm_Kgcm = logConstant_ - conditional->logNormalizationConstant(); if (Cgm_Kgcm == 0.0) { - return likelihood_m; + return {likelihood_m, 0.0}; } else { // Add a constant factor to the likelihood in case the noise models // are not all equal. - GaussianFactorGraph gfg; - gfg.push_back(likelihood_m); - Vector c(1); - c << std::sqrt(2.0 * Cgm_Kgcm); - auto constantFactor = std::make_shared(c); - gfg.push_back(constantFactor); - return std::make_shared(gfg); + double c = std::sqrt(2.0 * Cgm_Kgcm); + return {likelihood_m, c}; } }); return std::make_shared( diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index f7207bf81d..6fb453e75a 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -46,8 +46,10 @@ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { // Check the base and the factors: return Base::equals(*e, tol) && factors_.equals(e->factors_, - [tol](const sharedFactor &f1, const sharedFactor &f2) { - return f1->equals(*f2, tol); + [tol](const std::pair &f1, + const std::pair &f2) { + return f1.first->equals(*f2.first, tol) && + (f1.second == f2.second); }); } @@ -63,11 +65,13 @@ void HybridGaussianFactor::print(const std::string &s, } else { factors_.print( "", [&](Key k) { return formatter(k); }, - [&](const sharedFactor &gf) -> std::string { + [&](const std::pair &gfv) -> std::string { + auto [gf, val] = gfv; RedirectCout rd; std::cout << ":\n"; if (gf) { gf->print("", formatter); + std::cout << "value: " << val << std::endl; return rd.str(); } else { return "nullptr"; @@ -78,8 +82,8 @@ void HybridGaussianFactor::print(const std::string &s, } /* *******************************************************************************/ -HybridGaussianFactor::sharedFactor HybridGaussianFactor::operator()( - const DiscreteValues &assignment) const { +std::pair +HybridGaussianFactor::operator()(const DiscreteValues &assignment) const { return factors_(assignment); } @@ -99,7 +103,9 @@ GaussianFactorGraphTree HybridGaussianFactor::add( /* *******************************************************************************/ GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree() const { - auto wrap = [](const sharedFactor &gf) { return GaussianFactorGraph{gf}; }; + auto wrap = [](const std::pair &gfv) { + return GaussianFactorGraph{gfv.first}; + }; return {factors_, wrap}; } @@ -107,17 +113,19 @@ GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree() AlgebraicDecisionTree HybridGaussianFactor::errorTree( const VectorValues &continuousValues) const { // functor to convert from sharedFactor to double error value. - auto errorFunc = [&continuousValues](const sharedFactor &gf) { - return gf->error(continuousValues); - }; + auto errorFunc = + [&continuousValues](const std::pair &gfv) { + auto [gf, val] = gfv; + return gf->error(continuousValues) + val; + }; DecisionTree error_tree(factors_, errorFunc); return error_tree; } /* *******************************************************************************/ double HybridGaussianFactor::error(const HybridValues &values) const { - const sharedFactor gf = factors_(values.discrete()); - return gf->error(values.continuous()); + auto &&[gf, val] = factors_(values.discrete()); + return gf->error(values.continuous()) + val; } } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 650f7a5a7c..30c35dd2b9 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -53,7 +53,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { using sharedFactor = std::shared_ptr; /// typedef for Decision Tree of Gaussian factors and log-constant. - using Factors = DecisionTree; + using Factors = DecisionTree>; private: /// Decision tree of Gaussian factors indexed by discrete keys. @@ -80,8 +80,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { * @param continuousKeys A vector of keys representing continuous variables. * @param discreteKeys A vector of keys representing discrete variables and * their cardinalities. - * @param factors The decision tree of Gaussian factors stored - * as the mixture density. + * @param factors The decision tree of Gaussian factors and arbitrary scalars. */ HybridGaussianFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, @@ -93,11 +92,12 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { * * @param continuousKeys Vector of keys for continuous factors. * @param discreteKeys Vector of discrete keys. - * @param factors Vector of gaussian factor shared pointers. + * @param factors Vector of gaussian factor shared pointers + * and arbitrary scalars. */ - HybridGaussianFactor(const KeyVector &continuousKeys, - const DiscreteKeys &discreteKeys, - const std::vector &factors) + HybridGaussianFactor( + const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, + const std::vector> &factors) : HybridGaussianFactor(continuousKeys, discreteKeys, Factors(discreteKeys, factors)) {} @@ -114,8 +114,9 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { /// @name Standard API /// @{ - /// Get factor at a given discrete assignment. - sharedFactor operator()(const DiscreteValues &assignment) const; + /// Get the factor and scalar at a given discrete assignment. + std::pair operator()( + const DiscreteValues &assignment) const; /** * @brief Combine the Gaussian Factor Graphs in `sum` and `this` while diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 900ec90c78..776f559a46 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -92,13 +92,15 @@ void HybridGaussianFactorGraph::printErrors( // Clear the stringstream ss.str(std::string()); - if (auto gmf = std::dynamic_pointer_cast(factor)) { + if (auto hgf = std::dynamic_pointer_cast(factor)) { if (factor == nullptr) { std::cout << "nullptr" << "\n"; } else { - gmf->operator()(values.discrete())->print(ss.str(), keyFormatter); - std::cout << "error = " << gmf->error(values) << std::endl; + auto [factor, val] = hgf->operator()(values.discrete()); + factor->print(ss.str(), keyFormatter); + std::cout << "value: " << val << std::endl; + std::cout << "error = " << factor->error(values) << std::endl; } } else if (auto hc = std::dynamic_pointer_cast(factor)) { if (factor == nullptr) { @@ -262,9 +264,12 @@ discreteElimination(const HybridGaussianFactorGraph &factors, // Case where we have a HybridGaussianFactor with no continuous keys. // In this case, compute discrete probabilities. auto logProbability = - [&](const GaussianFactor::shared_ptr &factor) -> double { - if (!factor) return 0.0; - return -factor->error(VectorValues()); + [&](const std::pair &fv) + -> double { + auto [factor, val] = fv; + double v = 0.5 * val * val; + if (!factor) return -v; + return -(factor->error(VectorValues()) + v); }; AlgebraicDecisionTree logProbabilities = DecisionTree(gmf->factors(), logProbability); @@ -348,7 +353,8 @@ static std::shared_ptr createHybridGaussianFactor( const KeyVector &continuousSeparator, const DiscreteKeys &discreteSeparator) { // Correct for the normalization constant used up by the conditional - auto correct = [&](const Result &pair) -> GaussianFactor::shared_ptr { + auto correct = + [&](const Result &pair) -> std::pair { const auto &[conditional, factor] = pair; if (factor) { auto hf = std::dynamic_pointer_cast(factor); @@ -357,10 +363,10 @@ static std::shared_ptr createHybridGaussianFactor( // as per the Hessian definition hf->constantTerm() += 2.0 * conditional->logNormalizationConstant(); } - return factor; + return {factor, 0.0}; }; - DecisionTree newFactors(eliminationResults, - correct); + DecisionTree> newFactors( + eliminationResults, correct); return std::make_shared(continuousSeparator, discreteSeparator, newFactors); @@ -597,10 +603,10 @@ GaussianFactorGraph HybridGaussianFactorGraph::operator()( 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 if (auto hgf = std::dynamic_pointer_cast(f)) { + gfg.push_back((*hgf)(assignment).first); + } else if (auto hgc = dynamic_pointer_cast(f)) { + gfg.push_back((*hgc)(assignment)); } else { continue; } diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 2c2810f528..6a6055bd87 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -245,10 +245,11 @@ class HybridNonlinearFactor : public HybridFactor { const Values& continuousValues) const { // functional to linearize each factor in the decision tree auto linearizeDT = - [continuousValues](const std::pair& f) { - auto [factor, val] = f; - return {factor->linearize(continuousValues), val}; - }; + [continuousValues](const std::pair& f) + -> std::pair { + auto [factor, val] = f; + return {factor->linearize(continuousValues), val}; + }; DecisionTree> linearized_factors(factors_, linearizeDT); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 0b40ecc724..7e3e2defa9 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -71,8 +71,10 @@ TEST(HybridGaussianFactor, Sum) { auto f20 = std::make_shared(X(1), A1, X(3), A3, b); auto f21 = std::make_shared(X(1), A1, X(3), A3, b); auto f22 = std::make_shared(X(1), A1, X(3), A3, b); - std::vector factorsA{f10, f11}; - std::vector factorsB{f20, f21, f22}; + std::vector> factorsA{ + {f10, 0.0}, {f11, 0.0}}; + std::vector> factorsB{ + {f20, 0.0}, {f21, 0.0}, {f22, 0.0}}; // TODO(Frank): why specify keys at all? And: keys in factor should be *all* // keys, deviating from Kevin's scheme. Should we index DT on DiscreteKey? @@ -109,7 +111,8 @@ TEST(HybridGaussianFactor, Printing) { auto b = Matrix::Zero(2, 1); auto f10 = std::make_shared(X(1), A1, X(2), A2, b); auto f11 = std::make_shared(X(1), A1, X(2), A2, b); - std::vector factors{f10, f11}; + std::vector> factors{ + {f10, 0.0}, {f11, 0.0}}; HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors); @@ -128,6 +131,7 @@ Hybrid [x1 x2; 1]{ ] b = [ 0 0 ] No noise model +value: 0 1 Leaf : A[x1] = [ @@ -140,6 +144,7 @@ Hybrid [x1 x2; 1]{ ] b = [ 0 0 ] No noise model +value: 0 } )"; @@ -178,7 +183,8 @@ TEST(HybridGaussianFactor, Error) { auto f0 = std::make_shared(X(1), A01, X(2), A02, b); auto f1 = std::make_shared(X(1), A11, X(2), A12, b); - std::vector factors{f0, f1}; + std::vector> factors{{f0, 0.0}, + {f1, 0.0}}; HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors); From f3b920257d6177b8a8975b54e10081e2d97b8cb0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 14 Sep 2024 13:52:56 -0400 Subject: [PATCH 03/30] update tests --- gtsam/hybrid/tests/Switching.h | 11 ++-- gtsam/hybrid/tests/testHybridEstimation.cpp | 10 +-- gtsam/hybrid/tests/testHybridFactorGraph.cpp | 6 +- .../tests/testHybridGaussianConditional.cpp | 2 +- .../hybrid/tests/testHybridGaussianFactor.cpp | 11 ++-- .../tests/testHybridGaussianFactorGraph.cpp | 62 +++++++++---------- .../tests/testHybridNonlinearFactorGraph.cpp | 8 +++ .../hybrid/tests/testSerializationHybrid.cpp | 2 +- 8 files changed, 61 insertions(+), 51 deletions(-) diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 851a506983..f799168f28 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -57,12 +57,15 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( // keyFunc(1) to keyFunc(n+1) for (size_t t = 1; t < n; t++) { - hfg.add(HybridGaussianFactor( - {keyFunc(t), keyFunc(t + 1)}, {{dKeyFunc(t), 2}}, + std::vector components = { {std::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), I_3x3, Z_3x1), - std::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), - I_3x3, Vector3::Ones())})); + 0.0}, + {std::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), + I_3x3, Vector3::Ones()), + 0.0}}; + hfg.add(HybridGaussianFactor({keyFunc(t), keyFunc(t + 1)}, + {{dKeyFunc(t), 2}}, components)); if (t > 1) { hfg.add(DecisionTreeFactor({{dKeyFunc(t - 1), 2}, {dKeyFunc(t), 2}}, diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 9e64b73da0..f7e1a6cd68 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -542,8 +542,10 @@ std::shared_ptr mixedVarianceFactor( double logNormalizationConstant = log(1.0 / noise_tight); double logConstant = -0.5 * d * log2pi + logNormalizationConstant; - auto func = [&](const Assignment& assignment, - const GaussianFactor::shared_ptr& gf) { + auto func = + [&](const Assignment& assignment, + const GaussianFactorValuePair& gfv) -> GaussianFactorValuePair { + auto [gf, val] = gfv; if (assignment.at(mode) != tight_index) { double factor_log_constant = -0.5 * d * log2pi + log(1.0 / noise_loose); @@ -555,9 +557,9 @@ std::shared_ptr mixedVarianceFactor( } _gfg.emplace_shared(c); - return std::make_shared(_gfg); + return {std::make_shared(_gfg), 0.0}; } else { - return dynamic_pointer_cast(gf); + return {dynamic_pointer_cast(gf), 0.0}; } }; auto updated_components = gmf->factors().apply(func); diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp index bc0ef91d77..8a2e3be3c9 100644 --- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp @@ -52,9 +52,9 @@ TEST(HybridFactorGraph, Keys) { // Add a gaussian mixture factor ϕ(x1, c1) DiscreteKey m1(M(1), 2); - DecisionTree dt( - M(1), std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())); + DecisionTree dt( + M(1), {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(1)}, {m1}, dt)); KeySet expected_continuous{X(0), X(1)}; diff --git a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp index 6156fee96d..cfa7e5a5ae 100644 --- a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp @@ -183,7 +183,7 @@ TEST(HybridGaussianConditional, Likelihood2) { // Check the detailed JacobianFactor calculation for mode==1. { // We have a JacobianFactor - const auto gf1 = (*likelihood)(assignment1); + const auto gf1 = (*likelihood)(assignment1).first; const auto jf1 = std::dynamic_pointer_cast(gf1); CHECK(jf1); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 7e3e2defa9..6e41792aa1 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -71,9 +71,8 @@ TEST(HybridGaussianFactor, Sum) { auto f20 = std::make_shared(X(1), A1, X(3), A3, b); auto f21 = std::make_shared(X(1), A1, X(3), A3, b); auto f22 = std::make_shared(X(1), A1, X(3), A3, b); - std::vector> factorsA{ - {f10, 0.0}, {f11, 0.0}}; - std::vector> factorsB{ + std::vector factorsA{{f10, 0.0}, {f11, 0.0}}; + std::vector factorsB{ {f20, 0.0}, {f21, 0.0}, {f22, 0.0}}; // TODO(Frank): why specify keys at all? And: keys in factor should be *all* @@ -111,8 +110,7 @@ TEST(HybridGaussianFactor, Printing) { auto b = Matrix::Zero(2, 1); auto f10 = std::make_shared(X(1), A1, X(2), A2, b); auto f11 = std::make_shared(X(1), A1, X(2), A2, b); - std::vector> factors{ - {f10, 0.0}, {f11, 0.0}}; + std::vector factors{{f10, 0.0}, {f11, 0.0}}; HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors); @@ -183,8 +181,7 @@ TEST(HybridGaussianFactor, Error) { auto f0 = std::make_shared(X(1), A01, X(2), A02, b); auto f1 = std::make_shared(X(1), A11, X(2), A12, b); - std::vector> factors{{f0, 0.0}, - {f1, 0.0}}; + std::vector factors{{f0, 0.0}, {f1, 0.0}}; HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index acd7b280f1..69bc0dd58a 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -127,9 +127,9 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { // Add a gaussian mixture factor ϕ(x1, c1) DiscreteKey m1(M(1), 2); - DecisionTree dt( - M(1), std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())); + DecisionTree dt( + M(1), {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(1)}, {m1}, dt)); auto result = hfg.eliminateSequential(); @@ -153,9 +153,9 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) { // Add factor between x0 and x1 hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - std::vector factors = { - std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())}; + std::vector factors = { + {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}}; hfg.add(HybridGaussianFactor({X(1)}, {m1}, factors)); // Discrete probability table for c1 @@ -178,10 +178,10 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - hfg.add(HybridGaussianFactor( - {X(1)}, {{M(1), 2}}, - {std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())})); + std::vector factors = { + {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}}; + hfg.add(HybridGaussianFactor({X(1)}, {{M(1), 2}}, factors)); hfg.add(DecisionTreeFactor(m1, {2, 8})); // TODO(Varun) Adding extra discrete variable not connected to continuous @@ -208,9 +208,9 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); // Decision tree with different modes on x1 - DecisionTree dt( - M(1), std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())); + DecisionTree dt( + M(1), {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}); // Hybrid factor P(x1|c1) hfg.add(HybridGaussianFactor({X(1)}, {m}, dt)); @@ -238,14 +238,14 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1)); { - hfg.add(HybridGaussianFactor( - {X(0)}, {{M(0), 2}}, - {std::make_shared(X(0), I_3x3, Z_3x1), - std::make_shared(X(0), I_3x3, Vector3::Ones())})); + std::vector factors = { + {std::make_shared(X(0), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(0), I_3x3, Vector3::Ones()), 0.0}}; + hfg.add(HybridGaussianFactor({X(0)}, {{M(0), 2}}, factors)); - DecisionTree dt1( - M(1), std::make_shared(X(2), I_3x3, Z_3x1), - std::make_shared(X(2), I_3x3, Vector3::Ones())); + DecisionTree dt1( + M(1), {std::make_shared(X(2), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(2), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(2)}, {{M(1), 2}}, dt1)); } @@ -256,15 +256,15 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1)); { - DecisionTree dt( - M(3), std::make_shared(X(3), I_3x3, Z_3x1), - std::make_shared(X(3), I_3x3, Vector3::Ones())); + DecisionTree dt( + M(3), {std::make_shared(X(3), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(3), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(3)}, {{M(3), 2}}, dt)); - DecisionTree dt1( - M(2), std::make_shared(X(5), I_3x3, Z_3x1), - std::make_shared(X(5), I_3x3, Vector3::Ones())); + DecisionTree dt1( + M(2), {std::make_shared(X(5), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(5), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(5)}, {{M(2), 2}}, dt1)); } @@ -552,9 +552,9 @@ TEST(HybridGaussianFactorGraph, optimize) { hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - DecisionTree dt( - C(1), std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())); + DecisionTree dt( + C(1), {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(1)}, {c1}, dt)); @@ -734,8 +734,8 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) { // Expected decision tree with two factor graphs: // f(x0;mode=0)P(x0) and f(x0;mode=1)P(x0) GaussianFactorGraphTree expected{ - M(0), GaussianFactorGraph(std::vector{(*mixture)(d0), prior}), - GaussianFactorGraph(std::vector{(*mixture)(d1), prior})}; + M(0), GaussianFactorGraph(std::vector{(*mixture)(d0).first, prior}), + GaussianFactorGraph(std::vector{(*mixture)(d1).first, prior})}; EXPECT(assert_equal(expected(d0), actual(d0), 1e-5)); EXPECT(assert_equal(expected(d1), actual(d1), 1e-5)); diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 947c24677c..4f1a6fa461 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -526,6 +526,7 @@ Hybrid [x0 x1; m0]{ ] b = [ -1 ] No noise model +value: 0 1 Leaf : A[x0] = [ @@ -536,6 +537,7 @@ Hybrid [x0 x1; m0]{ ] b = [ -0 ] No noise model +value: 0 } factor 2: @@ -551,6 +553,7 @@ Hybrid [x1 x2; m1]{ ] b = [ -1 ] No noise model +value: 0 1 Leaf : A[x1] = [ @@ -561,6 +564,7 @@ Hybrid [x1 x2; m1]{ ] b = [ -0 ] No noise model +value: 0 } factor 3: @@ -609,6 +613,7 @@ Hybrid [x0 x1; m0]{ ] b = [ -1 ] No noise model +value: 0 1 Leaf: A[x0] = [ @@ -619,6 +624,7 @@ Hybrid [x0 x1; m0]{ ] b = [ -0 ] No noise model +value: 0 } factor 2: @@ -633,6 +639,7 @@ Hybrid [x1 x2; m1]{ ] b = [ -1 ] No noise model +value: 0 1 Leaf: A[x1] = [ @@ -643,6 +650,7 @@ Hybrid [x1 x2; m1]{ ] b = [ -0 ] No noise model +value: 0 } factor 3: diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp index 365dfcc9fc..74cfa72e6b 100644 --- a/gtsam/hybrid/tests/testSerializationHybrid.cpp +++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp @@ -83,7 +83,7 @@ TEST(HybridSerialization, HybridGaussianFactor) { auto b1 = Matrix::Ones(2, 1); auto f0 = std::make_shared(X(0), A, b0); auto f1 = std::make_shared(X(0), A, b1); - std::vector factors{f0, f1}; + std::vector factors{{f0, 0.0}, {f1, 0.0}}; const HybridGaussianFactor factor(continuousKeys, discreteKeys, factors); From 5ceda1e157c96f7347902a0a0b53c4eaa0fd1010 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 14 Sep 2024 13:53:31 -0400 Subject: [PATCH 04/30] update HybridGaussianFactor to allow for tree of pairs --- gtsam/hybrid/HybridGaussianConditional.cpp | 13 +++++++---- gtsam/hybrid/HybridGaussianFactor.cpp | 27 ++++++++++------------ gtsam/hybrid/HybridGaussianFactor.h | 14 ++++++----- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 11 ++++----- gtsam/hybrid/HybridNonlinearFactor.h | 2 +- 5 files changed, 34 insertions(+), 33 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 093d50fbf0..29e1434b1d 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -222,8 +222,8 @@ std::shared_ptr HybridGaussianConditional::likelihood( const HybridGaussianFactor::Factors likelihoods( conditionals_, [&](const GaussianConditional::shared_ptr &conditional) - -> std::pair { - auto likelihood_m = conditional->likelihood(given); + -> GaussianFactorValuePair { + const auto likelihood_m = conditional->likelihood(given); const double Cgm_Kgcm = logConstant_ - conditional->logNormalizationConstant(); if (Cgm_Kgcm == 0.0) { @@ -231,8 +231,13 @@ std::shared_ptr HybridGaussianConditional::likelihood( } else { // Add a constant factor to the likelihood in case the noise models // are not all equal. - double c = std::sqrt(2.0 * Cgm_Kgcm); - return {likelihood_m, c}; + GaussianFactorGraph gfg; + gfg.push_back(likelihood_m); + Vector c(1); + c << std::sqrt(2.0 * Cgm_Kgcm); + auto constantFactor = std::make_shared(c); + gfg.push_back(constantFactor); + return {std::make_shared(gfg), 0.0}; } }); return std::make_shared( diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index 6fb453e75a..f8d85a2538 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -45,12 +45,10 @@ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { // Check the base and the factors: return Base::equals(*e, tol) && - factors_.equals(e->factors_, - [tol](const std::pair &f1, - const std::pair &f2) { - return f1.first->equals(*f2.first, tol) && - (f1.second == f2.second); - }); + factors_.equals(e->factors_, [tol](const GaussianFactorValuePair &f1, + const GaussianFactorValuePair &f2) { + return f1.first->equals(*f2.first, tol) && (f1.second == f2.second); + }); } /* *******************************************************************************/ @@ -65,7 +63,7 @@ void HybridGaussianFactor::print(const std::string &s, } else { factors_.print( "", [&](Key k) { return formatter(k); }, - [&](const std::pair &gfv) -> std::string { + [&](const GaussianFactorValuePair &gfv) -> std::string { auto [gf, val] = gfv; RedirectCout rd; std::cout << ":\n"; @@ -82,8 +80,8 @@ void HybridGaussianFactor::print(const std::string &s, } /* *******************************************************************************/ -std::pair -HybridGaussianFactor::operator()(const DiscreteValues &assignment) const { +GaussianFactorValuePair HybridGaussianFactor::operator()( + const DiscreteValues &assignment) const { return factors_(assignment); } @@ -103,7 +101,7 @@ GaussianFactorGraphTree HybridGaussianFactor::add( /* *******************************************************************************/ GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree() const { - auto wrap = [](const std::pair &gfv) { + auto wrap = [](const GaussianFactorValuePair &gfv) { return GaussianFactorGraph{gfv.first}; }; return {factors_, wrap}; @@ -113,11 +111,10 @@ GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree() AlgebraicDecisionTree HybridGaussianFactor::errorTree( const VectorValues &continuousValues) const { // functor to convert from sharedFactor to double error value. - auto errorFunc = - [&continuousValues](const std::pair &gfv) { - auto [gf, val] = gfv; - return gf->error(continuousValues) + val; - }; + auto errorFunc = [&continuousValues](const GaussianFactorValuePair &gfv) { + auto [gf, v] = gfv; + return gf->error(continuousValues) + (0.5 * v * v); + }; DecisionTree error_tree(factors_, errorFunc); return error_tree; } diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 30c35dd2b9..f8a904539f 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -33,6 +33,9 @@ class HybridValues; class DiscreteValues; class VectorValues; +/// Alias for pair of GaussianFactor::shared_pointer and a double value. +using GaussianFactorValuePair = std::pair; + /** * @brief Implementation of a discrete conditional mixture factor. * Implements a joint discrete-continuous factor where the discrete variable @@ -53,7 +56,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { using sharedFactor = std::shared_ptr; /// typedef for Decision Tree of Gaussian factors and log-constant. - using Factors = DecisionTree>; + using Factors = DecisionTree; private: /// Decision tree of Gaussian factors indexed by discrete keys. @@ -95,9 +98,9 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { * @param factors Vector of gaussian factor shared pointers * and arbitrary scalars. */ - HybridGaussianFactor( - const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, - const std::vector> &factors) + HybridGaussianFactor(const KeyVector &continuousKeys, + const DiscreteKeys &discreteKeys, + const std::vector &factors) : HybridGaussianFactor(continuousKeys, discreteKeys, Factors(discreteKeys, factors)) {} @@ -115,8 +118,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { /// @{ /// Get the factor and scalar at a given discrete assignment. - std::pair operator()( - const DiscreteValues &assignment) const; + GaussianFactorValuePair operator()(const DiscreteValues &assignment) const; /** * @brief Combine the Gaussian Factor Graphs in `sum` and `this` while diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 776f559a46..8dbf6bd2bd 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -263,9 +263,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors, } else if (auto gmf = dynamic_pointer_cast(f)) { // Case where we have a HybridGaussianFactor with no continuous keys. // In this case, compute discrete probabilities. - auto logProbability = - [&](const std::pair &fv) - -> double { + auto logProbability = [&](const GaussianFactorValuePair &fv) -> double { auto [factor, val] = fv; double v = 0.5 * val * val; if (!factor) return -v; @@ -353,8 +351,7 @@ static std::shared_ptr createHybridGaussianFactor( const KeyVector &continuousSeparator, const DiscreteKeys &discreteSeparator) { // Correct for the normalization constant used up by the conditional - auto correct = - [&](const Result &pair) -> std::pair { + auto correct = [&](const Result &pair) -> GaussianFactorValuePair { const auto &[conditional, factor] = pair; if (factor) { auto hf = std::dynamic_pointer_cast(factor); @@ -365,8 +362,8 @@ static std::shared_ptr createHybridGaussianFactor( } return {factor, 0.0}; }; - DecisionTree> newFactors( - eliminationResults, correct); + DecisionTree newFactors(eliminationResults, + correct); return std::make_shared(continuousSeparator, discreteSeparator, newFactors); diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 6a6055bd87..1bd25a6b1c 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -246,7 +246,7 @@ class HybridNonlinearFactor : public HybridFactor { // functional to linearize each factor in the decision tree auto linearizeDT = [continuousValues](const std::pair& f) - -> std::pair { + -> GaussianFactorValuePair { auto [factor, val] = f; return {factor->linearize(continuousValues), val}; }; From a7f5173b88c2470cc4543e5d3a50e4930379fd18 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 14 Sep 2024 14:17:40 -0400 Subject: [PATCH 05/30] test TwoStateModel with only differing covariances --- .../hybrid/tests/testHybridGaussianFactor.cpp | 98 ++++++++++++++++++- 1 file changed, 96 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 6e41792aa1..ce649e521a 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -526,7 +526,7 @@ TEST(HybridGaussianFactor, TwoStateModel) { /** * Test a model P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1). * - * P(f01|x1,x0,m1) has different means and different covariances. + * P(x1|x0,m1) has different means and different covariances. * * Converting to a factor graph gives us * ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1) @@ -616,13 +616,107 @@ TEST(HybridGaussianFactor, TwoStateModel2) { } } +/* ************************************************************************* */ +/** + * Test a model P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1). + * + * P(x1|x0,m1) has the same means but different covariances. + * + * Converting to a factor graph gives us + * ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1) + * + * If we only have a measurement on z0, then + * the P(m1) should be 0.5/0.5. + * Getting a measurement on z1 gives use more information. + */ +TEST(HybridGaussianFactor, TwoStateModel3) { + using namespace test_two_state_estimation; + + double mu = 1.0; + double sigma0 = 0.5, sigma1 = 2.0; + auto hybridMotionModel = CreateHybridMotionModel(mu, mu, sigma0, sigma1); + + // Start with no measurement on x1, only on x0 + const Vector1 z0(0.5); + VectorValues given; + given.insert(Z(0), z0); + + { + HybridBayesNet hbn = CreateBayesNet(hybridMotionModel); + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + + // Check that ratio of Bayes net and factor graph for different modes is + // equal for several values of {x0,x1}. + for (VectorValues vv : + {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, + VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { + vv.insert(given); // add measurements for HBN + HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}}); + EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), + gfg.error(hv1) / hbn.error(hv1), 1e-9); + } + + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + // Importance sampling run with 100k samples gives 50.095/49.905 + // approximateDiscreteMarginal(hbn, hybridMotionModel, given); + + // Since no measurement on x1, we a 50/50 probability + auto p_m = bn->at(2)->asDiscrete(); + EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 0}}), 1e-9); + EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 1}}), 1e-9); + } + + { + // Now we add a measurement z1 on x1 + const Vector1 z1(4.0); // favors m==1 + given.insert(Z(1), z1); + + HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + + // Check that ratio of Bayes net and factor graph for different modes is + // equal for several values of {x0,x1}. + for (VectorValues vv : + {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, + VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { + vv.insert(given); // add measurements for HBN + HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}}); + EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), + gfg.error(hv1) / hbn.error(hv1), 1e-9); + } + + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + // Values taken from an importance sampling run with 100k samples: + // approximateDiscreteMarginal(hbn, hybridMotionModel, given); + DiscreteConditional expected(m1, "51.7762/48.2238"); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); + } + + { + // Add a different measurement z1 on x1 that favors m==1 + const Vector1 z1(7.0); + given.insert_or_assign(Z(1), z1); + + HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + // Values taken from an importance sampling run with 100k samples: + // approximateDiscreteMarginal(hbn, hybridMotionModel, given); + DiscreteConditional expected(m1, "49.0762/50.9238"); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.005)); + } +} + /* ************************************************************************* */ /** * Same model, P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1), but now with very informative * measurements and vastly different motion model: either stand still or move * far. This yields a very informative posterior. */ -TEST(HybridGaussianFactor, TwoStateModel3) { +TEST(HybridGaussianFactor, TwoStateModel4) { using namespace test_two_state_estimation; double mu0 = 0.0, mu1 = 10.0; From 9360165ef6ffaf5bd106b15fe2938256fab4e854 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 14 Sep 2024 14:54:36 -0400 Subject: [PATCH 06/30] update HybridGaussianFactor to leverage constant hiding for the Tree of Pairs --- gtsam/hybrid/HybridGaussianFactor.cpp | 90 +++++++++++++++++++++------ gtsam/hybrid/HybridGaussianFactor.h | 24 +++++-- 2 files changed, 91 insertions(+), 23 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index f8d85a2538..f38dd6b841 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -28,11 +28,54 @@ namespace gtsam { +/** + * @brief Helper function to augment the [A|b] matrices in the factor components + * with the normalizer values. + * This is done by storing the normalizer value in + * the `b` vector as an additional row. + * + * @param factors DecisionTree of GaussianFactors and arbitrary scalars. + * Gaussian factor in factors. + * @return HybridGaussianFactor::Factors + */ +HybridGaussianFactor::Factors augment( + const HybridGaussianFactor::FactorValuePairs &factors) { + // Find the minimum value so we can "proselytize" to positive values. + // Done because we can't have sqrt of negative numbers. + auto unzipped_pair = unzip(factors); + const HybridGaussianFactor::Factors gaussianFactors = unzipped_pair.first; + const AlgebraicDecisionTree valueTree = unzipped_pair.second; + double min_value = valueTree.min(); + AlgebraicDecisionTree values = + valueTree.apply([&min_value](double n) { return n - min_value; }); + + // Finally, update the [A|b] matrices. + auto update = [&values](const Assignment &assignment, + const HybridGaussianFactor::sharedFactor &gf) { + auto jf = std::dynamic_pointer_cast(gf); + if (!jf) return gf; + // If the log_normalizer is 0, do nothing + if (values(assignment) == 0.0) return gf; + + GaussianFactorGraph gfg; + gfg.push_back(jf); + + Vector c(1); + c << std::sqrt(values(assignment)); + auto constantFactor = std::make_shared(c); + + gfg.push_back(constantFactor); + return std::dynamic_pointer_cast( + std::make_shared(gfg)); + }; + return gaussianFactors.apply(update); +} + /* *******************************************************************************/ HybridGaussianFactor::HybridGaussianFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, - const Factors &factors) - : Base(continuousKeys, discreteKeys), factors_(factors) {} + const FactorValuePairs &factors) + : Base(continuousKeys, discreteKeys), factors_(augment(factors)) {} /* *******************************************************************************/ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { @@ -45,10 +88,10 @@ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { // Check the base and the factors: return Base::equals(*e, tol) && - factors_.equals(e->factors_, [tol](const GaussianFactorValuePair &f1, - const GaussianFactorValuePair &f2) { - return f1.first->equals(*f2.first, tol) && (f1.second == f2.second); - }); + factors_.equals(e->factors_, + [tol](const sharedFactor &f1, const sharedFactor &f2) { + return f1->equals(*f2, tol); + }); } /* *******************************************************************************/ @@ -63,13 +106,11 @@ void HybridGaussianFactor::print(const std::string &s, } else { factors_.print( "", [&](Key k) { return formatter(k); }, - [&](const GaussianFactorValuePair &gfv) -> std::string { - auto [gf, val] = gfv; + [&](const sharedFactor &gf) -> std::string { RedirectCout rd; std::cout << ":\n"; if (gf) { gf->print("", formatter); - std::cout << "value: " << val << std::endl; return rd.str(); } else { return "nullptr"; @@ -80,7 +121,7 @@ void HybridGaussianFactor::print(const std::string &s, } /* *******************************************************************************/ -GaussianFactorValuePair HybridGaussianFactor::operator()( +HybridGaussianFactor::sharedFactor HybridGaussianFactor::operator()( const DiscreteValues &assignment) const { return factors_(assignment); } @@ -101,9 +142,7 @@ GaussianFactorGraphTree HybridGaussianFactor::add( /* *******************************************************************************/ GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree() const { - auto wrap = [](const GaussianFactorValuePair &gfv) { - return GaussianFactorGraph{gfv.first}; - }; + auto wrap = [](const sharedFactor &gf) { return GaussianFactorGraph{gf}; }; return {factors_, wrap}; } @@ -111,9 +150,8 @@ GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree() AlgebraicDecisionTree HybridGaussianFactor::errorTree( const VectorValues &continuousValues) const { // functor to convert from sharedFactor to double error value. - auto errorFunc = [&continuousValues](const GaussianFactorValuePair &gfv) { - auto [gf, v] = gfv; - return gf->error(continuousValues) + (0.5 * v * v); + auto errorFunc = [&continuousValues](const sharedFactor &gf) { + return gf->error(continuousValues); }; DecisionTree error_tree(factors_, errorFunc); return error_tree; @@ -121,8 +159,24 @@ AlgebraicDecisionTree HybridGaussianFactor::errorTree( /* *******************************************************************************/ double HybridGaussianFactor::error(const HybridValues &values) const { - auto &&[gf, val] = factors_(values.discrete()); - return gf->error(values.continuous()) + val; + const sharedFactor gf = factors_(values.discrete()); + return gf->error(values.continuous()); +} + +/* *******************************************************************************/ +double ComputeLogNormalizer( + const noiseModel::Gaussian::shared_ptr &noise_model) { + // Since noise models are Gaussian, we can get the logDeterminant using + // the same trick as in GaussianConditional + double logDetR = noise_model->R() + .diagonal() + .unaryExpr([](double x) { return log(x); }) + .sum(); + double logDeterminantSigma = -2.0 * logDetR; + + size_t n = noise_model->dim(); + constexpr double log2pi = 1.8378770664093454835606594728112; + return n * log2pi + logDeterminantSigma; } } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index f8a904539f..e40713f46d 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -55,8 +55,10 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { using sharedFactor = std::shared_ptr; - /// typedef for Decision Tree of Gaussian factors and log-constant. - using Factors = DecisionTree; + /// typedef for Decision Tree of Gaussian factors and arbitrary value. + using FactorValuePairs = DecisionTree; + /// typedef for Decision Tree of Gaussian factors. + using Factors = DecisionTree; private: /// Decision tree of Gaussian factors indexed by discrete keys. @@ -87,7 +89,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { */ HybridGaussianFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, - const Factors &factors); + const FactorValuePairs &factors); /** * @brief Construct a new HybridGaussianFactor object using a vector of @@ -102,7 +104,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { const DiscreteKeys &discreteKeys, const std::vector &factors) : HybridGaussianFactor(continuousKeys, discreteKeys, - Factors(discreteKeys, factors)) {} + FactorValuePairs(discreteKeys, factors)) {} /// @} /// @name Testable @@ -118,7 +120,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { /// @{ /// Get the factor and scalar at a given discrete assignment. - GaussianFactorValuePair operator()(const DiscreteValues &assignment) const; + sharedFactor operator()(const DiscreteValues &assignment) const; /** * @brief Combine the Gaussian Factor Graphs in `sum` and `this` while @@ -173,4 +175,16 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { template <> struct traits : public Testable {}; +/** + * @brief Helper function to compute the sqrt(|2πΣ|) normalizer values + * for a Gaussian noise model. + * We compute this in the log-space for numerical accuracy. + * + * @param noise_model The Gaussian noise model + * whose normalizer we wish to compute. + * @return double + */ +GTSAM_EXPORT double ComputeLogNormalizer( + const noiseModel::Gaussian::shared_ptr &noise_model); + } // namespace gtsam From 0ee9aac434b82e8ba404ef4360e34b9e3706d558 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 14 Sep 2024 14:54:49 -0400 Subject: [PATCH 07/30] update other classes with correct types --- gtsam/hybrid/HybridGaussianConditional.cpp | 2 +- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 15 ++++++--------- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 29e1434b1d..eeeed7d13c 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -219,7 +219,7 @@ std::shared_ptr HybridGaussianConditional::likelihood( const DiscreteKeys discreteParentKeys = discreteKeys(); const KeyVector continuousParentKeys = continuousParents(); - const HybridGaussianFactor::Factors likelihoods( + const HybridGaussianFactor::FactorValuePairs likelihoods( conditionals_, [&](const GaussianConditional::shared_ptr &conditional) -> GaussianFactorValuePair { diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 8dbf6bd2bd..74091bf956 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -97,9 +97,7 @@ void HybridGaussianFactorGraph::printErrors( std::cout << "nullptr" << "\n"; } else { - auto [factor, val] = hgf->operator()(values.discrete()); - factor->print(ss.str(), keyFormatter); - std::cout << "value: " << val << std::endl; + hgf->operator()(values.discrete())->print(ss.str(), keyFormatter); std::cout << "error = " << factor->error(values) << std::endl; } } else if (auto hc = std::dynamic_pointer_cast(factor)) { @@ -263,11 +261,10 @@ discreteElimination(const HybridGaussianFactorGraph &factors, } else if (auto gmf = dynamic_pointer_cast(f)) { // Case where we have a HybridGaussianFactor with no continuous keys. // In this case, compute discrete probabilities. - auto logProbability = [&](const GaussianFactorValuePair &fv) -> double { - auto [factor, val] = fv; - double v = 0.5 * val * val; - if (!factor) return -v; - return -(factor->error(VectorValues()) + v); + auto logProbability = + [&](const GaussianFactor::shared_ptr &factor) -> double { + if (!factor) return 0.0; + return -factor->error(VectorValues()); }; AlgebraicDecisionTree logProbabilities = DecisionTree(gmf->factors(), logProbability); @@ -601,7 +598,7 @@ GaussianFactorGraph HybridGaussianFactorGraph::operator()( } else if (auto gc = std::dynamic_pointer_cast(f)) { gfg.push_back(gf); } else if (auto hgf = std::dynamic_pointer_cast(f)) { - gfg.push_back((*hgf)(assignment).first); + gfg.push_back((*hgf)(assignment)); } else if (auto hgc = dynamic_pointer_cast(f)) { gfg.push_back((*hgc)(assignment)); } else { From d5160f70060fa745d678e557c56fa3a331607522 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 14 Sep 2024 14:57:55 -0400 Subject: [PATCH 08/30] update tests --- gtsam/hybrid/tests/testHybridEstimation.cpp | 3 +-- gtsam/hybrid/tests/testHybridGaussianConditional.cpp | 2 +- gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp | 4 ++-- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index f7e1a6cd68..8f9875a9a4 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -544,8 +544,7 @@ std::shared_ptr mixedVarianceFactor( auto func = [&](const Assignment& assignment, - const GaussianFactorValuePair& gfv) -> GaussianFactorValuePair { - auto [gf, val] = gfv; + const GaussianFactor::shared_ptr& gf) -> GaussianFactorValuePair { if (assignment.at(mode) != tight_index) { double factor_log_constant = -0.5 * d * log2pi + log(1.0 / noise_loose); diff --git a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp index cfa7e5a5ae..6156fee96d 100644 --- a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp @@ -183,7 +183,7 @@ TEST(HybridGaussianConditional, Likelihood2) { // Check the detailed JacobianFactor calculation for mode==1. { // We have a JacobianFactor - const auto gf1 = (*likelihood)(assignment1).first; + const auto gf1 = (*likelihood)(assignment1); const auto jf1 = std::dynamic_pointer_cast(gf1); CHECK(jf1); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 69bc0dd58a..122f905fff 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -734,8 +734,8 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) { // Expected decision tree with two factor graphs: // f(x0;mode=0)P(x0) and f(x0;mode=1)P(x0) GaussianFactorGraphTree expected{ - M(0), GaussianFactorGraph(std::vector{(*mixture)(d0).first, prior}), - GaussianFactorGraph(std::vector{(*mixture)(d1).first, prior})}; + M(0), GaussianFactorGraph(std::vector{(*mixture)(d0), prior}), + GaussianFactorGraph(std::vector{(*mixture)(d1), prior})}; EXPECT(assert_equal(expected(d0), actual(d0), 1e-5)); EXPECT(assert_equal(expected(d1), actual(d1), 1e-5)); From df7850494cd516c06f0e170eea56b27507a7f350 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 14 Sep 2024 15:07:27 -0400 Subject: [PATCH 09/30] fix testHybridEstimation test --- gtsam/hybrid/tests/testHybridEstimation.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 8f9875a9a4..76e5880eee 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -542,9 +542,8 @@ std::shared_ptr mixedVarianceFactor( double logNormalizationConstant = log(1.0 / noise_tight); double logConstant = -0.5 * d * log2pi + logNormalizationConstant; - auto func = - [&](const Assignment& assignment, - const GaussianFactor::shared_ptr& gf) -> GaussianFactorValuePair { + auto func = [&](const Assignment& assignment, + const GaussianFactor::shared_ptr& gf) { if (assignment.at(mode) != tight_index) { double factor_log_constant = -0.5 * d * log2pi + log(1.0 / noise_loose); @@ -556,14 +555,19 @@ std::shared_ptr mixedVarianceFactor( } _gfg.emplace_shared(c); - return {std::make_shared(_gfg), 0.0}; + return std::make_shared(_gfg); } else { - return {dynamic_pointer_cast(gf), 0.0}; + return dynamic_pointer_cast(gf); } }; auto updated_components = gmf->factors().apply(func); + auto updated_pairs = HybridGaussianFactor::FactorValuePairs( + updated_components, + [](const GaussianFactor::shared_ptr& gf) -> GaussianFactorValuePair { + return {gf, 0.0}; + }); auto updated_gmf = std::make_shared( - gmf->continuousKeys(), gmf->discreteKeys(), updated_components); + gmf->continuousKeys(), gmf->discreteKeys(), updated_pairs); return updated_gmf; } From cfe2ad56bfb7ab377231ac4c47a10dd80d65887a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 14 Sep 2024 15:09:17 -0400 Subject: [PATCH 10/30] fix printing tests --- gtsam/hybrid/tests/testHybridGaussianFactor.cpp | 2 -- gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 10 +--------- 2 files changed, 1 insertion(+), 11 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index ce649e521a..8837e84a7c 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -129,7 +129,6 @@ Hybrid [x1 x2; 1]{ ] b = [ 0 0 ] No noise model -value: 0 1 Leaf : A[x1] = [ @@ -142,7 +141,6 @@ value: 0 ] b = [ 0 0 ] No noise model -value: 0 } )"; diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 4f1a6fa461..fbf6983d2c 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -442,7 +442,7 @@ TEST(HybridFactorGraph, Full_Elimination) { DiscreteFactorGraph discrete_fg; // TODO(Varun) Make this a function of HybridGaussianFactorGraph? - for (auto& factor : (*remainingFactorGraph_partial)) { + for (auto &factor : (*remainingFactorGraph_partial)) { auto df = dynamic_pointer_cast(factor); assert(df); discrete_fg.push_back(df); @@ -526,7 +526,6 @@ Hybrid [x0 x1; m0]{ ] b = [ -1 ] No noise model -value: 0 1 Leaf : A[x0] = [ @@ -537,7 +536,6 @@ value: 0 ] b = [ -0 ] No noise model -value: 0 } factor 2: @@ -553,7 +551,6 @@ Hybrid [x1 x2; m1]{ ] b = [ -1 ] No noise model -value: 0 1 Leaf : A[x1] = [ @@ -564,7 +561,6 @@ value: 0 ] b = [ -0 ] No noise model -value: 0 } factor 3: @@ -613,7 +609,6 @@ Hybrid [x0 x1; m0]{ ] b = [ -1 ] No noise model -value: 0 1 Leaf: A[x0] = [ @@ -624,7 +619,6 @@ value: 0 ] b = [ -0 ] No noise model -value: 0 } factor 2: @@ -639,7 +633,6 @@ Hybrid [x1 x2; m1]{ ] b = [ -1 ] No noise model -value: 0 1 Leaf: A[x1] = [ @@ -650,7 +643,6 @@ value: 0 ] b = [ -0 ] No noise model -value: 0 } factor 3: From 48e087e083842dd3dcbe8fad1a003849e7c4cb47 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 14 Sep 2024 15:14:02 -0400 Subject: [PATCH 11/30] fix error using value for HybridNonlinearFactor --- gtsam/hybrid/HybridGaussianFactor.h | 2 +- gtsam/hybrid/HybridNonlinearFactor.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index e40713f46d..f52022348b 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -119,7 +119,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { /// @name Standard API /// @{ - /// Get the factor and scalar at a given discrete assignment. + /// Get factor at a given discrete assignment. sharedFactor operator()(const DiscreteValues &assignment) const; /** diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 1bd25a6b1c..c3f127f4a8 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -138,7 +138,7 @@ class HybridNonlinearFactor : public HybridFactor { auto errorFunc = [continuousValues](const std::pair& f) { auto [factor, val] = f; - return factor->error(continuousValues) + val; + return factor->error(continuousValues) + (0.5 * val * val); }; DecisionTree result(factors_, errorFunc); return result; @@ -157,7 +157,7 @@ class HybridNonlinearFactor : public HybridFactor { auto [factor, val] = factors_(discreteValues); // Compute the error for the selected factor const double factorError = factor->error(continuousValues); - return factorError + val; + return factorError + (0.5 * val * val); } /** From 43e6bc6462c71845e085833adba1389857c07936 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 14 Sep 2024 15:15:52 -0400 Subject: [PATCH 12/30] cleaner specific factor linearization --- gtsam/hybrid/HybridNonlinearFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index c3f127f4a8..6cfce6bc03 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -236,7 +236,7 @@ class HybridNonlinearFactor : public HybridFactor { GaussianFactor::shared_ptr linearize( const Values& continuousValues, const DiscreteValues& discreteValues) const { - auto [factor, val] = factors_(discreteValues); + auto factor = factors_(discreteValues).first; return factor->linearize(continuousValues); } From a9013aad8e4df92a3d15ff1c01bb559dc2715391 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Sep 2024 08:51:32 -0400 Subject: [PATCH 13/30] HybridNonlinearFactorGraph tests --- .../tests/testHybridNonlinearFactorGraph.cpp | 167 +++++++++++++++++- 1 file changed, 166 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index fbf6983d2c..16098af7e8 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -841,9 +841,174 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { EXPECT_LONGS_EQUAL(1, remainingFactorGraph->size()); } +namespace test_relinearization { +/** + * @brief Create a Factor Graph by directly specifying all + * the factors instead of creating conditionals first. + * This way we can directly provide the likelihoods and + * then perform (re-)linearization. + * + * @param means The means of the GaussianMixtureFactor components. + * @param sigmas The covariances of the GaussianMixtureFactor components. + * @param m1 The discrete key. + * @param x0_measurement A measurement on X0 + * @return HybridGaussianFactorGraph + */ +static HybridNonlinearFactorGraph CreateFactorGraph( + const std::vector &means, const std::vector &sigmas, + DiscreteKey &m1, double x0_measurement) { + auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]); + auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]); + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); + + auto f0 = + std::make_shared>(X(0), X(1), means[0], model0); + auto f1 = + std::make_shared>(X(0), X(1), means[1], model1); + + // Create HybridNonlinearFactor + std::vector> factors{ + {f0, ComputeLogNormalizer(model0)}, {f1, ComputeLogNormalizer(model1)}}; + + HybridNonlinearFactor mixtureFactor({X(0), X(1)}, {m1}, factors); + + HybridNonlinearFactorGraph hfg; + hfg.push_back(mixtureFactor); + + hfg.push_back(PriorFactor(X(0), x0_measurement, prior_noise)); + + return hfg; +} +} // namespace test_relinearization + +/* ************************************************************************* */ +/** + * @brief Test components with differing means but the same covariances. + * The factor graph is + * *-X1-*-X2 + * | + * M1 + */ +TEST(HybridNonlinearFactorGraph, DifferentMeans) { + using namespace test_relinearization; + + DiscreteKey m1(M(1), 2); + + Values values; + double x0 = 0.0, x1 = 1.75; + values.insert(X(0), x0); + values.insert(X(1), x1); + + std::vector means = {0.0, 2.0}, sigmas = {1e-0, 1e-0}; + + HybridNonlinearFactorGraph hfg = CreateFactorGraph(means, sigmas, m1, x0); + + { + auto bn = hfg.linearize(values)->eliminateSequential(); + HybridValues actual = bn->optimize(); + + HybridValues expected( + VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(-1.75)}}, + DiscreteValues{{M(1), 0}}); + + EXPECT(assert_equal(expected, actual)); + + DiscreteValues dv0{{M(1), 0}}; + VectorValues cont0 = bn->optimize(dv0); + double error0 = bn->error(HybridValues(cont0, dv0)); + + // TODO(Varun) Perform importance sampling to estimate error? + + // regression + EXPECT_DOUBLES_EQUAL(0.69314718056, error0, 1e-9); + + DiscreteValues dv1{{M(1), 1}}; + VectorValues cont1 = bn->optimize(dv1); + double error1 = bn->error(HybridValues(cont1, dv1)); + EXPECT_DOUBLES_EQUAL(error0, error1, 1e-9); + } + + { + // Add measurement on x1 + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); + hfg.push_back(PriorFactor(X(1), means[1], prior_noise)); + + auto bn = hfg.linearize(values)->eliminateSequential(); + HybridValues actual = bn->optimize(); + + HybridValues expected( + VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(0.25)}}, + DiscreteValues{{M(1), 1}}); + + EXPECT(assert_equal(expected, actual)); + + { + DiscreteValues dv{{M(1), 0}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(2.12692448787, error, 1e-9); + } + { + DiscreteValues dv{{M(1), 1}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(0.126928487854, error, 1e-9); + } + } +} + /* ************************************************************************* */ +/** + * @brief Test components with differing covariances but the same means. + * The factor graph is + * *-X1-*-X2 + * | + * M1 + */ +TEST_DISABLED(HybridNonlinearFactorGraph, DifferentCovariances) { + using namespace test_relinearization; + + DiscreteKey m1(M(1), 2); + + Values values; + double x0 = 1.0, x1 = 1.0; + values.insert(X(0), x0); + values.insert(X(1), x1); + + std::vector means = {0.0, 0.0}, sigmas = {1e2, 1e-2}; + + // Create FG with HybridNonlinearFactor and prior on X1 + HybridNonlinearFactorGraph hfg = CreateFactorGraph(means, sigmas, m1, x0); + // Linearize and eliminate + auto hbn = hfg.linearize(values)->eliminateSequential(); + + VectorValues cv; + cv.insert(X(0), Vector1(0.0)); + cv.insert(X(1), Vector1(0.0)); + + // Check that the error values at the MLE point μ. + AlgebraicDecisionTree errorTree = hbn->errorTree(cv); + + DiscreteValues dv0{{M(1), 0}}; + DiscreteValues dv1{{M(1), 1}}; + + // regression + EXPECT_DOUBLES_EQUAL(9.90348755254, errorTree(dv0), 1e-9); + EXPECT_DOUBLES_EQUAL(0.69314718056, errorTree(dv1), 1e-9); + + DiscreteConditional expected_m1(m1, "0.5/0.5"); + DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete()); + + EXPECT(assert_equal(expected_m1, actual_m1)); +} + +/* ************************************************************************* + */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ +/* ************************************************************************* + */ From e2f1ad78a0129420b7bede7e8014ba58fefb308f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Sep 2024 08:55:36 -0400 Subject: [PATCH 14/30] move ComputeLogNormalizer to NoiseModel.h --- gtsam/hybrid/HybridGaussianFactor.cpp | 16 ---------------- gtsam/hybrid/HybridGaussianFactor.h | 12 ------------ gtsam/linear/NoiseModel.cpp | 16 ++++++++++++++++ gtsam/linear/NoiseModel.h | 12 ++++++++++++ 4 files changed, 28 insertions(+), 28 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index f38dd6b841..bfebb064e1 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -163,20 +163,4 @@ double HybridGaussianFactor::error(const HybridValues &values) const { return gf->error(values.continuous()); } -/* *******************************************************************************/ -double ComputeLogNormalizer( - const noiseModel::Gaussian::shared_ptr &noise_model) { - // Since noise models are Gaussian, we can get the logDeterminant using - // the same trick as in GaussianConditional - double logDetR = noise_model->R() - .diagonal() - .unaryExpr([](double x) { return log(x); }) - .sum(); - double logDeterminantSigma = -2.0 * logDetR; - - size_t n = noise_model->dim(); - constexpr double log2pi = 1.8378770664093454835606594728112; - return n * log2pi + logDeterminantSigma; -} - } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index f52022348b..333c5d25bb 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -175,16 +175,4 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { template <> struct traits : public Testable {}; -/** - * @brief Helper function to compute the sqrt(|2πΣ|) normalizer values - * for a Gaussian noise model. - * We compute this in the log-space for numerical accuracy. - * - * @param noise_model The Gaussian noise model - * whose normalizer we wish to compute. - * @return double - */ -GTSAM_EXPORT double ComputeLogNormalizer( - const noiseModel::Gaussian::shared_ptr &noise_model); - } // namespace gtsam diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 7ea8e5a546..4936161f2a 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -706,6 +706,22 @@ const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){ return shared_ptr(new Robust(robust,noise)); } +/* *******************************************************************************/ +double ComputeLogNormalizer( + const noiseModel::Gaussian::shared_ptr& noise_model) { + // Since noise models are Gaussian, we can get the logDeterminant using + // the same trick as in GaussianConditional + double logDetR = noise_model->R() + .diagonal() + .unaryExpr([](double x) { return log(x); }) + .sum(); + double logDeterminantSigma = -2.0 * logDetR; + + size_t n = noise_model->dim(); + constexpr double log2pi = 1.8378770664093454835606594728112; + return n * log2pi + logDeterminantSigma; +} + /* ************************************************************************* */ } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 489f11e4c2..831cfd7ddf 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -751,6 +751,18 @@ namespace gtsam { template<> struct traits : public Testable {}; template<> struct traits : public Testable {}; + /** + * @brief Helper function to compute the sqrt(|2πΣ|) normalizer values + * for a Gaussian noise model. + * We compute this in the log-space for numerical accuracy. + * + * @param noise_model The Gaussian noise model + * whose normalizer we wish to compute. + * @return double + */ + GTSAM_EXPORT double ComputeLogNormalizer( + const noiseModel::Gaussian::shared_ptr& noise_model); + } //\ namespace gtsam From 3eb91a4ba06cf0bf3a0d1a4af40f210d64b13bca Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Sep 2024 09:16:31 -0400 Subject: [PATCH 15/30] address some PR comments --- gtsam/hybrid/HybridNonlinearFactor.h | 2 +- gtsam/hybrid/tests/testHybridGaussianFactor.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 6cfce6bc03..d8e3990f0e 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -110,7 +110,7 @@ class HybridNonlinearFactor : public HybridFactor { std::inserter(factor_keys_set, factor_keys_set.end())); if (auto nf = std::dynamic_pointer_cast(f)) { - nonlinear_factors.push_back(std::make_pair(nf, val)); + nonlinear_factors.emplace_back(nf, val); } else { throw std::runtime_error( "Factors passed into HybridNonlinearFactor need to be nonlinear!"); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 8837e84a7c..11226e19d1 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -616,15 +616,15 @@ TEST(HybridGaussianFactor, TwoStateModel2) { /* ************************************************************************* */ /** - * Test a model P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1). + * Test a model p(z0|x0)p(x1|x0,m1)p(z1|x1)p(m1). * - * P(x1|x0,m1) has the same means but different covariances. + * p(x1|x0,m1) has the same means but different covariances. * * Converting to a factor graph gives us - * ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1) + * ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)p(m1) * * If we only have a measurement on z0, then - * the P(m1) should be 0.5/0.5. + * the p(m1) should be 0.5/0.5. * Getting a measurement on z1 gives use more information. */ TEST(HybridGaussianFactor, TwoStateModel3) { From c2dc1fcdb2946dec0ba0aff537c2c6694912ef6e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Sep 2024 09:16:51 -0400 Subject: [PATCH 16/30] NonlinearFactorValuePair typedef --- gtsam/hybrid/HybridNonlinearFactor.h | 8 +++++--- gtsam/hybrid/tests/Switching.h | 2 +- gtsam/hybrid/tests/testHybridBayesNet.cpp | 4 ++-- gtsam/hybrid/tests/testHybridEstimation.cpp | 12 ++++++------ gtsam/hybrid/tests/testHybridNonlinearFactor.cpp | 6 ++---- .../hybrid/tests/testHybridNonlinearFactorGraph.cpp | 3 ++- 6 files changed, 18 insertions(+), 17 deletions(-) diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index d8e3990f0e..742b4c1625 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -33,6 +33,9 @@ namespace gtsam { +/// Alias for a NonlinearFactor shared pointer and double scalar pair. +using NonlinearFactorValuePair = std::pair; + /** * @brief Implementation of a discrete conditional mixture factor. * @@ -55,7 +58,7 @@ class HybridNonlinearFactor : public HybridFactor { * @brief typedef for DecisionTree which has Keys as node labels and * pairs of NonlinearFactor & an arbitrary scalar as leaf nodes. */ - using Factors = DecisionTree>; + using Factors = DecisionTree; private: /// Decision tree of Gaussian factors indexed by discrete keys. @@ -100,8 +103,7 @@ class HybridNonlinearFactor : public HybridFactor { const std::vector, double>>& factors, bool normalized = false) : Base(keys, discreteKeys), normalized_(normalized) { - std::vector> - nonlinear_factors; + std::vector nonlinear_factors; KeySet continuous_keys_set(keys.begin(), keys.end()); KeySet factor_keys_set; for (auto&& [f, val] : factors) { diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index f799168f28..9b7c44d02e 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -162,7 +162,7 @@ struct Switching { for (size_t k = 0; k < K - 1; k++) { KeyVector keys = {X(k), X(k + 1)}; auto motion_models = motionModels(k, between_sigma); - std::vector> components; + std::vector components; for (auto &&f : motion_models) { components.push_back( {std::dynamic_pointer_cast(f), 0.0}); diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index bf3301d2fe..cf4231dba2 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -387,8 +387,8 @@ TEST(HybridBayesNet, Sampling) { std::make_shared>(X(0), X(1), 0, noise_model); auto one_motion = std::make_shared>(X(0), X(1), 1, noise_model); - std::vector> factors = { - {zero_motion, 0.0}, {one_motion, 0.0}}; + std::vector factors = {{zero_motion, 0.0}, + {one_motion, 0.0}}; nfg.emplace_shared>(X(0), 0.0, noise_model); nfg.emplace_shared( KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 76e5880eee..7204d819d9 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -435,8 +435,8 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() { std::make_shared>(X(0), X(1), 0, noise_model); const auto one_motion = std::make_shared>(X(0), X(1), 1, noise_model); - std::vector> components = { - {zero_motion, 0.0}, {one_motion, 0.0}}; + std::vector components = {{zero_motion, 0.0}, + {one_motion, 0.0}}; nfg.emplace_shared(KeyVector{X(0), X(1)}, DiscreteKeys{m}, components); @@ -595,8 +595,8 @@ TEST(HybridEstimation, ModeSelection) { model1 = std::make_shared( X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight)); - std::vector> components = { - {model0, 0.0}, {model1, 0.0}}; + std::vector components = {{model0, 0.0}, + {model1, 0.0}}; KeyVector keys = {X(0), X(1)}; HybridNonlinearFactor mf(keys, modes, components); @@ -687,8 +687,8 @@ TEST(HybridEstimation, ModeSelection2) { model1 = std::make_shared>( X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight)); - std::vector> components = { - {model0, 0.0}, {model1, 0.0}}; + std::vector components = {{model0, 0.0}, + {model1, 0.0}}; KeyVector keys = {X(0), X(1)}; HybridNonlinearFactor mf(keys, modes, components); diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp index fa31fbe7db..a4c1b4cc7e 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp @@ -58,8 +58,7 @@ TEST(HybridNonlinearFactor, Printing) { std::make_shared>(X(1), X(2), between0, model); auto f1 = std::make_shared>(X(1), X(2), between1, model); - std::vector> factors{ - {f0, 0.0}, {f1, 0.0}}; + std::vector factors{{f0, 0.0}, {f1, 0.0}}; HybridNonlinearFactor mixtureFactor({X(1), X(2)}, {m1}, factors); @@ -87,8 +86,7 @@ static HybridNonlinearFactor getHybridNonlinearFactor() { std::make_shared>(X(1), X(2), between0, model); auto f1 = std::make_shared>(X(1), X(2), between1, model); - std::vector> factors{ - {f0, 0.0}, {f1, 0.0}}; + std::vector factors{{f0, 0.0}, {f1, 0.0}}; return HybridNonlinearFactor({X(1), X(2)}, {m1}, factors); } diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 16098af7e8..00d0d3a39a 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -867,7 +868,7 @@ static HybridNonlinearFactorGraph CreateFactorGraph( std::make_shared>(X(0), X(1), means[1], model1); // Create HybridNonlinearFactor - std::vector> factors{ + std::vector factors{ {f0, ComputeLogNormalizer(model0)}, {f1, ComputeLogNormalizer(model1)}}; HybridNonlinearFactor mixtureFactor({X(0), X(1)}, {m1}, factors); From ccea10410debc654bb49fd3d5f09cc3a788bb28b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Sep 2024 09:17:05 -0400 Subject: [PATCH 17/30] put ComputeLogNormalizer in the correct namespace --- gtsam/linear/NoiseModel.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 4936161f2a..29222a1066 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -706,6 +706,9 @@ const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){ return shared_ptr(new Robust(robust,noise)); } +/* ************************************************************************* */ +} // namespace noiseModel + /* *******************************************************************************/ double ComputeLogNormalizer( const noiseModel::Gaussian::shared_ptr& noise_model) { @@ -722,7 +725,4 @@ double ComputeLogNormalizer( return n * log2pi + logDeterminantSigma; } -/* ************************************************************************* */ - -} } // gtsam From 6b9fb5b22f4c4aee2113840d67a7a1b71990f764 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Sep 2024 09:17:51 -0400 Subject: [PATCH 18/30] fix python wrapper --- gtsam/hybrid/hybrid.i | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 686ce60ac2..dcf0fce042 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -77,7 +77,8 @@ class HybridGaussianFactor : gtsam::HybridFactor { HybridGaussianFactor( const gtsam::KeyVector& continuousKeys, const gtsam::DiscreteKeys& discreteKeys, - const std::vector& factorsList); + const std::vector>& + factorsList); void print(string s = "HybridGaussianFactor\n", const gtsam::KeyFormatter& keyFormatter = @@ -242,14 +243,14 @@ class HybridNonlinearFactorGraph { class HybridNonlinearFactor : gtsam::HybridFactor { HybridNonlinearFactor( const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, - const gtsam::DecisionTree& factors, + const gtsam::DecisionTree< + gtsam::Key, std::pair>& factors, bool normalized = false); - template - HybridNonlinearFactor(const gtsam::KeyVector& keys, - const gtsam::DiscreteKeys& discreteKeys, - const std::vector& factors, - bool normalized = false); + HybridNonlinearFactor( + const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, + const std::vector>& factors, + bool normalized = false); double error(const gtsam::Values& continuousValues, const gtsam::DiscreteValues& discreteValues) const; From 1b7435361e85450a4d4339b7b9869cc74cdc2fad Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Sep 2024 09:30:59 -0400 Subject: [PATCH 19/30] leverage hiding inside HybridGaussianFactor --- gtsam/hybrid/HybridGaussianConditional.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index eeeed7d13c..2b2062e6f4 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -229,15 +229,10 @@ std::shared_ptr HybridGaussianConditional::likelihood( if (Cgm_Kgcm == 0.0) { return {likelihood_m, 0.0}; } else { - // Add a constant factor to the likelihood in case the noise models + // Add a constant to the likelihood in case the noise models // are not all equal. - GaussianFactorGraph gfg; - gfg.push_back(likelihood_m); - Vector c(1); - c << std::sqrt(2.0 * Cgm_Kgcm); - auto constantFactor = std::make_shared(c); - gfg.push_back(constantFactor); - return {std::make_shared(gfg), 0.0}; + double c = 2.0 * Cgm_Kgcm; + return {likelihood_m, c}; } }); return std::make_shared( From 1c74da26f455a3f12b692eecfc5142ab463449a5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Sep 2024 09:46:33 -0400 Subject: [PATCH 20/30] fix python tests --- python/gtsam/tests/test_HybridFactorGraph.py | 4 ++-- .../gtsam/tests/test_HybridNonlinearFactorGraph.py | 13 ++++++------- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index f82665c495..adbda59ce6 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -37,7 +37,7 @@ def test_create(self): jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model) jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model) - gmf = HybridGaussianFactor([X(0)], dk, [jf1, jf2]) + gmf = HybridGaussianFactor([X(0)], dk, [(jf1, 0), (jf2, 0)]) hfg = HybridGaussianFactorGraph() hfg.push_back(jf1) @@ -64,7 +64,7 @@ def test_optimize(self): jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model) jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model) - gmf = HybridGaussianFactor([X(0)], dk, [jf1, jf2]) + gmf = HybridGaussianFactor([X(0)], dk, [(jf1, 0), (jf2, 0)]) hfg = HybridGaussianFactorGraph() hfg.push_back(jf1) diff --git a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py index 3a659c4e87..247f83c191 100644 --- a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py +++ b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py @@ -35,13 +35,12 @@ def test_nonlinear_hybrid(self): nlfg.push_back( PriorFactorPoint3(2, Point3(1, 2, 3), noiseModel.Diagonal.Variances([0.5, 0.5, 0.5]))) - nlfg.push_back( - gtsam.HybridNonlinearFactor([1], dk, [ - PriorFactorPoint3(1, Point3(0, 0, 0), - noiseModel.Unit.Create(3)), - PriorFactorPoint3(1, Point3(1, 2, 1), - noiseModel.Unit.Create(3)) - ])) + + factors = [(PriorFactorPoint3(1, Point3(0, 0, 0), + noiseModel.Unit.Create(3)), 0.0), + (PriorFactorPoint3(1, Point3(1, 2, 1), + noiseModel.Unit.Create(3)), 0.0)] + nlfg.push_back(gtsam.HybridNonlinearFactor([1], dk, factors)) nlfg.push_back(gtsam.DecisionTreeFactor((10, 2), "1 3")) values = gtsam.Values() values.insert_point3(1, Point3(0, 0, 0)) From 64d9fc67bd41aa07eb17c807f03a5e6da9b93c3b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Sep 2024 17:30:08 -0400 Subject: [PATCH 21/30] tree based constructors --- gtsam/hybrid/tests/Switching.h | 19 ++++++++++--------- gtsam/hybrid/tests/testHybridBayesNet.cpp | 12 +++++++----- 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 9b7c44d02e..4be3cff01b 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -57,15 +57,16 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( // keyFunc(1) to keyFunc(n+1) for (size_t t = 1; t < n; t++) { - std::vector components = { - {std::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), - I_3x3, Z_3x1), - 0.0}, - {std::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), - I_3x3, Vector3::Ones()), - 0.0}}; - hfg.add(HybridGaussianFactor({keyFunc(t), keyFunc(t + 1)}, - {{dKeyFunc(t), 2}}, components)); + DiscreteKeys dKeys{{dKeyFunc(t), 2}}; + HybridGaussianFactor::FactorValuePairs components( + dKeys, {{std::make_shared(keyFunc(t), I_3x3, + keyFunc(t + 1), I_3x3, Z_3x1), + 0.0}, + {std::make_shared( + keyFunc(t), I_3x3, keyFunc(t + 1), I_3x3, Vector3::Ones()), + 0.0}}); + hfg.add( + HybridGaussianFactor({keyFunc(t), keyFunc(t + 1)}, dKeys, components)); if (t > 1) { hfg.add(DecisionTreeFactor({{dKeyFunc(t - 1), 2}, {dKeyFunc(t), 2}}, diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index cf4231dba2..8b48c14b63 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -383,15 +383,17 @@ TEST(HybridBayesNet, Sampling) { HybridNonlinearFactorGraph nfg; auto noise_model = noiseModel::Diagonal::Sigmas(Vector1(1.0)); + nfg.emplace_shared>(X(0), 0.0, noise_model); + auto zero_motion = std::make_shared>(X(0), X(1), 0, noise_model); auto one_motion = std::make_shared>(X(0), X(1), 1, noise_model); - std::vector factors = {{zero_motion, 0.0}, - {one_motion, 0.0}}; - nfg.emplace_shared>(X(0), 0.0, noise_model); - nfg.emplace_shared( - KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors); + DiscreteKeys discreteKeys{DiscreteKey(M(0), 2)}; + HybridNonlinearFactor::Factors factors( + discreteKeys, {{zero_motion, 0.0}, {one_motion, 0.0}}); + nfg.emplace_shared(KeyVector{X(0), X(1)}, discreteKeys, + factors); DiscreteKey mode(M(0), 2); nfg.emplace_shared(mode, "1/1"); From 094db1eb799dcb913675342699fdb3802b095d92 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Sep 2024 18:39:40 -0400 Subject: [PATCH 22/30] correct documentation and test for ComputeLogNormalizer --- gtsam/linear/NoiseModel.cpp | 3 +++ gtsam/linear/NoiseModel.h | 2 +- gtsam/linear/tests/testNoiseModel.cpp | 20 ++++++++++++++++++++ 3 files changed, 24 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 29222a1066..a586d119b7 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -714,6 +714,9 @@ double ComputeLogNormalizer( const noiseModel::Gaussian::shared_ptr& noise_model) { // Since noise models are Gaussian, we can get the logDeterminant using // the same trick as in GaussianConditional + // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1} + // log det(Sigma) = -log(det(R'R)) = -2*log(det(R)) + // Hence, log det(Sigma)) = -2.0 * logDetR() double logDetR = noise_model->R() .diagonal() .unaryExpr([](double x) { return log(x); }) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 831cfd7ddf..ffc1a3ebcd 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -752,7 +752,7 @@ namespace gtsam { template<> struct traits : public Testable {}; /** - * @brief Helper function to compute the sqrt(|2πΣ|) normalizer values + * @brief Helper function to compute the log(|2πΣ|) normalizer values * for a Gaussian noise model. * We compute this in the log-space for numerical accuracy. * diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 725680501d..49874f901c 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -807,6 +807,26 @@ TEST(NoiseModel, NonDiagonalGaussian) } } +TEST(NoiseModel, ComputeLogNormalizer) { + // Very simple 1D noise model, which we can compute by hand. + double sigma = 0.1; + auto noise_model = Isotropic::Sigma(1, sigma); + double actual_value = ComputeLogNormalizer(noise_model); + // Compute log(|2πΣ|) by hand. + // = log(2π) + log(Σ) (since it is 1D) + constexpr double log2pi = 1.8378770664093454835606594728112; + double expected_value = log2pi + log(sigma * sigma); + EXPECT_DOUBLES_EQUAL(expected_value, actual_value, 1e-9); + + // Similar situation in the 3D case + size_t n = 3; + auto noise_model2 = Isotropic::Sigma(n, sigma); + double actual_value2 = ComputeLogNormalizer(noise_model2); + // We multiply by 3 due to the determinant + double expected_value2 = n * (log2pi + log(sigma * sigma)); + EXPECT_DOUBLES_EQUAL(expected_value2, actual_value2, 1e-9); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From f9031f53b454ed2932693e2fbc2551e353df2850 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Sep 2024 18:43:07 -0400 Subject: [PATCH 23/30] fix error function --- gtsam/hybrid/HybridNonlinearFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 742b4c1625..096c14f772 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -140,7 +140,7 @@ class HybridNonlinearFactor : public HybridFactor { auto errorFunc = [continuousValues](const std::pair& f) { auto [factor, val] = f; - return factor->error(continuousValues) + (0.5 * val * val); + return factor->error(continuousValues) + (0.5 * val); }; DecisionTree result(factors_, errorFunc); return result; @@ -159,7 +159,7 @@ class HybridNonlinearFactor : public HybridFactor { auto [factor, val] = factors_(discreteValues); // Compute the error for the selected factor const double factorError = factor->error(continuousValues); - return factorError + (0.5 * val * val); + return factorError + (0.5 * val); } /** From ef2ffd4115b6a40d1b2b2567cbf3d2060633bd12 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Sep 2024 19:46:00 -0400 Subject: [PATCH 24/30] cleaner assignment in augment() --- gtsam/hybrid/HybridGaussianFactor.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index bfebb064e1..cabe6defcc 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -42,9 +42,11 @@ HybridGaussianFactor::Factors augment( const HybridGaussianFactor::FactorValuePairs &factors) { // Find the minimum value so we can "proselytize" to positive values. // Done because we can't have sqrt of negative numbers. - auto unzipped_pair = unzip(factors); - const HybridGaussianFactor::Factors gaussianFactors = unzipped_pair.first; - const AlgebraicDecisionTree valueTree = unzipped_pair.second; + HybridGaussianFactor::Factors gaussianFactors; + AlgebraicDecisionTree valueTree; + std::tie(gaussianFactors, valueTree) = unzip(factors); + + // Normalize double min_value = valueTree.min(); AlgebraicDecisionTree values = valueTree.apply([&min_value](double n) { return n - min_value; }); From 091352806b2ec648270d18c3ee21baeb44a72186 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Sep 2024 13:37:44 -0400 Subject: [PATCH 25/30] update API to only allow a single DiscreteKey and vector of size the same as the discrete key cardinality --- gtsam/hybrid/HybridGaussianFactor.h | 10 +++++----- gtsam/hybrid/HybridNonlinearFactor.h | 9 +++++---- gtsam/hybrid/tests/Switching.h | 4 ++-- gtsam/hybrid/tests/testHybridEstimation.cpp | 18 ++++++------------ .../tests/testHybridGaussianFactorGraph.cpp | 4 ++-- gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 6 +++--- .../tests/testHybridNonlinearFactorGraph.cpp | 8 ++++---- gtsam/hybrid/tests/testHybridNonlinearISAM.cpp | 6 +++--- gtsam/hybrid/tests/testSerializationHybrid.cpp | 4 ++-- 9 files changed, 32 insertions(+), 37 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 333c5d25bb..5f9cd925e3 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -96,15 +96,15 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { * GaussianFactor shared pointers. * * @param continuousKeys Vector of keys for continuous factors. - * @param discreteKeys Vector of discrete keys. + * @param discreteKey The discrete key to index each component. * @param factors Vector of gaussian factor shared pointers - * and arbitrary scalars. + * and arbitrary scalars. Same size as the cardinality of discreteKey. */ HybridGaussianFactor(const KeyVector &continuousKeys, - const DiscreteKeys &discreteKeys, + const DiscreteKey &discreteKey, const std::vector &factors) - : HybridGaussianFactor(continuousKeys, discreteKeys, - FactorValuePairs(discreteKeys, factors)) {} + : HybridGaussianFactor(continuousKeys, {discreteKey}, + FactorValuePairs({discreteKey}, factors)) {} /// @} /// @name Testable diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 096c14f772..1120fc9480 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -92,17 +92,18 @@ class HybridNonlinearFactor : public HybridFactor { * @tparam FACTOR The type of the factor shared pointers being passed in. * Will be typecast to NonlinearFactor shared pointers. * @param keys Vector of keys for continuous factors. - * @param discreteKeys Vector of discrete keys. + * @param discreteKey The discrete key indexing each component factor. * @param factors Vector of nonlinear factor and scalar pairs. + * Same size as the cardinality of discreteKey. * @param normalized Flag indicating if the factor error is already * normalized. */ template HybridNonlinearFactor( - const KeyVector& keys, const DiscreteKeys& discreteKeys, + const KeyVector& keys, const DiscreteKey& discreteKey, const std::vector, double>>& factors, bool normalized = false) - : Base(keys, discreteKeys), normalized_(normalized) { + : Base(keys, {discreteKey}), normalized_(normalized) { std::vector nonlinear_factors; KeySet continuous_keys_set(keys.begin(), keys.end()); KeySet factor_keys_set; @@ -118,7 +119,7 @@ class HybridNonlinearFactor : public HybridFactor { "Factors passed into HybridNonlinearFactor need to be nonlinear!"); } } - factors_ = Factors(discreteKeys, nonlinear_factors); + factors_ = Factors({discreteKey}, nonlinear_factors); if (continuous_keys_set != factor_keys_set) { throw std::runtime_error( diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 4be3cff01b..f18c195594 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -168,8 +168,8 @@ struct Switching { components.push_back( {std::dynamic_pointer_cast(f), 0.0}); } - nonlinearFactorGraph.emplace_shared( - keys, DiscreteKeys{modes[k]}, components); + nonlinearFactorGraph.emplace_shared(keys, modes[k], + components); } // Add measurement factors diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 7204d819d9..ce70e41fa4 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -437,8 +437,8 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() { std::make_shared>(X(0), X(1), 1, noise_model); std::vector components = {{zero_motion, 0.0}, {one_motion, 0.0}}; - nfg.emplace_shared(KeyVector{X(0), X(1)}, - DiscreteKeys{m}, components); + nfg.emplace_shared(KeyVector{X(0), X(1)}, m, + components); return nfg; } @@ -583,9 +583,6 @@ TEST(HybridEstimation, ModeSelection) { graph.emplace_shared>(X(0), 0.0, measurement_model); graph.emplace_shared>(X(1), 0.0, measurement_model); - DiscreteKeys modes; - modes.emplace_back(M(0), 2); - // The size of the noise model size_t d = 1; double noise_tight = 0.5, noise_loose = 5.0; @@ -594,11 +591,11 @@ TEST(HybridEstimation, ModeSelection) { X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_loose)), model1 = std::make_shared( X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight)); - std::vector components = {{model0, 0.0}, {model1, 0.0}}; KeyVector keys = {X(0), X(1)}; + DiscreteKey modes(M(0), 2); HybridNonlinearFactor mf(keys, modes, components); initial.insert(X(0), 0.0); @@ -617,7 +614,7 @@ TEST(HybridEstimation, ModeSelection) { /**************************************************************/ HybridBayesNet bn; - const DiscreteKey mode{M(0), 2}; + const DiscreteKey mode(M(0), 2); bn.push_back( GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(0), Z_1x1, 0.1)); @@ -648,7 +645,7 @@ TEST(HybridEstimation, ModeSelection2) { double noise_tight = 0.5, noise_loose = 5.0; HybridBayesNet bn; - const DiscreteKey mode{M(0), 2}; + const DiscreteKey mode(M(0), 2); bn.push_back( GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(0), Z_3x1, 0.1)); @@ -679,18 +676,15 @@ TEST(HybridEstimation, ModeSelection2) { graph.emplace_shared>(X(0), Z_3x1, measurement_model); graph.emplace_shared>(X(1), Z_3x1, measurement_model); - DiscreteKeys modes; - modes.emplace_back(M(0), 2); - auto model0 = std::make_shared>( X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_loose)), model1 = std::make_shared>( X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight)); - std::vector components = {{model0, 0.0}, {model1, 0.0}}; KeyVector keys = {X(0), X(1)}; + DiscreteKey modes(M(0), 2); HybridNonlinearFactor mf(keys, modes, components); initial.insert(X(0), Z_3x1); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 122f905fff..75ea80029c 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -181,7 +181,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { std::vector factors = { {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}}; - hfg.add(HybridGaussianFactor({X(1)}, {{M(1), 2}}, factors)); + hfg.add(HybridGaussianFactor({X(1)}, {M(1), 2}, factors)); hfg.add(DecisionTreeFactor(m1, {2, 8})); // TODO(Varun) Adding extra discrete variable not connected to continuous @@ -241,7 +241,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { std::vector factors = { {std::make_shared(X(0), I_3x3, Z_3x1), 0.0}, {std::make_shared(X(0), I_3x3, Vector3::Ones()), 0.0}}; - hfg.add(HybridGaussianFactor({X(0)}, {{M(0), 2}}, factors)); + hfg.add(HybridGaussianFactor({X(0)}, {M(0), 2}, factors)); DecisionTree dt1( M(1), {std::make_shared(X(2), I_3x3, Z_3x1), 0.0}, diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index cf983254a2..e2a10a783d 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -423,7 +423,7 @@ TEST(HybridGaussianISAM, NonTrivial) { std::vector> components = { {moving, 0.0}, {still, 0.0}}; auto mixtureFactor = std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); + contKeys, gtsam::DiscreteKey(M(1), 2), components); fg.push_back(mixtureFactor); // Add equivalent of ImuFactor @@ -463,7 +463,7 @@ TEST(HybridGaussianISAM, NonTrivial) { std::make_shared(W(1), W(2), odometry, noise_model); components = {{moving, 0.0}, {still, 0.0}}; mixtureFactor = std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); + contKeys, gtsam::DiscreteKey(M(2), 2), components); fg.push_back(mixtureFactor); // Add equivalent of ImuFactor @@ -506,7 +506,7 @@ TEST(HybridGaussianISAM, NonTrivial) { std::make_shared(W(2), W(3), odometry, noise_model); components = {{moving, 0.0}, {still, 0.0}}; mixtureFactor = std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); + contKeys, gtsam::DiscreteKey(M(3), 2), components); fg.push_back(mixtureFactor); // Add equivalent of ImuFactor diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 00d0d3a39a..3631ac44ed 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -135,7 +135,7 @@ TEST(HybridGaussianFactorGraph, Resize) { std::vector> components = { {still, 0.0}, {moving, 0.0}}; auto dcFactor = std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); + contKeys, gtsam::DiscreteKey(M(1), 2), components); nhfg.push_back(dcFactor); Values linearizationPoint; @@ -170,12 +170,12 @@ TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) { // Check for exception when number of continuous keys are under-specified. KeyVector contKeys = {X(0)}; THROWS_EXCEPTION(std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components)); + contKeys, gtsam::DiscreteKey(M(1), 2), components)); // Check for exception when number of continuous keys are too many. contKeys = {X(0), X(1), X(2)}; THROWS_EXCEPTION(std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components)); + contKeys, gtsam::DiscreteKey(M(1), 2), components)); } /***************************************************************************** @@ -807,7 +807,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { std::vector> motion_models = {{still, 0.0}, {moving, 0.0}}; fg.emplace_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models); + contKeys, gtsam::DiscreteKey(M(1), 2), motion_models); // Add Range-Bearing measurements to from X0 to L0 and X1 to L1. // create a noise model for the landmark measurements diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 7519162ebe..6932508dfb 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -442,7 +442,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { std::vector> components = { {moving, 0.0}, {still, 0.0}}; auto mixtureFactor = std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); + contKeys, gtsam::DiscreteKey(M(1), 2), components); fg.push_back(mixtureFactor); // Add equivalent of ImuFactor @@ -482,7 +482,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { std::make_shared(W(1), W(2), odometry, noise_model); components = {{moving, 0.0}, {still, 0.0}}; mixtureFactor = std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); + contKeys, gtsam::DiscreteKey(M(2), 2), components); fg.push_back(mixtureFactor); // Add equivalent of ImuFactor @@ -525,7 +525,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { std::make_shared(W(2), W(3), odometry, noise_model); components = {{moving, 0.0}, {still, 0.0}}; mixtureFactor = std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); + contKeys, gtsam::DiscreteKey(M(3), 2), components); fg.push_back(mixtureFactor); // Add equivalent of ImuFactor diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp index 74cfa72e6b..55f6259920 100644 --- a/gtsam/hybrid/tests/testSerializationHybrid.cpp +++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp @@ -76,7 +76,7 @@ BOOST_CLASS_EXPORT_GUID(HybridBayesNet, "gtsam_HybridBayesNet"); // Test HybridGaussianFactor serialization. TEST(HybridSerialization, HybridGaussianFactor) { KeyVector continuousKeys{X(0)}; - DiscreteKeys discreteKeys{{M(0), 2}}; + DiscreteKey discreteKey{M(0), 2}; auto A = Matrix::Zero(2, 1); auto b0 = Matrix::Zero(2, 1); @@ -85,7 +85,7 @@ TEST(HybridSerialization, HybridGaussianFactor) { auto f1 = std::make_shared(X(0), A, b1); std::vector factors{{f0, 0.0}, {f1, 0.0}}; - const HybridGaussianFactor factor(continuousKeys, discreteKeys, factors); + const HybridGaussianFactor factor(continuousKeys, discreteKey, factors); EXPECT(equalsObj(factor)); EXPECT(equalsXML(factor)); From d4923dbfa9012beaf8d33f25550f0c3c7885d4de Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Sep 2024 13:58:59 -0400 Subject: [PATCH 26/30] Use DecisionTree for constructing HybridGaussianConditional --- gtsam/hybrid/HybridGaussianConditional.cpp | 18 ------- gtsam/hybrid/HybridGaussianConditional.h | 28 +---------- gtsam/hybrid/tests/TinyHybridExample.h | 9 ++-- gtsam/hybrid/tests/testHybridBayesNet.cpp | 12 +++-- gtsam/hybrid/tests/testHybridEstimation.cpp | 24 ++++++---- .../tests/testHybridGaussianConditional.cpp | 8 +++- .../hybrid/tests/testHybridGaussianFactor.cpp | 10 +++- .../tests/testHybridGaussianFactorGraph.cpp | 47 ++++++++++++------- .../hybrid/tests/testSerializationHybrid.cpp | 3 +- 9 files changed, 76 insertions(+), 83 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 2b2062e6f4..8fce251ccc 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -55,24 +55,6 @@ HybridGaussianConditional::conditionals() const { return conditionals_; } -/* *******************************************************************************/ -HybridGaussianConditional::HybridGaussianConditional( - KeyVector &&continuousFrontals, KeyVector &&continuousParents, - DiscreteKeys &&discreteParents, - std::vector &&conditionals) - : HybridGaussianConditional(continuousFrontals, continuousParents, - discreteParents, - Conditionals(discreteParents, conditionals)) {} - -/* *******************************************************************************/ -HybridGaussianConditional::HybridGaussianConditional( - const KeyVector &continuousFrontals, const KeyVector &continuousParents, - const DiscreteKeys &discreteParents, - const std::vector &conditionals) - : HybridGaussianConditional(continuousFrontals, continuousParents, - discreteParents, - Conditionals(discreteParents, conditionals)) {} - /* *******************************************************************************/ // TODO(dellaert): This is copy/paste: HybridGaussianConditional should be // derived from HybridGaussianFactor, no? diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index fc315c6f9e..2c132d3bf0 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -106,32 +106,6 @@ class GTSAM_EXPORT HybridGaussianConditional const DiscreteKeys &discreteParents, const Conditionals &conditionals); - /** - * @brief Make a Gaussian Mixture from a list of Gaussian conditionals - * - * @param continuousFrontals The continuous frontal variables - * @param continuousParents The continuous parent variables - * @param discreteParents Discrete parents variables - * @param conditionals List of conditionals - */ - HybridGaussianConditional( - KeyVector &&continuousFrontals, KeyVector &&continuousParents, - DiscreteKeys &&discreteParents, - std::vector &&conditionals); - - /** - * @brief Make a Gaussian Mixture from a list of Gaussian conditionals - * - * @param continuousFrontals The continuous frontal variables - * @param continuousParents The continuous parent variables - * @param discreteParents Discrete parents variables - * @param conditionals List of conditionals - */ - HybridGaussianConditional( - const KeyVector &continuousFrontals, const KeyVector &continuousParents, - const DiscreteKeys &discreteParents, - const std::vector &conditionals); - /// @} /// @name Testable /// @{ @@ -273,7 +247,7 @@ class GTSAM_EXPORT HybridGaussianConditional #endif }; -/// Return the DiscreteKey vector as a set. +/// Return the DiscreteKeys vector as a set. std::set DiscreteKeysAsSet(const DiscreteKeys &discreteKeys); // traits diff --git a/gtsam/hybrid/tests/TinyHybridExample.h b/gtsam/hybrid/tests/TinyHybridExample.h index 00af283085..b2b944f91e 100644 --- a/gtsam/hybrid/tests/TinyHybridExample.h +++ b/gtsam/hybrid/tests/TinyHybridExample.h @@ -43,12 +43,13 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1, // Create Gaussian mixture z_i = x0 + noise for each measurement. for (size_t i = 0; i < num_measurements; i++) { const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode; + DiscreteKeys modes{mode_i}; + std::vector conditionals{ + GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 0.5), + GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 3)}; bayesNet.emplace_shared( KeyVector{Z(i)}, KeyVector{X(0)}, DiscreteKeys{mode_i}, - std::vector{GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), - Z_1x1, 0.5), - GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), - Z_1x1, 3)}); + HybridGaussianConditional::Conditionals(modes, conditionals)); } // Create prior on X(0). diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 8b48c14b63..462ce8625a 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -107,9 +107,11 @@ TEST(HybridBayesNet, evaluateHybrid) { // Create hybrid Bayes net. HybridBayesNet bayesNet; bayesNet.push_back(continuousConditional); + DiscreteKeys discreteParents{Asia}; bayesNet.emplace_shared( - KeyVector{X(1)}, KeyVector{}, DiscreteKeys{Asia}, - std::vector{conditional0, conditional1}); + KeyVector{X(1)}, KeyVector{}, discreteParents, + HybridGaussianConditional::Conditionals( + discreteParents, std::vector{conditional0, conditional1})); bayesNet.emplace_shared(Asia, "99/1"); // Create values at which to evaluate. @@ -168,9 +170,11 @@ TEST(HybridBayesNet, Error) { conditional1 = std::make_shared( X(1), Vector1::Constant(2), I_1x1, model1); + DiscreteKeys discreteParents{Asia}; auto gm = std::make_shared( - KeyVector{X(1)}, KeyVector{}, DiscreteKeys{Asia}, - std::vector{conditional0, conditional1}); + KeyVector{X(1)}, KeyVector{}, discreteParents, + HybridGaussianConditional::Conditionals( + discreteParents, std::vector{conditional0, conditional1})); // Create hybrid Bayes net. HybridBayesNet bayesNet; bayesNet.push_back(continuousConditional); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index ce70e41fa4..ee48ad5d8b 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -620,12 +620,16 @@ TEST(HybridEstimation, ModeSelection) { GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(0), Z_1x1, 0.1)); bn.push_back( GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(1), Z_1x1, 0.1)); + + std::vector conditionals{ + GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1), + Z_1x1, noise_loose), + GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1), + Z_1x1, noise_tight)}; bn.emplace_shared( KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode}, - std::vector{GaussianConditional::sharedMeanAndStddev( - Z(0), I_1x1, X(0), -I_1x1, X(1), Z_1x1, noise_loose), - GaussianConditional::sharedMeanAndStddev( - Z(0), I_1x1, X(0), -I_1x1, X(1), Z_1x1, noise_tight)}); + HybridGaussianConditional::Conditionals(DiscreteKeys{mode}, + conditionals)); VectorValues vv; vv.insert(Z(0), Z_1x1); @@ -651,12 +655,16 @@ TEST(HybridEstimation, ModeSelection2) { GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(0), Z_3x1, 0.1)); bn.push_back( GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(1), Z_3x1, 0.1)); + + std::vector conditionals{ + GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1), + Z_3x1, noise_loose), + GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1), + Z_3x1, noise_tight)}; bn.emplace_shared( KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode}, - std::vector{GaussianConditional::sharedMeanAndStddev( - Z(0), I_3x3, X(0), -I_3x3, X(1), Z_3x1, noise_loose), - GaussianConditional::sharedMeanAndStddev( - Z(0), I_3x3, X(0), -I_3x3, X(1), Z_3x1, noise_tight)}); + HybridGaussianConditional::Conditionals(DiscreteKeys{mode}, + conditionals)); VectorValues vv; vv.insert(Z(0), Z_3x1); diff --git a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp index 6156fee96d..d011584133 100644 --- a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp @@ -52,7 +52,9 @@ const std::vector conditionals{ commonSigma), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), commonSigma)}; -const HybridGaussianConditional mixture({Z(0)}, {X(0)}, {mode}, conditionals); +const HybridGaussianConditional mixture( + {Z(0)}, {X(0)}, {mode}, + HybridGaussianConditional::Conditionals({mode}, conditionals)); } // namespace equal_constants /* ************************************************************************* */ @@ -153,7 +155,9 @@ const std::vector conditionals{ 0.5), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), 3.0)}; -const HybridGaussianConditional mixture({Z(0)}, {X(0)}, {mode}, conditionals); +const HybridGaussianConditional mixture( + {Z(0)}, {X(0)}, {mode}, + HybridGaussianConditional::Conditionals({mode}, conditionals)); } // namespace mode_dependent_constants /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 11226e19d1..850a1e4d1c 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -233,8 +233,11 @@ static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, c1 = make_shared(z, Vector1(mu1), I_1x1, model1); HybridBayesNet hbn; + DiscreteKeys discreteParents{m}; hbn.emplace_shared( - KeyVector{z}, KeyVector{}, DiscreteKeys{m}, std::vector{c0, c1}); + KeyVector{z}, KeyVector{}, discreteParents, + HybridGaussianConditional::Conditionals(discreteParents, + std::vector{c0, c1})); auto mixing = make_shared(m, "50/50"); hbn.push_back(mixing); @@ -408,8 +411,11 @@ static HybridGaussianConditional::shared_ptr CreateHybridMotionModel( -I_1x1, model0), c1 = make_shared(X(1), Vector1(mu1), I_1x1, X(0), -I_1x1, model1); + DiscreteKeys discreteParents{m1}; return std::make_shared( - KeyVector{X(1)}, KeyVector{X(0)}, DiscreteKeys{m1}, std::vector{c0, c1}); + KeyVector{X(1)}, KeyVector{X(0)}, discreteParents, + HybridGaussianConditional::Conditionals(discreteParents, + std::vector{c0, c1})); } /// Create two state Bayes network with 1 or two measurement models diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 75ea80029c..dc88c8b7ee 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -682,8 +682,11 @@ TEST(HybridGaussianFactorGraph, ErrorTreeWithConditional) { x0, -I_1x1, model0), c1 = make_shared(f01, Vector1(mu), I_1x1, x1, I_1x1, x0, -I_1x1, model1); + DiscreteKeys discreteParents{m1}; hbn.emplace_shared( - KeyVector{f01}, KeyVector{x0, x1}, DiscreteKeys{m1}, std::vector{c0, c1}); + KeyVector{f01}, KeyVector{x0, x1}, discreteParents, + HybridGaussianConditional::Conditionals(discreteParents, + std::vector{c0, c1})); // Discrete uniform prior. hbn.emplace_shared(m1, "0.5/0.5"); @@ -806,9 +809,11 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) { X(0), Vector1(14.1421), I_1x1 * 2.82843), conditional1 = std::make_shared( X(0), Vector1(10.1379), I_1x1 * 2.02759); + DiscreteKeys discreteParents{mode}; expectedBayesNet.emplace_shared( - KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode}, - std::vector{conditional0, conditional1}); + KeyVector{X(0)}, KeyVector{}, discreteParents, + HybridGaussianConditional::Conditionals( + discreteParents, std::vector{conditional0, conditional1})); // Add prior on mode. expectedBayesNet.emplace_shared(mode, "74/26"); @@ -831,12 +836,13 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) { HybridBayesNet bn; // Create Gaussian mixture z_0 = x0 + noise for each measurement. + std::vector conditionals{ + GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 3), + GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 0.5)}; auto gm = std::make_shared( KeyVector{Z(0)}, KeyVector{X(0)}, DiscreteKeys{mode}, - std::vector{ - GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 3), - GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, - 0.5)}); + HybridGaussianConditional::Conditionals(DiscreteKeys{mode}, + conditionals)); bn.push_back(gm); // Create prior on X(0). @@ -865,7 +871,8 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) { X(0), Vector1(14.1421), I_1x1 * 2.82843); expectedBayesNet.emplace_shared( KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode}, - std::vector{conditional0, conditional1}); + HybridGaussianConditional::Conditionals( + DiscreteKeys{mode}, std::vector{conditional0, conditional1})); // Add prior on mode. expectedBayesNet.emplace_shared(mode, "1/1"); @@ -902,7 +909,8 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) { X(0), Vector1(10.274), I_1x1 * 2.0548); expectedBayesNet.emplace_shared( KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode}, - std::vector{conditional0, conditional1}); + HybridGaussianConditional::Conditionals( + DiscreteKeys{mode}, std::vector{conditional0, conditional1})); // Add prior on mode. expectedBayesNet.emplace_shared(mode, "23/77"); @@ -947,12 +955,14 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) { for (size_t t : {0, 1, 2}) { // Create Gaussian mixture on Z(t) conditioned on X(t) and mode N(t): const auto noise_mode_t = DiscreteKey{N(t), 2}; + std::vector conditionals{ + GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1, 0.5), + GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1, + 3.0)}; bn.emplace_shared( KeyVector{Z(t)}, KeyVector{X(t)}, DiscreteKeys{noise_mode_t}, - std::vector{GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), - Z_1x1, 0.5), - GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), - Z_1x1, 3.0)}); + HybridGaussianConditional::Conditionals(DiscreteKeys{noise_mode_t}, + conditionals)); // Create prior on discrete mode N(t): bn.emplace_shared(noise_mode_t, "20/80"); @@ -962,12 +972,15 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) { for (size_t t : {2, 1}) { // Create Gaussian mixture on X(t) conditioned on X(t-1) and mode M(t-1): const auto motion_model_t = DiscreteKey{M(t), 2}; + std::vector conditionals{ + GaussianConditional::sharedMeanAndStddev(X(t), I_1x1, X(t - 1), Z_1x1, + 0.2), + GaussianConditional::sharedMeanAndStddev(X(t), I_1x1, X(t - 1), I_1x1, + 0.2)}; auto gm = std::make_shared( KeyVector{X(t)}, KeyVector{X(t - 1)}, DiscreteKeys{motion_model_t}, - std::vector{GaussianConditional::sharedMeanAndStddev( - X(t), I_1x1, X(t - 1), Z_1x1, 0.2), - GaussianConditional::sharedMeanAndStddev( - X(t), I_1x1, X(t - 1), I_1x1, 0.2)}); + HybridGaussianConditional::Conditionals(DiscreteKeys{motion_model_t}, + conditionals)); bn.push_back(gm); // Create prior on motion model M(t): diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp index 55f6259920..701b870f1c 100644 --- a/gtsam/hybrid/tests/testSerializationHybrid.cpp +++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp @@ -116,7 +116,8 @@ TEST(HybridSerialization, HybridGaussianConditional) { const auto conditional1 = std::make_shared( GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3)); const HybridGaussianConditional gm({Z(0)}, {X(0)}, {mode}, - {conditional0, conditional1}); + HybridGaussianConditional::Conditionals( + {mode}, {conditional0, conditional1})); EXPECT(equalsObj(gm)); EXPECT(equalsXML(gm)); From a276affe00e095f7f69e94b7ad9fbf2ba50ec0a1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Sep 2024 14:20:46 -0400 Subject: [PATCH 27/30] fix wrapper --- gtsam/hybrid/hybrid.i | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index dcf0fce042..8b825e4823 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -76,7 +76,7 @@ virtual class HybridConditional { class HybridGaussianFactor : gtsam::HybridFactor { HybridGaussianFactor( const gtsam::KeyVector& continuousKeys, - const gtsam::DiscreteKeys& discreteKeys, + const gtsam::DiscreteKey& discreteKey, const std::vector>& factorsList); @@ -91,8 +91,7 @@ class HybridGaussianConditional : gtsam::HybridFactor { const gtsam::KeyVector& continuousFrontals, const gtsam::KeyVector& continuousParents, const gtsam::DiscreteKeys& discreteParents, - const std::vector& - conditionalsList); + const gtsam::HybridGaussianConditional::Conditionals& conditionals); gtsam::HybridGaussianFactor* likelihood( const gtsam::VectorValues& frontals) const; @@ -248,7 +247,7 @@ class HybridNonlinearFactor : gtsam::HybridFactor { bool normalized = false); HybridNonlinearFactor( - const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, + const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey, const std::vector>& factors, bool normalized = false); From dbd0ae1f272e6da6d84f457ea1106debd27ba5d6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Sep 2024 14:51:24 -0400 Subject: [PATCH 28/30] reintroduce vector based HybridGaussianConditional constructor --- gtsam/hybrid/HybridGaussianConditional.cpp | 9 +++++++++ gtsam/hybrid/HybridGaussianConditional.h | 16 +++++++++++++++- gtsam/hybrid/hybrid.i | 5 +++++ 3 files changed, 29 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 8fce251ccc..a8ed7eed13 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -55,6 +55,15 @@ HybridGaussianConditional::conditionals() const { return conditionals_; } +/* *******************************************************************************/ +HybridGaussianConditional::HybridGaussianConditional( + const KeyVector &continuousFrontals, const KeyVector &continuousParents, + const DiscreteKeys &discreteParents, + const std::vector &conditionals) + : HybridGaussianConditional(continuousFrontals, continuousParents, + discreteParents, + Conditionals(discreteParents, conditionals)) {} + /* *******************************************************************************/ // TODO(dellaert): This is copy/paste: HybridGaussianConditional should be // derived from HybridGaussianFactor, no? diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 2c132d3bf0..9cd13cbec8 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -106,6 +106,20 @@ class GTSAM_EXPORT HybridGaussianConditional const DiscreteKeys &discreteParents, const Conditionals &conditionals); + /** + * @brief Make a Gaussian Mixture from a vector of Gaussian conditionals. + * The DecisionTree-based constructor is preferred over this one. + * + * @param continuousFrontals The continuous frontal variables + * @param continuousParents The continuous parent variables + * @param discreteParents Discrete parents variables + * @param conditionals Vector of conditionals + */ + HybridGaussianConditional( + const KeyVector &continuousFrontals, const KeyVector &continuousParents, + const DiscreteKeys &discreteParents, + const std::vector &conditionals); + /// @} /// @name Testable /// @{ @@ -247,7 +261,7 @@ class GTSAM_EXPORT HybridGaussianConditional #endif }; -/// Return the DiscreteKeys vector as a set. +/// Return the DiscreteKey vector as a set. std::set DiscreteKeysAsSet(const DiscreteKeys &discreteKeys); // traits diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 8b825e4823..485d108487 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -92,6 +92,11 @@ class HybridGaussianConditional : gtsam::HybridFactor { const gtsam::KeyVector& continuousParents, const gtsam::DiscreteKeys& discreteParents, const gtsam::HybridGaussianConditional::Conditionals& conditionals); + HybridGaussianConditional( + const gtsam::KeyVector& continuousFrontals, + const gtsam::KeyVector& continuousParents, + const gtsam::DiscreteKeys& discreteParents, + const std::vector& conditionals); gtsam::HybridGaussianFactor* likelihood( const gtsam::VectorValues& frontals) const; From c1ebdb200c9f1f8c5d6c59ef271d6e86b61e2650 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Sep 2024 14:53:31 -0400 Subject: [PATCH 29/30] update python tests --- python/gtsam/tests/test_HybridFactorGraph.py | 16 ++++++---------- .../tests/test_HybridNonlinearFactorGraph.py | 4 +--- 2 files changed, 7 insertions(+), 13 deletions(-) diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index adbda59ce6..ce71d75ed8 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -20,7 +20,7 @@ from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional, HybridBayesNet, HybridGaussianConditional, HybridGaussianFactor, HybridGaussianFactorGraph, - HybridValues, JacobianFactor, Ordering, noiseModel) + HybridValues, JacobianFactor, noiseModel) DEBUG_MARGINALS = False @@ -31,13 +31,11 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): def test_create(self): """Test construction of hybrid factor graph.""" model = noiseModel.Unit.Create(3) - dk = DiscreteKeys() - dk.push_back((C(0), 2)) jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model) jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model) - gmf = HybridGaussianFactor([X(0)], dk, [(jf1, 0), (jf2, 0)]) + gmf = HybridGaussianFactor([X(0)], (C(0), 2), [(jf1, 0), (jf2, 0)]) hfg = HybridGaussianFactorGraph() hfg.push_back(jf1) @@ -58,13 +56,11 @@ def test_create(self): def test_optimize(self): """Test construction of hybrid factor graph.""" model = noiseModel.Unit.Create(3) - dk = DiscreteKeys() - dk.push_back((C(0), 2)) jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model) jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model) - gmf = HybridGaussianFactor([X(0)], dk, [(jf1, 0), (jf2, 0)]) + gmf = HybridGaussianFactor([X(0)], (C(0), 2), [(jf1, 0), (jf2, 0)]) hfg = HybridGaussianFactorGraph() hfg.push_back(jf1) @@ -96,8 +92,6 @@ def tiny(num_measurements: int = 1, # Create Gaussian mixture Z(0) = X(0) + noise for each measurement. I_1x1 = np.eye(1) - keys = DiscreteKeys() - keys.push_back(mode) for i in range(num_measurements): conditional0 = GaussianConditional.FromMeanAndStddev(Z(i), I_1x1, @@ -107,8 +101,10 @@ def tiny(num_measurements: int = 1, I_1x1, X(0), [0], sigma=3) + discreteParents = DiscreteKeys() + discreteParents.push_back(mode) bayesNet.push_back( - HybridGaussianConditional([Z(i)], [X(0)], keys, + HybridGaussianConditional([Z(i)], [X(0)], discreteParents, [conditional0, conditional1])) # Create prior on X(0). diff --git a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py index 247f83c191..4a1abdcf39 100644 --- a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py +++ b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py @@ -27,8 +27,6 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): def test_nonlinear_hybrid(self): nlfg = gtsam.HybridNonlinearFactorGraph() - dk = gtsam.DiscreteKeys() - dk.push_back((10, 2)) nlfg.push_back( BetweenFactorPoint3(1, 2, Point3(1, 2, 3), noiseModel.Diagonal.Variances([1, 1, 1]))) @@ -40,7 +38,7 @@ def test_nonlinear_hybrid(self): noiseModel.Unit.Create(3)), 0.0), (PriorFactorPoint3(1, Point3(1, 2, 1), noiseModel.Unit.Create(3)), 0.0)] - nlfg.push_back(gtsam.HybridNonlinearFactor([1], dk, factors)) + nlfg.push_back(gtsam.HybridNonlinearFactor([1], (10, 2), factors)) nlfg.push_back(gtsam.DecisionTreeFactor((10, 2), "1 3")) values = gtsam.Values() values.insert_point3(1, Point3(0, 0, 0)) From fa353840b3d0dd16574cf5dadce5a11d6da4ff2c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Sep 2024 15:08:09 -0400 Subject: [PATCH 30/30] update HybridGaussianConditional to use single discrete parent --- gtsam/hybrid/HybridGaussianConditional.cpp | 6 +++--- gtsam/hybrid/HybridGaussianConditional.h | 7 ++++--- gtsam/hybrid/hybrid.i | 2 +- gtsam/hybrid/tests/TinyHybridExample.h | 4 +--- gtsam/hybrid/tests/testHybridBayesNet.cpp | 21 ++++++++------------ python/gtsam/tests/test_HybridBayesNet.py | 4 +--- python/gtsam/tests/test_HybridFactorGraph.py | 4 +--- 7 files changed, 19 insertions(+), 29 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index a8ed7eed13..4c2739c865 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -58,11 +58,11 @@ HybridGaussianConditional::conditionals() const { /* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( const KeyVector &continuousFrontals, const KeyVector &continuousParents, - const DiscreteKeys &discreteParents, + const DiscreteKey &discreteParent, const std::vector &conditionals) : HybridGaussianConditional(continuousFrontals, continuousParents, - discreteParents, - Conditionals(discreteParents, conditionals)) {} + DiscreteKeys{discreteParent}, + Conditionals({discreteParent}, conditionals)) {} /* *******************************************************************************/ // TODO(dellaert): This is copy/paste: HybridGaussianConditional should be diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 9cd13cbec8..240160744b 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -112,12 +112,13 @@ class GTSAM_EXPORT HybridGaussianConditional * * @param continuousFrontals The continuous frontal variables * @param continuousParents The continuous parent variables - * @param discreteParents Discrete parents variables - * @param conditionals Vector of conditionals + * @param discreteParent Single discrete parent variable + * @param conditionals Vector of conditionals with the same size as the + * cardinality of the discrete parent. */ HybridGaussianConditional( const KeyVector &continuousFrontals, const KeyVector &continuousParents, - const DiscreteKeys &discreteParents, + const DiscreteKey &discreteParent, const std::vector &conditionals); /// @} diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 485d108487..37bcefe811 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -95,7 +95,7 @@ class HybridGaussianConditional : gtsam::HybridFactor { HybridGaussianConditional( const gtsam::KeyVector& continuousFrontals, const gtsam::KeyVector& continuousParents, - const gtsam::DiscreteKeys& discreteParents, + const gtsam::DiscreteKey& discreteParent, const std::vector& conditionals); gtsam::HybridGaussianFactor* likelihood( diff --git a/gtsam/hybrid/tests/TinyHybridExample.h b/gtsam/hybrid/tests/TinyHybridExample.h index b2b944f91e..b3bb14c8f8 100644 --- a/gtsam/hybrid/tests/TinyHybridExample.h +++ b/gtsam/hybrid/tests/TinyHybridExample.h @@ -43,13 +43,11 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1, // Create Gaussian mixture z_i = x0 + noise for each measurement. for (size_t i = 0; i < num_measurements; i++) { const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode; - DiscreteKeys modes{mode_i}; std::vector conditionals{ GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 0.5), GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 3)}; bayesNet.emplace_shared( - KeyVector{Z(i)}, KeyVector{X(0)}, DiscreteKeys{mode_i}, - HybridGaussianConditional::Conditionals(modes, conditionals)); + KeyVector{Z(i)}, KeyVector{X(0)}, mode_i, conditionals); } // Create prior on X(0). diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 462ce8625a..bbe2d3df51 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -107,11 +107,9 @@ TEST(HybridBayesNet, evaluateHybrid) { // Create hybrid Bayes net. HybridBayesNet bayesNet; bayesNet.push_back(continuousConditional); - DiscreteKeys discreteParents{Asia}; bayesNet.emplace_shared( - KeyVector{X(1)}, KeyVector{}, discreteParents, - HybridGaussianConditional::Conditionals( - discreteParents, std::vector{conditional0, conditional1})); + KeyVector{X(1)}, KeyVector{}, Asia, + std::vector{conditional0, conditional1}); bayesNet.emplace_shared(Asia, "99/1"); // Create values at which to evaluate. @@ -170,11 +168,9 @@ TEST(HybridBayesNet, Error) { conditional1 = std::make_shared( X(1), Vector1::Constant(2), I_1x1, model1); - DiscreteKeys discreteParents{Asia}; auto gm = std::make_shared( - KeyVector{X(1)}, KeyVector{}, discreteParents, - HybridGaussianConditional::Conditionals( - discreteParents, std::vector{conditional0, conditional1})); + KeyVector{X(1)}, KeyVector{}, Asia, + std::vector{conditional0, conditional1}); // Create hybrid Bayes net. HybridBayesNet bayesNet; bayesNet.push_back(continuousConditional); @@ -393,11 +389,10 @@ TEST(HybridBayesNet, Sampling) { std::make_shared>(X(0), X(1), 0, noise_model); auto one_motion = std::make_shared>(X(0), X(1), 1, noise_model); - DiscreteKeys discreteKeys{DiscreteKey(M(0), 2)}; - HybridNonlinearFactor::Factors factors( - discreteKeys, {{zero_motion, 0.0}, {one_motion, 0.0}}); - nfg.emplace_shared(KeyVector{X(0), X(1)}, discreteKeys, - factors); + nfg.emplace_shared( + KeyVector{X(0), X(1)}, DiscreteKey(M(0), 2), + std::vector{{zero_motion, 0.0}, + {one_motion, 0.0}}); DiscreteKey mode(M(0), 2); nfg.emplace_shared(mode, "1/1"); diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py index 6e50c2bfe0..2f7dd4a987 100644 --- a/python/gtsam/tests/test_HybridBayesNet.py +++ b/python/gtsam/tests/test_HybridBayesNet.py @@ -43,14 +43,12 @@ def test_evaluate(self): # Create the conditionals conditional0 = GaussianConditional(X(1), [5], I_1x1, model0) conditional1 = GaussianConditional(X(1), [2], I_1x1, model1) - discrete_keys = DiscreteKeys() - discrete_keys.push_back(Asia) # Create hybrid Bayes net. bayesNet = HybridBayesNet() bayesNet.push_back(conditional) bayesNet.push_back( - HybridGaussianConditional([X(1)], [], discrete_keys, + HybridGaussianConditional([X(1)], [], Asia, [conditional0, conditional1])) bayesNet.push_back(DiscreteConditional(Asia, "99/1")) diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index ce71d75ed8..5eafc05aed 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -101,10 +101,8 @@ def tiny(num_measurements: int = 1, I_1x1, X(0), [0], sigma=3) - discreteParents = DiscreteKeys() - discreteParents.push_back(mode) bayesNet.push_back( - HybridGaussianConditional([Z(i)], [X(0)], discreteParents, + HybridGaussianConditional([Z(i)], [X(0)], mode, [conditional0, conditional1])) # Create prior on X(0).