From dac90db441525fcc4836c4958ed1a7c519ac56f0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Sep 2024 19:53:09 -0700 Subject: [PATCH 01/14] Small things --- gtsam/hybrid/tests/testHybridBayesNet.cpp | 2 +- gtsam/hybrid/tests/testHybridEstimation.cpp | 3 +- .../tests/testHybridGaussianFactorGraph.cpp | 39 +++++++------------ gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 2 +- .../tests/testHybridNonlinearFactorGraph.cpp | 4 +- .../hybrid/tests/testHybridNonlinearISAM.cpp | 2 +- 6 files changed, 20 insertions(+), 32 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 769512bede..ec8f087413 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -20,11 +20,11 @@ #include #include +#include #include #include "Switching.h" #include "TinyHybridExample.h" -#include "gtsam/nonlinear/NonlinearFactor.h" // Include for test suite #include diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 2a5fb93ba3..672f475034 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -29,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -39,7 +41,6 @@ #include #include "Switching.h" -#include "gtsam/nonlinear/NonlinearFactor.h" using namespace std; using namespace gtsam; diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index e4acda3871..0421727ffa 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -61,6 +61,8 @@ using gtsam::symbol_shorthand::Z; // Set up sampling std::mt19937_64 kRng(42); +static const DiscreteKey m1(M(1), 2); + /* ************************************************************************* */ TEST(HybridGaussianFactorGraph, Creation) { HybridConditional conditional; @@ -98,11 +100,9 @@ TEST(HybridGaussianFactorGraph, EliminateMultifrontal) { // Test multifrontal elimination HybridGaussianFactorGraph hfg; - DiscreteKey m(M(1), 2); - // Add priors on x0 and c1 hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); - hfg.add(DecisionTreeFactor(m, {2, 8})); + hfg.add(DecisionTreeFactor(m1, {2, 8})); Ordering ordering; ordering.push_back(X(0)); @@ -131,7 +131,6 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); // Add a hybrid gaussian factor ϕ(x1, c1) - DiscreteKey m1(M(1), 2); hfg.add(HybridGaussianFactor(m1, two::components(X(1)))); auto result = hfg.eliminateSequential(); @@ -148,8 +147,6 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) { HybridGaussianFactorGraph hfg; - DiscreteKey m1(M(1), 2); - // Add prior on x0 hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); // Add factor between x0 and x1 @@ -172,8 +169,6 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) { TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { HybridGaussianFactorGraph hfg; - DiscreteKey m1(M(1), 2); - hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); @@ -196,17 +191,15 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { HybridGaussianFactorGraph hfg; - DiscreteKey m(M(1), 2); - // Prior on x0 hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); // Factor between x0-x1 hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); // Hybrid factor P(x1|c1) - hfg.add(HybridGaussianFactor(m, two::components(X(1)))); + hfg.add(HybridGaussianFactor(m1, two::components(X(1)))); // Prior factor on c1 - hfg.add(DecisionTreeFactor(m, {2, 8})); + hfg.add(DecisionTreeFactor(m1, {2, 8})); // Get a constrained ordering keeping c1 last auto ordering_full = HybridOrdering(hfg); @@ -521,17 +514,15 @@ TEST(HybridGaussianFactorGraph, DiscreteSelection) { TEST(HybridGaussianFactorGraph, optimize) { HybridGaussianFactorGraph hfg; - DiscreteKey c1(C(1), 2); - 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(c1, two::components(X(1)))); + hfg.add(HybridGaussianFactor(m1, two::components(X(1)))); auto result = hfg.eliminateSequential(); HybridValues hv = result->optimize(); - EXPECT(assert_equal(hv.atDiscrete(C(1)), int(0))); + EXPECT(assert_equal(hv.atDiscrete(M(1)), int(0))); } /* ************************************************************************* */ @@ -626,7 +617,6 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrimeTree) { TEST(HybridGaussianFactorGraph, ErrorTreeWithConditional) { using symbol_shorthand::F; - DiscreteKey m1(M(1), 2); Key z0 = Z(0), f01 = F(0); Key x0 = X(0), x1 = X(1); @@ -842,23 +832,22 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) { TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) { const VectorValues measurements{{Z(0), Vector1(5.0)}}; - // Create mode key: 1 is low-noise, 0 is high-noise. - const DiscreteKey mode{M(0), 2}; HybridBayesNet bn; + // mode-dependent: 1 is low-noise, 0 is high-noise. // Create hybrid Gaussian factor 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(mode, conditionals); + auto gm = std::make_shared(m1, conditionals); bn.push_back(gm); // Create prior on X(0). bn.push_back( GaussianConditional::sharedMeanAndStddev(X(0), Vector1(5.0), 0.5)); - // Add prior on mode. - bn.emplace_shared(mode, "1/1"); + // Add prior on m1. + bn.emplace_shared(m1, "1/1"); // bn.print(); auto fg = bn.toFactorGraph(measurements); @@ -878,10 +867,10 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) { conditional1 = std::make_shared( X(0), Vector1(14.1421), I_1x1 * 2.82843); expectedBayesNet.emplace_shared( - mode, std::vector{conditional0, conditional1}); + m1, std::vector{conditional0, conditional1}); - // Add prior on mode. - expectedBayesNet.emplace_shared(mode, "1/1"); + // Add prior on m1. + expectedBayesNet.emplace_shared(m1, "1/1"); // Test elimination const auto posterior = fg.eliminateSequential(); diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 7572a64e26..1c72268de0 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -24,13 +24,13 @@ #include #include #include +#include #include #include #include #include "Switching.h" -#include "gtsam/nonlinear/NonlinearFactor.h" // Include for test suite #include diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index d323125086..b5f04bb990 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -28,15 +28,13 @@ #include #include #include +#include #include #include #include #include -#include - #include "Switching.h" -#include "gtsam/nonlinear/NonlinearFactor.h" // Include for test suite #include diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 5a04fc089e..2452ebe6d0 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -24,13 +24,13 @@ #include #include #include +#include #include #include #include #include "Switching.h" -#include "gtsam/nonlinear/NonlinearFactor.h" // Include for test suite #include From ea54525d375683d111f293878ff74406ec3bcd79 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Sep 2024 19:35:11 -0700 Subject: [PATCH 02/14] Make hybrid elimination a method of HGFG --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 34 +++++++------------ gtsam/hybrid/HybridGaussianFactorGraph.h | 8 +++++ .../tests/testHybridGaussianFactorGraph.cpp | 33 +++++++++++++----- 3 files changed, 46 insertions(+), 29 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 7ab7893cb1..787c0edd7d 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -366,18 +366,17 @@ static std::shared_ptr createHybridGaussianFactor( return std::make_shared(discreteSeparator, newFactors); } -static std::pair> -hybridElimination(const HybridGaussianFactorGraph &factors, - const Ordering &frontalKeys, - const std::set &discreteSeparatorSet) { - // NOTE: since we use the special JunctionTree, - // only possibility is continuous conditioned on discrete. - DiscreteKeys discreteSeparator(discreteSeparatorSet.begin(), - discreteSeparatorSet.end()); +std::pair> +HybridGaussianFactorGraph::eliminate(const Ordering &keys) const { + // Since we eliminate all continuous variables first, + // the discrete separator will be *all* the discrete keys. + const std::set keysForDiscreteVariables = discreteKeys(); + DiscreteKeys discreteSeparator(keysForDiscreteVariables.begin(), + keysForDiscreteVariables.end()); // Collect all the factors to create a set of Gaussian factor graphs in a // decision tree indexed by all discrete keys involved. - GaussianFactorGraphTree factorGraphTree = factors.assembleGraphTree(); + GaussianFactorGraphTree factorGraphTree = assembleGraphTree(); // Convert factor graphs with a nullptr to an empty factor graph. // This is done after assembly since it is non-trivial to keep track of which @@ -392,7 +391,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, } // Expensive elimination of product factor. - auto result = EliminatePreferCholesky(graph, frontalKeys); + auto result = EliminatePreferCholesky(graph, keys); // Record whether there any continuous variables left someContinuousLeft |= !result.second->empty(); @@ -436,7 +435,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, */ std::pair> // EliminateHybrid(const HybridGaussianFactorGraph &factors, - const Ordering &frontalKeys) { + const Ordering &keys) { // NOTE: Because we are in the Conditional Gaussian regime there are only // a few cases: // 1. continuous variable, make a hybrid Gaussian conditional if there are @@ -510,20 +509,13 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, if (only_discrete) { // Case 1: we are only dealing with discrete - return discreteElimination(factors, frontalKeys); + return discreteElimination(factors, keys); } else if (only_continuous) { // Case 2: we are only dealing with continuous - return continuousElimination(factors, frontalKeys); + return continuousElimination(factors, keys); } else { // Case 3: We are now in the hybrid land! - KeySet frontalKeysSet(frontalKeys.begin(), frontalKeys.end()); - - // Find all discrete keys. - // Since we eliminate all continuous variables first, - // the discrete separator will be *all* the discrete keys. - std::set discreteSeparator = factors.discreteKeys(); - - return hybridElimination(factors, frontalKeys, discreteSeparator); + return factors.eliminate(keys); } } diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index d7aa1042f5..923f48e380 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -217,6 +217,14 @@ class GTSAM_EXPORT HybridGaussianFactorGraph */ GaussianFactorGraphTree assembleGraphTree() const; + /** + * @brief Eliminate the given continuous keys. + * + * @param keys The continuous keys to eliminate. + * @return The conditional on the keys and a factor on the separator. + */ + std::pair, std::shared_ptr> + eliminate(const Ordering& keys) const; /// @} /// Get the GaussianFactorGraph at a given discrete assignment. diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 0421727ffa..ee7ca5265d 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -42,6 +43,7 @@ #include #include #include +#include #include #include @@ -120,6 +122,25 @@ std::vector components(Key key) { } } // namespace two +/* ************************************************************************* */ +TEST(HybridGaussianFactorGraph, hybridEliminationOneFactor) { + HybridGaussianFactorGraph hfg; + hfg.add(HybridGaussianFactor(m1, two::components(X(1)))); + + auto result = hfg.eliminate({X(1)}); + + // Check that we have a valid Gaussian conditional. + auto hgc = result.first->asHybrid(); + CHECK(hgc); + const HybridValues values{{{X(1), Z_3x1}}, {{M(1), 1}}}; + EXPECT(HybridConditional::CheckInvariants(*result.first, values)); + + // Check that factor is discrete and correct + auto factor = std::dynamic_pointer_cast(result.second); + CHECK(factor); + EXPECT(assert_equal(DecisionTreeFactor{m1, "1 1"}, *factor)); +} + /* ************************************************************************* */ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { HybridGaussianFactorGraph hfg; @@ -221,20 +242,16 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1)); - { - hfg.add(HybridGaussianFactor({M(0), 2}, two::components(X(0)))); - hfg.add(HybridGaussianFactor({M(1), 2}, two::components(X(2)))); - } + hfg.add(HybridGaussianFactor({M(0), 2}, two::components(X(0)))); + hfg.add(HybridGaussianFactor({M(1), 2}, two::components(X(2)))); hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); hfg.add(JacobianFactor(X(3), I_3x3, X(4), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1)); - { - hfg.add(HybridGaussianFactor({M(3), 2}, two::components(X(3)))); - hfg.add(HybridGaussianFactor({M(2), 2}, two::components(X(5)))); - } + hfg.add(HybridGaussianFactor({M(3), 2}, two::components(X(3)))); + hfg.add(HybridGaussianFactor({M(2), 2}, two::components(X(5)))); auto ordering_full = Ordering::ColamdConstrainedLast(hfg, {M(0), M(1), M(2), M(3)}); From 32c67892ed618f719fdaf4a1f76befde90ef6ab5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Sep 2024 20:09:24 -0700 Subject: [PATCH 03/14] Move test where it belongs --- gtsam/hybrid/tests/testHybridBayesNet.cpp | 51 +++++++++++++++++++ .../tests/testHybridGaussianFactorGraph.cpp | 50 ------------------ 2 files changed, 51 insertions(+), 50 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index ec8f087413..67005b470d 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -444,6 +444,57 @@ TEST(HybridBayesNet, Sampling) { // num_samples))); } +/* ****************************************************************************/ +// Test hybrid gaussian factor graph errorTree when +// there is a HybridConditional in the graph +TEST(HybridBayesNet, ErrorTreeWithConditional) { + using symbol_shorthand::F; + + Key z0 = Z(0), f01 = F(0); + Key x0 = X(0), x1 = X(1); + + HybridBayesNet hbn; + + auto prior_model = noiseModel::Isotropic::Sigma(1, 1e-1); + auto measurement_model = noiseModel::Isotropic::Sigma(1, 2.0); + + // Set a prior P(x0) at x0=0 + hbn.emplace_shared(x0, Vector1(0.0), I_1x1, prior_model); + + // Add measurement P(z0 | x0) + hbn.emplace_shared(z0, Vector1(0.0), -I_1x1, x0, I_1x1, + measurement_model); + + // Add hybrid motion model + double mu = 0.0; + double sigma0 = 1e2, sigma1 = 1e-2; + auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); + auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); + auto c0 = make_shared(f01, Vector1(mu), I_1x1, x1, I_1x1, + x0, -I_1x1, model0), + c1 = make_shared(f01, Vector1(mu), I_1x1, x1, I_1x1, + x0, -I_1x1, model1); + DiscreteKey m1(M(2), 2); + hbn.emplace_shared(m1, std::vector{c0, c1}); + + // Discrete uniform prior. + hbn.emplace_shared(m1, "0.5/0.5"); + + VectorValues given; + given.insert(z0, Vector1(0.0)); + given.insert(f01, Vector1(0.0)); + auto gfg = hbn.toFactorGraph(given); + + VectorValues vv; + vv.insert(x0, Vector1(1.0)); + vv.insert(x1, Vector1(2.0)); + AlgebraicDecisionTree errorTree = gfg.errorTree(vv); + + // regression + AlgebraicDecisionTree expected(m1, 59.335390372, 5050.125); + EXPECT(assert_equal(expected, errorTree, 1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index ee7ca5265d..38429ac603 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -628,56 +628,6 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrimeTree) { EXPECT(assert_equal(expected_probabilities, probabilities, 1e-7)); } -/* ****************************************************************************/ -// Test hybrid gaussian factor graph errorTree when -// there is a HybridConditional in the graph -TEST(HybridGaussianFactorGraph, ErrorTreeWithConditional) { - using symbol_shorthand::F; - - Key z0 = Z(0), f01 = F(0); - Key x0 = X(0), x1 = X(1); - - HybridBayesNet hbn; - - auto prior_model = noiseModel::Isotropic::Sigma(1, 1e-1); - auto measurement_model = noiseModel::Isotropic::Sigma(1, 2.0); - - // Set a prior P(x0) at x0=0 - hbn.emplace_shared(x0, Vector1(0.0), I_1x1, prior_model); - - // Add measurement P(z0 | x0) - hbn.emplace_shared(z0, Vector1(0.0), -I_1x1, x0, I_1x1, - measurement_model); - - // Add hybrid motion model - double mu = 0.0; - double sigma0 = 1e2, sigma1 = 1e-2; - auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); - auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); - auto c0 = make_shared(f01, Vector1(mu), I_1x1, x1, I_1x1, - x0, -I_1x1, model0), - c1 = make_shared(f01, Vector1(mu), I_1x1, x1, I_1x1, - x0, -I_1x1, model1); - hbn.emplace_shared(m1, std::vector{c0, c1}); - - // Discrete uniform prior. - hbn.emplace_shared(m1, "0.5/0.5"); - - VectorValues given; - given.insert(z0, Vector1(0.0)); - given.insert(f01, Vector1(0.0)); - auto gfg = hbn.toFactorGraph(given); - - VectorValues vv; - vv.insert(x0, Vector1(1.0)); - vv.insert(x1, Vector1(2.0)); - AlgebraicDecisionTree errorTree = gfg.errorTree(vv); - - // regression - AlgebraicDecisionTree expected(m1, 59.335390372, 5050.125); - EXPECT(assert_equal(expected, errorTree, 1e-9)); -} - /* ****************************************************************************/ // Test hybrid gaussian factor graph errorTree during // incremental operation From e690ff817ad49e07401479e72a2d325a09ed612d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Sep 2024 20:47:22 -0700 Subject: [PATCH 04/14] Create mean/stddev constructors --- gtsam/hybrid/HybridGaussianConditional.cpp | 79 +++++++++++++++++----- gtsam/hybrid/HybridGaussianConditional.h | 32 +++++++-- gtsam/hybrid/tests/testGaussianMixture.cpp | 31 +++------ 3 files changed, 97 insertions(+), 45 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 4e896206f7..d0f812d73c 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -29,16 +29,20 @@ #include +#include "gtsam/linear/JacobianFactor.h" + namespace gtsam { /* *******************************************************************************/ struct HybridGaussianConditional::ConstructorHelper { std::optional nrFrontals; - HybridGaussianFactor::FactorValuePairs pairs; + FactorValuePairs pairs; + Conditionals conditionals; double minNegLogConstant; - /// Compute all variables needed for the private constructor below. + /// Construct from tree of GaussianConditionals. ConstructorHelper(const Conditionals &conditionals) - : minNegLogConstant(std::numeric_limits::infinity()) { + : conditionals(conditionals), + minNegLogConstant(std::numeric_limits::infinity()) { auto func = [this](const GaussianConditional::shared_ptr &c) -> GaussianFactorValuePair { double value = 0.0; @@ -51,38 +55,79 @@ struct HybridGaussianConditional::ConstructorHelper { } return {std::dynamic_pointer_cast(c), value}; }; - pairs = HybridGaussianFactor::FactorValuePairs(conditionals, func); + pairs = FactorValuePairs(conditionals, func); if (!nrFrontals.has_value()) { throw std::runtime_error( "HybridGaussianConditional: need at least one frontal variable."); } } + + /// Construct from means and a single sigma. + ConstructorHelper(Key x, const DiscreteKey mode, + const std::vector &means, double sigma) + : nrFrontals(1), minNegLogConstant(0) { + std::vector gcs; + for (const auto &mean : means) { + auto c = GaussianConditional::sharedMeanAndStddev(x, mean, sigma); + gcs.push_back(c); + } + conditionals = Conditionals({mode}, gcs); + pairs = FactorValuePairs(conditionals, [](const auto &c) { + return GaussianFactorValuePair{c, 0.0}; + }); + } + + /// Construct from means and a sigmas. + ConstructorHelper(Key x, const DiscreteKey mode, + const std::vector> ¶meters) + : nrFrontals(1), + minNegLogConstant(std::numeric_limits::infinity()) { + std::vector gcs; + std::vector fvs; + for (const auto &[mean, sigma] : parameters) { + auto c = GaussianConditional::sharedMeanAndStddev(x, mean, sigma); + double value = c->negLogConstant(); + minNegLogConstant = std::min(minNegLogConstant, value); + gcs.push_back(c); + fvs.push_back({c, value}); + } + conditionals = Conditionals({mode}, gcs); + pairs = FactorValuePairs({mode}, fvs); + } }; /* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKeys &discreteParents, - const HybridGaussianConditional::Conditionals &conditionals, - const ConstructorHelper &helper) + const DiscreteKeys &discreteParents, const ConstructorHelper &helper) : BaseFactor(discreteParents, helper.pairs), BaseConditional(*helper.nrFrontals), - conditionals_(conditionals), + conditionals_(helper.conditionals), negLogConstant_(helper.minNegLogConstant) {} -/* *******************************************************************************/ +HybridGaussianConditional::HybridGaussianConditional( + const DiscreteKey &mode, + const std::vector &conditionals) + : HybridGaussianConditional(DiscreteKeys{mode}, + Conditionals({mode}, conditionals)) {} + +HybridGaussianConditional::HybridGaussianConditional( + Key x, const DiscreteKey mode, const std::vector &means, + double sigma) + : HybridGaussianConditional(DiscreteKeys{mode}, + ConstructorHelper(x, mode, means, sigma)) {} + +HybridGaussianConditional::HybridGaussianConditional( + Key x, const DiscreteKey mode, + const std::vector> ¶meters) + : HybridGaussianConditional(DiscreteKeys{mode}, + ConstructorHelper(x, mode, parameters)) {} + HybridGaussianConditional::HybridGaussianConditional( const DiscreteKeys &discreteParents, const HybridGaussianConditional::Conditionals &conditionals) - : HybridGaussianConditional(discreteParents, conditionals, + : HybridGaussianConditional(discreteParents, ConstructorHelper(conditionals)) {} -/* *******************************************************************************/ -HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKey &discreteParent, - const std::vector &conditionals) - : HybridGaussianConditional(DiscreteKeys{discreteParent}, - Conditionals({discreteParent}, conditionals)) {} - /* *******************************************************************************/ const HybridGaussianConditional::Conditionals & HybridGaussianConditional::conditionals() const { diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 02f6bba1ea..662318837d 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -79,14 +79,36 @@ class GTSAM_EXPORT HybridGaussianConditional /** * @brief Construct from one discrete key and vector of conditionals. * - * @param discreteParent Single discrete parent variable + * @param mode Single discrete parent variable * @param conditionals Vector of conditionals with the same size as the * cardinality of the discrete parent. */ HybridGaussianConditional( - const DiscreteKey &discreteParent, + const DiscreteKey &mode, const std::vector &conditionals); + /** + * @brief Construct from vector of means and a single sigma. + * + * @param x The continuous key. + * @param mode The discrete key. + * @param means The means for the Gaussian conditionals. + * @param sigma The standard deviation for the Gaussian conditionals. + */ + HybridGaussianConditional(Key x, const DiscreteKey mode, + const std::vector &means, double sigma); + + /** + * @brief Construct from vector of means and sigmas. + * + * @param x The continuous key. + * @param mode The discrete key. + * @param parameters The means and sigmas for the Gaussian conditionals. + */ + HybridGaussianConditional( + Key x, const DiscreteKey mode, + const std::vector> ¶meters); + /** * @brief Construct from multiple discrete keys and conditional tree. * @@ -186,10 +208,8 @@ class GTSAM_EXPORT HybridGaussianConditional struct ConstructorHelper; /// Private constructor that uses helper struct above. - HybridGaussianConditional( - const DiscreteKeys &discreteParents, - const HybridGaussianConditional::Conditionals &conditionals, - const ConstructorHelper &helper); + HybridGaussianConditional(const DiscreteKeys &discreteParents, + const ConstructorHelper &helper); /// Convert to a DecisionTree of Gaussian factor graphs. GaussianFactorGraphTree asGaussianFactorGraphTree() const; diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index cde7e4063b..87ef5e25d1 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -43,26 +43,6 @@ const DiscreteValues m1Assignment{{M(0), 1}}; DiscreteConditional::shared_ptr mixing = std::make_shared(m, "60/40"); -/** - * Create a simple Gaussian Mixture Model represented as p(z|m)P(m) - * where m is a discrete variable and z is a continuous variable. - * The "mode" m is binary and depending on m, we have 2 different means - * μ1 and μ2 for the Gaussian density p(z|m). - */ -HybridBayesNet GaussianMixtureModel(double mu0, double mu1, double sigma0, - double sigma1) { - HybridBayesNet hbn; - auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); - auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); - auto c0 = std::make_shared(Z(0), Vector1(mu0), I_1x1, - model0), - c1 = std::make_shared(Z(0), Vector1(mu1), I_1x1, - model1); - hbn.emplace_shared(m, std::vector{c0, c1}); - hbn.push_back(mixing); - return hbn; -} - /// Gaussian density function double Gaussian(double mu, double sigma, double z) { return exp(-0.5 * pow((z - mu) / sigma, 2)) / sqrt(2 * M_PI * sigma * sigma); @@ -99,7 +79,10 @@ TEST(GaussianMixture, GaussianMixtureModel) { double mu0 = 1.0, mu1 = 3.0; double sigma = 2.0; - auto hbn = GaussianMixtureModel(mu0, mu1, sigma, sigma); + HybridBayesNet hbn; + std::vector means{Vector1(mu0), Vector1(mu1)}; + hbn.emplace_shared(Z(0), m, means, sigma); + hbn.push_back(mixing); // At the halfway point between the means, we should get P(m|z)=0.5 double midway = mu1 - mu0; @@ -133,7 +116,11 @@ TEST(GaussianMixture, GaussianMixtureModel2) { double mu0 = 1.0, mu1 = 3.0; double sigma0 = 8.0, sigma1 = 4.0; - auto hbn = GaussianMixtureModel(mu0, mu1, sigma0, sigma1); + HybridBayesNet hbn; + std::vector> parameters{{Vector1(mu0), sigma0}, + {Vector1(mu1), sigma1}}; + hbn.emplace_shared(Z(0), m, parameters); + hbn.push_back(mixing); // We get zMax=3.1333 by finding the maximum value of the function, at which // point the mode m==1 is about twice as probable as m==0. From 85e57f1c20839bdf2e7ed12e2388488faacf4b7a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Sep 2024 22:05:07 -0700 Subject: [PATCH 05/14] Use new constructors --- gtsam/hybrid/HybridBayesNet.h | 7 +- gtsam/hybrid/tests/testHybridBayesNet.cpp | 101 ++++++++-------------- 2 files changed, 43 insertions(+), 65 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index d5d50afd29..22a43f3bd3 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -44,9 +44,14 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { /// @name Standard Constructors /// @{ - /** Construct empty Bayes net */ + /// Construct empty Bayes net. HybridBayesNet() = default; + /// Constructor that takes an initializer list of shared pointers. + HybridBayesNet( + std::initializer_list conditionals) + : Base(conditionals) {} + /// @} /// @name Testable /// @{ diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 67005b470d..e52d611bd5 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -32,7 +33,6 @@ using namespace std; using namespace gtsam; -using noiseModel::Isotropic; using symbol_shorthand::M; using symbol_shorthand::X; using symbol_shorthand::Z; @@ -92,39 +92,51 @@ TEST(HybridBayesNet, Tiny) { } /* ****************************************************************************/ -// Test evaluate for a hybrid Bayes net P(X0|X1) P(X1|Asia) P(Asia). -TEST(HybridBayesNet, evaluateHybrid) { - const auto continuousConditional = GaussianConditional::sharedMeanAndStddev( - X(0), 2 * I_1x1, X(1), Vector1(-4.0), 5.0); +// Hybrid Bayes net P(X0|X1) P(X1|Asia) P(Asia). +namespace different_sigmas { +const auto gc = GaussianConditional::sharedMeanAndStddev(X(0), 2 * I_1x1, X(1), + Vector1(-4.0), 5.0); - const SharedDiagonal model0 = noiseModel::Diagonal::Sigmas(Vector1(2.0)), - model1 = noiseModel::Diagonal::Sigmas(Vector1(3.0)); +const std::vector> parms{{Vector1(5), 2.0}, + {Vector1(2), 3.0}}; +const auto hgc = std::make_shared(X(1), Asia, parms); - const auto conditional0 = std::make_shared( - X(1), Vector1::Constant(5), I_1x1, model0), - conditional1 = std::make_shared( - X(1), Vector1::Constant(2), I_1x1, model1); +const auto prior = std::make_shared(Asia, "99/1"); +auto wrap = [](const auto& c) { + return std::make_shared(c); +}; +const HybridBayesNet bayesNet{wrap(gc), wrap(hgc), wrap(prior)}; - // Create hybrid Bayes net. - HybridBayesNet bayesNet; - bayesNet.push_back(continuousConditional); - bayesNet.emplace_shared( - Asia, std::vector{conditional0, conditional1}); - bayesNet.emplace_shared(Asia, "99/1"); +// Create values at which to evaluate. +HybridValues values{{{X(0), Vector1(-6)}, {X(1), Vector1(1)}}, {{asiaKey, 0}}}; +} // namespace different_sigmas - // Create values at which to evaluate. - HybridValues values; - values.insert(asiaKey, 0); - values.insert(X(0), Vector1(-6)); - values.insert(X(1), Vector1(1)); +/* ****************************************************************************/ +// Test evaluate for a hybrid Bayes net P(X0|X1) P(X1|Asia) P(Asia). +TEST(HybridBayesNet, evaluateHybrid) { + using namespace different_sigmas; - const double conditionalProbability = - continuousConditional->evaluate(values.continuous()); - const double mixtureProbability = conditional0->evaluate(values.continuous()); + const double conditionalProbability = gc->evaluate(values.continuous()); + const double mixtureProbability = hgc->evaluate(values); EXPECT_DOUBLES_EQUAL(conditionalProbability * mixtureProbability * 0.99, bayesNet.evaluate(values), 1e-9); } +/* ****************************************************************************/ +// Test error for a hybrid Bayes net P(X0|X1) P(X1|Asia) P(Asia). +TEST(HybridBayesNet, Error) { + using namespace different_sigmas; + + AlgebraicDecisionTree actual = bayesNet.errorTree(values.continuous()); + + // Regression. + // Manually added all the error values from the 3 conditional types. + AlgebraicDecisionTree expected( + {Asia}, std::vector{2.33005033585, 5.38619084965}); + + EXPECT(assert_equal(expected, actual)); +} + /* ****************************************************************************/ // Test choosing an assignment of conditionals TEST(HybridBayesNet, Choose) { @@ -154,45 +166,6 @@ TEST(HybridBayesNet, Choose) { *gbn.at(3))); } -/* ****************************************************************************/ -// Test error for a hybrid Bayes net P(X0|X1) P(X1|Asia) P(Asia). -TEST(HybridBayesNet, Error) { - const auto continuousConditional = GaussianConditional::sharedMeanAndStddev( - X(0), 2 * I_1x1, X(1), Vector1(-4.0), 5.0); - - const SharedDiagonal model0 = noiseModel::Diagonal::Sigmas(Vector1(2.0)), - model1 = noiseModel::Diagonal::Sigmas(Vector1(3.0)); - - const auto conditional0 = std::make_shared( - X(1), Vector1::Constant(5), I_1x1, model0), - conditional1 = std::make_shared( - X(1), Vector1::Constant(2), I_1x1, model1); - - auto gm = std::make_shared( - Asia, std::vector{conditional0, conditional1}); - // Create hybrid Bayes net. - HybridBayesNet bayesNet; - bayesNet.push_back(continuousConditional); - bayesNet.push_back(gm); - bayesNet.emplace_shared(Asia, "99/1"); - - // Create values at which to evaluate. - HybridValues values; - values.insert(asiaKey, 0); - values.insert(X(0), Vector1(-6)); - values.insert(X(1), Vector1(1)); - - AlgebraicDecisionTree actual_errors = - bayesNet.errorTree(values.continuous()); - - // Regression. - // Manually added all the error values from the 3 conditional types. - AlgebraicDecisionTree expected_errors( - {Asia}, std::vector{2.33005033585, 5.38619084965}); - - EXPECT(assert_equal(expected_errors, actual_errors)); -} - /* ****************************************************************************/ // Test Bayes net optimize TEST(HybridBayesNet, OptimizeAssignment) { From 3a5d7a46480559e315e437ab5d54de4e51704a31 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Sep 2024 22:15:35 -0700 Subject: [PATCH 06/14] Remove one bougie constructor --- gtsam/hybrid/HybridGaussianConditional.cpp | 21 --------------------- gtsam/hybrid/HybridGaussianConditional.h | 11 ----------- gtsam/hybrid/tests/testGaussianMixture.cpp | 5 +++-- 3 files changed, 3 insertions(+), 34 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index d0f812d73c..5e80f802a2 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -62,21 +62,6 @@ struct HybridGaussianConditional::ConstructorHelper { } } - /// Construct from means and a single sigma. - ConstructorHelper(Key x, const DiscreteKey mode, - const std::vector &means, double sigma) - : nrFrontals(1), minNegLogConstant(0) { - std::vector gcs; - for (const auto &mean : means) { - auto c = GaussianConditional::sharedMeanAndStddev(x, mean, sigma); - gcs.push_back(c); - } - conditionals = Conditionals({mode}, gcs); - pairs = FactorValuePairs(conditionals, [](const auto &c) { - return GaussianFactorValuePair{c, 0.0}; - }); - } - /// Construct from means and a sigmas. ConstructorHelper(Key x, const DiscreteKey mode, const std::vector> ¶meters) @@ -110,12 +95,6 @@ HybridGaussianConditional::HybridGaussianConditional( : HybridGaussianConditional(DiscreteKeys{mode}, Conditionals({mode}, conditionals)) {} -HybridGaussianConditional::HybridGaussianConditional( - Key x, const DiscreteKey mode, const std::vector &means, - double sigma) - : HybridGaussianConditional(DiscreteKeys{mode}, - ConstructorHelper(x, mode, means, sigma)) {} - HybridGaussianConditional::HybridGaussianConditional( Key x, const DiscreteKey mode, const std::vector> ¶meters) diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 662318837d..79cafece7c 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -87,17 +87,6 @@ class GTSAM_EXPORT HybridGaussianConditional const DiscreteKey &mode, const std::vector &conditionals); - /** - * @brief Construct from vector of means and a single sigma. - * - * @param x The continuous key. - * @param mode The discrete key. - * @param means The means for the Gaussian conditionals. - * @param sigma The standard deviation for the Gaussian conditionals. - */ - HybridGaussianConditional(Key x, const DiscreteKey mode, - const std::vector &means, double sigma); - /** * @brief Construct from vector of means and sigmas. * diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index 87ef5e25d1..7d227f2d74 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -80,8 +80,9 @@ TEST(GaussianMixture, GaussianMixtureModel) { double sigma = 2.0; HybridBayesNet hbn; - std::vector means{Vector1(mu0), Vector1(mu1)}; - hbn.emplace_shared(Z(0), m, means, sigma); + std::vector> parameters{{Vector1(mu0), sigma}, + {Vector1(mu1), sigma}}; + hbn.emplace_shared(Z(0), m, parameters); hbn.push_back(mixing); // At the halfway point between the means, we should get P(m|z)=0.5 From ad9fd1969e994ac845d853382ab54b289e059f51 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Sep 2024 23:19:02 -0700 Subject: [PATCH 07/14] Add conditional constructors as well --- gtsam/hybrid/HybridGaussianConditional.cpp | 74 ++++++++++++++++--- gtsam/hybrid/HybridGaussianConditional.h | 26 ++++--- gtsam/hybrid/tests/TinyHybridExample.h | 8 +- gtsam/hybrid/tests/testGaussianMixture.cpp | 4 +- gtsam/hybrid/tests/testHybridBayesNet.cpp | 2 +- gtsam/hybrid/tests/testHybridEstimation.cpp | 20 ++--- .../tests/testHybridGaussianFactorGraph.cpp | 30 +++----- gtsam/hybrid/tests/testHybridMotionModel.cpp | 11 +-- 8 files changed, 110 insertions(+), 65 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 5e80f802a2..998afb1a32 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -33,14 +33,14 @@ namespace gtsam { /* *******************************************************************************/ -struct HybridGaussianConditional::ConstructorHelper { +struct HybridGaussianConditional::Helper { std::optional nrFrontals; FactorValuePairs pairs; Conditionals conditionals; double minNegLogConstant; /// Construct from tree of GaussianConditionals. - ConstructorHelper(const Conditionals &conditionals) + Helper(const Conditionals &conditionals) : conditionals(conditionals), minNegLogConstant(std::numeric_limits::infinity()) { auto func = [this](const GaussianConditional::shared_ptr &c) @@ -63,14 +63,54 @@ struct HybridGaussianConditional::ConstructorHelper { } /// Construct from means and a sigmas. - ConstructorHelper(Key x, const DiscreteKey mode, - const std::vector> ¶meters) + Helper(const DiscreteKey mode, Key key, + const std::vector> ¶meters) : nrFrontals(1), minNegLogConstant(std::numeric_limits::infinity()) { std::vector gcs; std::vector fvs; for (const auto &[mean, sigma] : parameters) { - auto c = GaussianConditional::sharedMeanAndStddev(x, mean, sigma); + auto c = GaussianConditional::sharedMeanAndStddev(key, mean, sigma); + double value = c->negLogConstant(); + minNegLogConstant = std::min(minNegLogConstant, value); + gcs.push_back(c); + fvs.push_back({c, value}); + } + conditionals = Conditionals({mode}, gcs); + pairs = FactorValuePairs({mode}, fvs); + } + + /// Construct from means and a sigmas. + Helper(const DiscreteKey mode, Key key, // + const Matrix &A, Key parent, + const std::vector> ¶meters) + : nrFrontals(1), + minNegLogConstant(std::numeric_limits::infinity()) { + std::vector gcs; + std::vector fvs; + for (const auto &[mean, sigma] : parameters) { + auto c = + GaussianConditional::sharedMeanAndStddev(key, A, parent, mean, sigma); + double value = c->negLogConstant(); + minNegLogConstant = std::min(minNegLogConstant, value); + gcs.push_back(c); + fvs.push_back({c, value}); + } + conditionals = Conditionals({mode}, gcs); + pairs = FactorValuePairs({mode}, fvs); + } + + /// Construct from means and a sigmas. + Helper(const DiscreteKey mode, Key key, // + const Matrix &A1, Key parent1, const Matrix &A2, Key parent2, + const std::vector> ¶meters) + : nrFrontals(1), + minNegLogConstant(std::numeric_limits::infinity()) { + std::vector gcs; + std::vector fvs; + for (const auto &[mean, sigma] : parameters) { + auto c = GaussianConditional::sharedMeanAndStddev(key, A1, parent1, A2, + parent2, mean, sigma); double value = c->negLogConstant(); minNegLogConstant = std::min(minNegLogConstant, value); gcs.push_back(c); @@ -83,7 +123,7 @@ struct HybridGaussianConditional::ConstructorHelper { /* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKeys &discreteParents, const ConstructorHelper &helper) + const DiscreteKeys &discreteParents, const Helper &helper) : BaseFactor(discreteParents, helper.pairs), BaseConditional(*helper.nrFrontals), conditionals_(helper.conditionals), @@ -96,16 +136,30 @@ HybridGaussianConditional::HybridGaussianConditional( Conditionals({mode}, conditionals)) {} HybridGaussianConditional::HybridGaussianConditional( - Key x, const DiscreteKey mode, + const DiscreteKey mode, Key key, // const std::vector> ¶meters) : HybridGaussianConditional(DiscreteKeys{mode}, - ConstructorHelper(x, mode, parameters)) {} + Helper(mode, key, parameters)) {} + +HybridGaussianConditional::HybridGaussianConditional( + const DiscreteKey mode, Key key, // + const Matrix &A, Key parent, + const std::vector> ¶meters) + : HybridGaussianConditional(DiscreteKeys{mode}, + Helper(mode, key, A, parent, parameters)) {} + +HybridGaussianConditional::HybridGaussianConditional( + const DiscreteKey mode, Key key, // + const Matrix &A1, Key parent1, const Matrix &A2, Key parent2, + const std::vector> ¶meters) + : HybridGaussianConditional( + DiscreteKeys{mode}, + Helper(mode, key, A1, parent1, A2, parent2, parameters)) {} HybridGaussianConditional::HybridGaussianConditional( const DiscreteKeys &discreteParents, const HybridGaussianConditional::Conditionals &conditionals) - : HybridGaussianConditional(discreteParents, - ConstructorHelper(conditionals)) {} + : HybridGaussianConditional(discreteParents, Helper(conditionals)) {} /* *******************************************************************************/ const HybridGaussianConditional::Conditionals & diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 79cafece7c..7c14f00085 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -87,15 +87,21 @@ class GTSAM_EXPORT HybridGaussianConditional const DiscreteKey &mode, const std::vector &conditionals); - /** - * @brief Construct from vector of means and sigmas. - * - * @param x The continuous key. - * @param mode The discrete key. - * @param parameters The means and sigmas for the Gaussian conditionals. - */ + /// Construct from mean `mu_i` and `sigma_i`. + HybridGaussianConditional( + const DiscreteKey mode, Key key, // + const std::vector> ¶meters); + + /// Construct from conditional mean `A1 p1 + b_i` and `sigma_i`. + HybridGaussianConditional( + const DiscreteKey mode, Key key, // + const Matrix &A, Key parent, + const std::vector> ¶meters); + + /// Construct from conditional mean `A1 p1 + A2 p2 + b_i` and `sigma_i`. HybridGaussianConditional( - Key x, const DiscreteKey mode, + const DiscreteKey mode, Key key, // + const Matrix &A1, Key parent1, const Matrix &A2, Key parent2, const std::vector> ¶meters); /** @@ -194,11 +200,11 @@ class GTSAM_EXPORT HybridGaussianConditional private: /// Helper struct for private constructor. - struct ConstructorHelper; + struct Helper; /// Private constructor that uses helper struct above. HybridGaussianConditional(const DiscreteKeys &discreteParents, - const ConstructorHelper &helper); + const Helper &helper); /// Convert to a DecisionTree of Gaussian factor graphs. GaussianFactorGraphTree asGaussianFactorGraphTree() const; diff --git a/gtsam/hybrid/tests/TinyHybridExample.h b/gtsam/hybrid/tests/TinyHybridExample.h index f2ff7dde2c..1862e8a318 100644 --- a/gtsam/hybrid/tests/TinyHybridExample.h +++ b/gtsam/hybrid/tests/TinyHybridExample.h @@ -41,12 +41,12 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1, HybridBayesNet bayesNet; // Create hybrid Gaussian factor z_i = x0 + noise for each measurement. + std::vector> measurementModels{{Z_1x1, 0.5}, + {Z_1x1, 3.0}}; for (size_t i = 0; i < num_measurements; i++) { const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode; - 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(mode_i, conditionals); + bayesNet.emplace_shared(mode_i, Z(i), I_1x1, + X(0), measurementModels); } // Create prior on X(0). diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index 7d227f2d74..d6eac9c38a 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -82,7 +82,7 @@ TEST(GaussianMixture, GaussianMixtureModel) { HybridBayesNet hbn; std::vector> parameters{{Vector1(mu0), sigma}, {Vector1(mu1), sigma}}; - hbn.emplace_shared(Z(0), m, parameters); + hbn.emplace_shared(m, Z(0), parameters); hbn.push_back(mixing); // At the halfway point between the means, we should get P(m|z)=0.5 @@ -120,7 +120,7 @@ TEST(GaussianMixture, GaussianMixtureModel2) { HybridBayesNet hbn; std::vector> parameters{{Vector1(mu0), sigma0}, {Vector1(mu1), sigma1}}; - hbn.emplace_shared(Z(0), m, parameters); + hbn.emplace_shared(m, Z(0), parameters); hbn.push_back(mixing); // We get zMax=3.1333 by finding the maximum value of the function, at which diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index e52d611bd5..c33ff84549 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -99,7 +99,7 @@ const auto gc = GaussianConditional::sharedMeanAndStddev(X(0), 2 * I_1x1, X(1), const std::vector> parms{{Vector1(5), 2.0}, {Vector1(2), 3.0}}; -const auto hgc = std::make_shared(X(1), Asia, parms); +const auto hgc = std::make_shared(Asia, X(1), parms); const auto prior = std::make_shared(Asia, "99/1"); auto wrap = [](const auto& c) { diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 672f475034..25d6974512 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -573,12 +573,10 @@ TEST(HybridEstimation, ModeSelection) { 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(mode, conditionals); + std::vector> parameters{{Z_1x1, noise_loose}, + {Z_1x1, noise_tight}}; + bn.emplace_shared(mode, Z(0), I_1x1, X(0), -I_1x1, + X(1), parameters); VectorValues vv; vv.insert(Z(0), Z_1x1); @@ -605,12 +603,10 @@ TEST(HybridEstimation, ModeSelection2) { 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(mode, conditionals); + std::vector> parameters{{Z_3x1, noise_loose}, + {Z_3x1, noise_tight}}; + bn.emplace_shared(mode, Z(0), I_3x3, X(0), -I_3x3, + X(1), parameters); VectorValues vv; vv.insert(Z(0), Z_3x1); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 38429ac603..6aef603868 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -803,11 +803,8 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) { // mode-dependent: 1 is low-noise, 0 is high-noise. // Create hybrid Gaussian factor 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(m1, conditionals); - bn.push_back(gm); + std::vector> parms{{Z_1x1, 3}, {Z_1x1, 0.5}}; + bn.emplace_shared(m1, Z(0), I_1x1, X(0), parms); // Create prior on X(0). bn.push_back( @@ -912,32 +909,27 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) { // NOTE: we add reverse topological so we can sample from the Bayes net.: // Add measurements: + std::vector> measurementModels{{Z_1x1, 3}, + {Z_1x1, 0.5}}; for (size_t t : {0, 1, 2}) { // Create hybrid Gaussian factor 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(noise_mode_t, conditionals); + bn.emplace_shared(noise_mode_t, Z(t), I_1x1, + X(t), measurementModels); // Create prior on discrete mode N(t): bn.emplace_shared(noise_mode_t, "20/80"); } - // Add motion models: + // Add motion models. TODO(frank): why are they exactly the same? + std::vector> motionModels{{Z_1x1, 0.2}, + {Z_1x1, 0.2}}; for (size_t t : {2, 1}) { // Create hybrid Gaussian factor 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(motion_model_t, - conditionals); - bn.push_back(gm); + bn.emplace_shared(motion_model_t, X(t), I_1x1, + X(t - 1), motionModels); // Create prior on motion model M(t): bn.emplace_shared(motion_model_t, "40/60"); diff --git a/gtsam/hybrid/tests/testHybridMotionModel.cpp b/gtsam/hybrid/tests/testHybridMotionModel.cpp index 1a2a09e153..cbfdc7fe4e 100644 --- a/gtsam/hybrid/tests/testHybridMotionModel.cpp +++ b/gtsam/hybrid/tests/testHybridMotionModel.cpp @@ -55,13 +55,10 @@ void addMeasurement(HybridBayesNet &hbn, Key z_key, Key x_key, double sigma) { /// Create hybrid motion model p(x1 | x0, m1) static HybridGaussianConditional::shared_ptr CreateHybridMotionModel( double mu0, double mu1, double sigma0, double sigma1) { - auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); - auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); - auto c0 = make_shared(X(1), Vector1(mu0), I_1x1, X(0), - -I_1x1, model0), - c1 = make_shared(X(1), Vector1(mu1), I_1x1, X(0), - -I_1x1, model1); - return std::make_shared(m1, std::vector{c0, c1}); + std::vector> motionModels{{Vector1(mu0), sigma0}, + {Vector1(mu1), sigma1}}; + return std::make_shared(m1, X(1), I_1x1, X(0), + motionModels); } /// Create two state Bayes network with 1 or two measurement models From 14d0f9f1ef4dde40a8d510fa918c3097b6e8bec6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Sep 2024 23:51:58 -0700 Subject: [PATCH 08/14] Avoid copy/pasta --- gtsam/hybrid/HybridGaussianConditional.cpp | 105 +++++++++------------ 1 file changed, 45 insertions(+), 60 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 998afb1a32..068bd2e5d4 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -39,12 +39,55 @@ struct HybridGaussianConditional::Helper { Conditionals conditionals; double minNegLogConstant; + using GC = GaussianConditional; + using P = std::vector>; + + // Common code for three constructors below: + template + void initialize(const DiscreteKey &mode, const P &p, Create create) { + nrFrontals = 1; + minNegLogConstant = std::numeric_limits::infinity(); + + std::vector fvs; + std::vector gcs; + for (const auto &[mean, sigma] : p) { + auto c = create(mean, sigma); + double value = c->negLogConstant(); + minNegLogConstant = std::min(minNegLogConstant, value); + fvs.push_back({c, value}); + gcs.push_back(c); + } + + conditionals = Conditionals({mode}, gcs); + pairs = FactorValuePairs({mode}, fvs); + } + + // Constructors for different types of GaussianConditionals: + + Helper(const DiscreteKey &mode, Key x0, const P &p) { + initialize(mode, p, [x0](const Vector &mean, double sigma) { + return GC::sharedMeanAndStddev(x0, mean, sigma); + }); + } + + Helper(const DiscreteKey &mode, Key x0, const Matrix &A, Key x1, const P &p) { + initialize(mode, p, [x0, A, x1](const Vector &mean, double sigma) { + return GC::sharedMeanAndStddev(x0, A, x1, mean, sigma); + }); + } + + Helper(const DiscreteKey &mode, Key x0, // + const Matrix &A1, Key x1, const Matrix &A2, Key x2, const P &p) { + initialize(mode, p, [x0, A1, x1, A2, x2](const Vector &mean, double sigma) { + return GC::sharedMeanAndStddev(x0, A1, x1, A2, x2, mean, sigma); + }); + } + /// Construct from tree of GaussianConditionals. Helper(const Conditionals &conditionals) : conditionals(conditionals), minNegLogConstant(std::numeric_limits::infinity()) { - auto func = [this](const GaussianConditional::shared_ptr &c) - -> GaussianFactorValuePair { + auto func = [this](const GC::shared_ptr &c) -> GaussianFactorValuePair { double value = 0.0; if (c) { if (!nrFrontals.has_value()) { @@ -61,64 +104,6 @@ struct HybridGaussianConditional::Helper { "HybridGaussianConditional: need at least one frontal variable."); } } - - /// Construct from means and a sigmas. - Helper(const DiscreteKey mode, Key key, - const std::vector> ¶meters) - : nrFrontals(1), - minNegLogConstant(std::numeric_limits::infinity()) { - std::vector gcs; - std::vector fvs; - for (const auto &[mean, sigma] : parameters) { - auto c = GaussianConditional::sharedMeanAndStddev(key, mean, sigma); - double value = c->negLogConstant(); - minNegLogConstant = std::min(minNegLogConstant, value); - gcs.push_back(c); - fvs.push_back({c, value}); - } - conditionals = Conditionals({mode}, gcs); - pairs = FactorValuePairs({mode}, fvs); - } - - /// Construct from means and a sigmas. - Helper(const DiscreteKey mode, Key key, // - const Matrix &A, Key parent, - const std::vector> ¶meters) - : nrFrontals(1), - minNegLogConstant(std::numeric_limits::infinity()) { - std::vector gcs; - std::vector fvs; - for (const auto &[mean, sigma] : parameters) { - auto c = - GaussianConditional::sharedMeanAndStddev(key, A, parent, mean, sigma); - double value = c->negLogConstant(); - minNegLogConstant = std::min(minNegLogConstant, value); - gcs.push_back(c); - fvs.push_back({c, value}); - } - conditionals = Conditionals({mode}, gcs); - pairs = FactorValuePairs({mode}, fvs); - } - - /// Construct from means and a sigmas. - Helper(const DiscreteKey mode, Key key, // - const Matrix &A1, Key parent1, const Matrix &A2, Key parent2, - const std::vector> ¶meters) - : nrFrontals(1), - minNegLogConstant(std::numeric_limits::infinity()) { - std::vector gcs; - std::vector fvs; - for (const auto &[mean, sigma] : parameters) { - auto c = GaussianConditional::sharedMeanAndStddev(key, A1, parent1, A2, - parent2, mean, sigma); - double value = c->negLogConstant(); - minNegLogConstant = std::min(minNegLogConstant, value); - gcs.push_back(c); - fvs.push_back({c, value}); - } - conditionals = Conditionals({mode}, gcs); - pairs = FactorValuePairs({mode}, fvs); - } }; /* *******************************************************************************/ From a25a0a923bebb50c531f725df0ff732d8c9db244 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 29 Sep 2024 00:13:08 -0700 Subject: [PATCH 09/14] perfect forwarding magic --- gtsam/hybrid/HybridGaussianConditional.cpp | 45 ++++++---------------- 1 file changed, 12 insertions(+), 33 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 068bd2e5d4..c348b85f0a 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -26,11 +26,10 @@ #include #include #include +#include #include -#include "gtsam/linear/JacobianFactor.h" - namespace gtsam { /* *******************************************************************************/ struct HybridGaussianConditional::Helper { @@ -42,47 +41,27 @@ struct HybridGaussianConditional::Helper { using GC = GaussianConditional; using P = std::vector>; - // Common code for three constructors below: - template - void initialize(const DiscreteKey &mode, const P &p, Create create) { + /// Construct from a vector of mean and sigma pairs, plus extra args. + template + Helper(const DiscreteKey &mode, const P &p, Args &&...args) { nrFrontals = 1; minNegLogConstant = std::numeric_limits::infinity(); std::vector fvs; std::vector gcs; for (const auto &[mean, sigma] : p) { - auto c = create(mean, sigma); - double value = c->negLogConstant(); + auto gaussianConditional = + GC::sharedMeanAndStddev(std::forward(args)..., mean, sigma); + double value = gaussianConditional->negLogConstant(); minNegLogConstant = std::min(minNegLogConstant, value); - fvs.push_back({c, value}); - gcs.push_back(c); + fvs.push_back({gaussianConditional, value}); + gcs.push_back(gaussianConditional); } conditionals = Conditionals({mode}, gcs); pairs = FactorValuePairs({mode}, fvs); } - // Constructors for different types of GaussianConditionals: - - Helper(const DiscreteKey &mode, Key x0, const P &p) { - initialize(mode, p, [x0](const Vector &mean, double sigma) { - return GC::sharedMeanAndStddev(x0, mean, sigma); - }); - } - - Helper(const DiscreteKey &mode, Key x0, const Matrix &A, Key x1, const P &p) { - initialize(mode, p, [x0, A, x1](const Vector &mean, double sigma) { - return GC::sharedMeanAndStddev(x0, A, x1, mean, sigma); - }); - } - - Helper(const DiscreteKey &mode, Key x0, // - const Matrix &A1, Key x1, const Matrix &A2, Key x2, const P &p) { - initialize(mode, p, [x0, A1, x1, A2, x2](const Vector &mean, double sigma) { - return GC::sharedMeanAndStddev(x0, A1, x1, A2, x2, mean, sigma); - }); - } - /// Construct from tree of GaussianConditionals. Helper(const Conditionals &conditionals) : conditionals(conditionals), @@ -124,14 +103,14 @@ HybridGaussianConditional::HybridGaussianConditional( const DiscreteKey mode, Key key, // const std::vector> ¶meters) : HybridGaussianConditional(DiscreteKeys{mode}, - Helper(mode, key, parameters)) {} + Helper(mode, parameters, key)) {} HybridGaussianConditional::HybridGaussianConditional( const DiscreteKey mode, Key key, // const Matrix &A, Key parent, const std::vector> ¶meters) : HybridGaussianConditional(DiscreteKeys{mode}, - Helper(mode, key, A, parent, parameters)) {} + Helper(mode, parameters, key, A, parent)) {} HybridGaussianConditional::HybridGaussianConditional( const DiscreteKey mode, Key key, // @@ -139,7 +118,7 @@ HybridGaussianConditional::HybridGaussianConditional( const std::vector> ¶meters) : HybridGaussianConditional( DiscreteKeys{mode}, - Helper(mode, key, A1, parent1, A2, parent2, parameters)) {} + Helper(mode, parameters, key, A1, parent1, A2, parent2)) {} HybridGaussianConditional::HybridGaussianConditional( const DiscreteKeys &discreteParents, From 4fa192248de37473fbc4a7e8ba8c9549bfa7c590 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 29 Sep 2024 00:21:40 -0700 Subject: [PATCH 10/14] Address o1 review comments --- gtsam/hybrid/HybridGaussianConditional.cpp | 17 ++++++++++------- gtsam/hybrid/HybridGaussianConditional.h | 6 +++--- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index c348b85f0a..b7c79a5b27 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -43,18 +43,20 @@ struct HybridGaussianConditional::Helper { /// Construct from a vector of mean and sigma pairs, plus extra args. template - Helper(const DiscreteKey &mode, const P &p, Args &&...args) { + explicit Helper(const DiscreteKey &mode, const P &p, Args &&...args) { nrFrontals = 1; minNegLogConstant = std::numeric_limits::infinity(); std::vector fvs; std::vector gcs; + fvs.reserve(p.size()); + gcs.reserve(p.size()); for (const auto &[mean, sigma] : p) { auto gaussianConditional = GC::sharedMeanAndStddev(std::forward(args)..., mean, sigma); double value = gaussianConditional->negLogConstant(); minNegLogConstant = std::min(minNegLogConstant, value); - fvs.push_back({gaussianConditional, value}); + fvs.emplace_back(gaussianConditional, value); gcs.push_back(gaussianConditional); } @@ -63,7 +65,7 @@ struct HybridGaussianConditional::Helper { } /// Construct from tree of GaussianConditionals. - Helper(const Conditionals &conditionals) + explicit Helper(const Conditionals &conditionals) : conditionals(conditionals), minNegLogConstant(std::numeric_limits::infinity()) { auto func = [this](const GC::shared_ptr &c) -> GaussianFactorValuePair { @@ -80,7 +82,8 @@ struct HybridGaussianConditional::Helper { pairs = FactorValuePairs(conditionals, func); if (!nrFrontals.has_value()) { throw std::runtime_error( - "HybridGaussianConditional: need at least one frontal variable."); + "HybridGaussianConditional: need at least one frontal variable. " + "Provided conditionals do not contain any frontal variables."); } } }; @@ -100,20 +103,20 @@ HybridGaussianConditional::HybridGaussianConditional( Conditionals({mode}, conditionals)) {} HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKey mode, Key key, // + const DiscreteKey &mode, Key key, // const std::vector> ¶meters) : HybridGaussianConditional(DiscreteKeys{mode}, Helper(mode, parameters, key)) {} HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKey mode, Key key, // + const DiscreteKey &mode, Key key, // const Matrix &A, Key parent, const std::vector> ¶meters) : HybridGaussianConditional(DiscreteKeys{mode}, Helper(mode, parameters, key, A, parent)) {} HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKey mode, Key key, // + const DiscreteKey &mode, Key key, // const Matrix &A1, Key parent1, const Matrix &A2, Key parent2, const std::vector> ¶meters) : HybridGaussianConditional( diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 7c14f00085..16fc0500f6 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -89,18 +89,18 @@ class GTSAM_EXPORT HybridGaussianConditional /// Construct from mean `mu_i` and `sigma_i`. HybridGaussianConditional( - const DiscreteKey mode, Key key, // + const DiscreteKey &mode, Key key, // const std::vector> ¶meters); /// Construct from conditional mean `A1 p1 + b_i` and `sigma_i`. HybridGaussianConditional( - const DiscreteKey mode, Key key, // + const DiscreteKey &mode, Key key, // const Matrix &A, Key parent, const std::vector> ¶meters); /// Construct from conditional mean `A1 p1 + A2 p2 + b_i` and `sigma_i`. HybridGaussianConditional( - const DiscreteKey mode, Key key, // + const DiscreteKey &mode, Key key, // const Matrix &A1, Key parent1, const Matrix &A2, Key parent2, const std::vector> ¶meters); From cc79d0432dbf1278800680687fd12aa368c92784 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 29 Sep 2024 00:34:31 -0700 Subject: [PATCH 11/14] Doxygen --- gtsam/hybrid/HybridGaussianConditional.h | 38 ++++++++++++++++++++---- 1 file changed, 32 insertions(+), 6 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 16fc0500f6..bd7849d332 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -87,18 +87,44 @@ class GTSAM_EXPORT HybridGaussianConditional const DiscreteKey &mode, const std::vector &conditionals); - /// Construct from mean `mu_i` and `sigma_i`. + /** + * @brief Constructs a HybridGaussianConditional with means mu_i and + * standard deviations sigma_i. + * + * @param mode The discrete mode key. + * @param key The key for this conditional variable. + * @param parameters A vector of pairs (mu_i, sigma_i). + */ HybridGaussianConditional( - const DiscreteKey &mode, Key key, // + const DiscreteKey &mode, Key key, const std::vector> ¶meters); - /// Construct from conditional mean `A1 p1 + b_i` and `sigma_i`. + /** + * @brief Constructs a HybridGaussianConditional with conditional means + * A × parent + b_i and standard deviations sigma_i. + * + * @param mode The discrete mode key. + * @param key The key for this conditional variable. + * @param A The matrix A. + * @param parent The key of the parent variable. + * @param parameters A vector of pairs (b_i, sigma_i). + */ HybridGaussianConditional( - const DiscreteKey &mode, Key key, // - const Matrix &A, Key parent, + const DiscreteKey &mode, Key key, const Matrix &A, Key parent, const std::vector> ¶meters); - /// Construct from conditional mean `A1 p1 + A2 p2 + b_i` and `sigma_i`. + /** + * @brief Constructs a HybridGaussianConditional with conditional means + * A1 × parent1 + A2 × parent2 + b_i and standard deviations sigma_i. + * + * @param mode The discrete mode key. + * @param key The key for this conditional variable. + * @param A1 The first matrix. + * @param parent1 The key of the first parent variable. + * @param A2 The second matrix. + * @param parent2 The key of the second parent variable. + * @param parameters A vector of pairs (b_i, sigma_i). + */ HybridGaussianConditional( const DiscreteKey &mode, Key key, // const Matrix &A1, Key parent1, const Matrix &A2, Key parent2, From cfebf0cca009ddc44b4a9d980725c98262375419 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 29 Sep 2024 10:26:14 -0400 Subject: [PATCH 12/14] formatting --- gtsam/hybrid/HybridGaussianConditional.cpp | 5 +++++ gtsam/hybrid/HybridGaussianFactorGraph.cpp | 1 + 2 files changed, 6 insertions(+) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index b7c79a5b27..e8d2ec35f1 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -96,18 +96,21 @@ HybridGaussianConditional::HybridGaussianConditional( conditionals_(helper.conditionals), negLogConstant_(helper.minNegLogConstant) {} +/* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( const DiscreteKey &mode, const std::vector &conditionals) : HybridGaussianConditional(DiscreteKeys{mode}, Conditionals({mode}, conditionals)) {} +/* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( const DiscreteKey &mode, Key key, // const std::vector> ¶meters) : HybridGaussianConditional(DiscreteKeys{mode}, Helper(mode, parameters, key)) {} +/* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( const DiscreteKey &mode, Key key, // const Matrix &A, Key parent, @@ -115,6 +118,7 @@ HybridGaussianConditional::HybridGaussianConditional( : HybridGaussianConditional(DiscreteKeys{mode}, Helper(mode, parameters, key, A, parent)) {} +/* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( const DiscreteKey &mode, Key key, // const Matrix &A1, Key parent1, const Matrix &A2, Key parent2, @@ -123,6 +127,7 @@ HybridGaussianConditional::HybridGaussianConditional( DiscreteKeys{mode}, Helper(mode, parameters, key, A1, parent1, A2, parent2)) {} +/* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( const DiscreteKeys &discreteParents, const HybridGaussianConditional::Conditionals &conditionals) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 787c0edd7d..c107aa8a8f 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -366,6 +366,7 @@ static std::shared_ptr createHybridGaussianFactor( return std::make_shared(discreteSeparator, newFactors); } +/* *******************************************************************************/ std::pair> HybridGaussianFactorGraph::eliminate(const Ordering &keys) const { // Since we eliminate all continuous variables first, From 3d8603b23b220e3af9e9845393ec9b88809f1870 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 29 Sep 2024 10:27:15 -0400 Subject: [PATCH 13/14] remove unnecessary includes --- gtsam/hybrid/tests/Switching.h | 7 +++---- gtsam/hybrid/tests/testHybridBayesNet.cpp | 1 - gtsam/hybrid/tests/testHybridEstimation.cpp | 1 - gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 1 - gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 1 - gtsam/hybrid/tests/testHybridNonlinearISAM.cpp | 1 - 6 files changed, 3 insertions(+), 9 deletions(-) diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 90137a4e30..82876fd2c4 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -24,17 +24,16 @@ #include #include #include +#include +#include #include #include +#include #include #include #include -#include "gtsam/linear/GaussianFactor.h" -#include "gtsam/linear/GaussianFactorGraph.h" -#include "gtsam/nonlinear/NonlinearFactor.h" - #pragma once namespace gtsam { diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index c33ff84549..b555f6bd9f 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include "Switching.h" diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 25d6974512..58decc695c 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -30,7 +30,6 @@ #include #include #include -#include #include #include #include diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 1c72268de0..70fa321ada 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include #include diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index b5f04bb990..4735c16573 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -28,7 +28,6 @@ #include #include #include -#include #include #include #include diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 2452ebe6d0..b804a176ba 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include #include From 2cf21007104fff0969722e0440b56cee1967d53c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 29 Sep 2024 09:40:30 -0700 Subject: [PATCH 14/14] address review comments --- gtsam/hybrid/HybridGaussianConditional.cpp | 32 ++++++++++------------ gtsam/hybrid/HybridGaussianConditional.h | 16 +++++------ gtsam/hybrid/tests/testGaussianMixture.cpp | 22 ++++++++------- 3 files changed, 34 insertions(+), 36 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index e8d2ec35f1..1712e06a9f 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -51,7 +51,7 @@ struct HybridGaussianConditional::Helper { std::vector gcs; fvs.reserve(p.size()); gcs.reserve(p.size()); - for (const auto &[mean, sigma] : p) { + for (auto &&[mean, sigma] : p) { auto gaussianConditional = GC::sharedMeanAndStddev(std::forward(args)..., mean, sigma); double value = gaussianConditional->negLogConstant(); @@ -96,38 +96,34 @@ HybridGaussianConditional::HybridGaussianConditional( conditionals_(helper.conditionals), negLogConstant_(helper.minNegLogConstant) {} -/* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKey &mode, + const DiscreteKey &discreteParent, const std::vector &conditionals) - : HybridGaussianConditional(DiscreteKeys{mode}, - Conditionals({mode}, conditionals)) {} + : HybridGaussianConditional(DiscreteKeys{discreteParent}, + Conditionals({discreteParent}, conditionals)) {} -/* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKey &mode, Key key, // + const DiscreteKey &discreteParent, Key key, // const std::vector> ¶meters) - : HybridGaussianConditional(DiscreteKeys{mode}, - Helper(mode, parameters, key)) {} + : HybridGaussianConditional(DiscreteKeys{discreteParent}, + Helper(discreteParent, parameters, key)) {} -/* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKey &mode, Key key, // + const DiscreteKey &discreteParent, Key key, // const Matrix &A, Key parent, const std::vector> ¶meters) - : HybridGaussianConditional(DiscreteKeys{mode}, - Helper(mode, parameters, key, A, parent)) {} + : HybridGaussianConditional( + DiscreteKeys{discreteParent}, + Helper(discreteParent, parameters, key, A, parent)) {} -/* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKey &mode, Key key, // + const DiscreteKey &discreteParent, Key key, // const Matrix &A1, Key parent1, const Matrix &A2, Key parent2, const std::vector> ¶meters) : HybridGaussianConditional( - DiscreteKeys{mode}, - Helper(mode, parameters, key, A1, parent1, A2, parent2)) {} + DiscreteKeys{discreteParent}, + Helper(discreteParent, parameters, key, A1, parent1, A2, parent2)) {} -/* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( const DiscreteKeys &discreteParents, const HybridGaussianConditional::Conditionals &conditionals) diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index bd7849d332..827b7f309d 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -79,45 +79,45 @@ class GTSAM_EXPORT HybridGaussianConditional /** * @brief Construct from one discrete key and vector of conditionals. * - * @param mode Single discrete parent variable + * @param discreteParent Single discrete parent variable * @param conditionals Vector of conditionals with the same size as the * cardinality of the discrete parent. */ HybridGaussianConditional( - const DiscreteKey &mode, + const DiscreteKey &discreteParent, const std::vector &conditionals); /** * @brief Constructs a HybridGaussianConditional with means mu_i and * standard deviations sigma_i. * - * @param mode The discrete mode key. + * @param discreteParent The discrete parent or "mode" key. * @param key The key for this conditional variable. * @param parameters A vector of pairs (mu_i, sigma_i). */ HybridGaussianConditional( - const DiscreteKey &mode, Key key, + const DiscreteKey &discreteParent, Key key, const std::vector> ¶meters); /** * @brief Constructs a HybridGaussianConditional with conditional means * A × parent + b_i and standard deviations sigma_i. * - * @param mode The discrete mode key. + * @param discreteParent The discrete parent or "mode" key. * @param key The key for this conditional variable. * @param A The matrix A. * @param parent The key of the parent variable. * @param parameters A vector of pairs (b_i, sigma_i). */ HybridGaussianConditional( - const DiscreteKey &mode, Key key, const Matrix &A, Key parent, + const DiscreteKey &discreteParent, Key key, const Matrix &A, Key parent, const std::vector> ¶meters); /** * @brief Constructs a HybridGaussianConditional with conditional means * A1 × parent1 + A2 × parent2 + b_i and standard deviations sigma_i. * - * @param mode The discrete mode key. + * @param discreteParent The discrete parent or "mode" key. * @param key The key for this conditional variable. * @param A1 The first matrix. * @param parent1 The key of the first parent variable. @@ -126,7 +126,7 @@ class GTSAM_EXPORT HybridGaussianConditional * @param parameters A vector of pairs (b_i, sigma_i). */ HybridGaussianConditional( - const DiscreteKey &mode, Key key, // + const DiscreteKey &discreteParent, Key key, // const Matrix &A1, Key parent1, const Matrix &A2, Key parent2, const std::vector> ¶meters); diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index d6eac9c38a..3256f5686c 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -79,15 +79,16 @@ TEST(GaussianMixture, GaussianMixtureModel) { double mu0 = 1.0, mu1 = 3.0; double sigma = 2.0; - HybridBayesNet hbn; + // Create a Gaussian mixture model p(z|m) with same sigma. + HybridBayesNet gmm; std::vector> parameters{{Vector1(mu0), sigma}, {Vector1(mu1), sigma}}; - hbn.emplace_shared(m, Z(0), parameters); - hbn.push_back(mixing); + gmm.emplace_shared(m, Z(0), parameters); + gmm.push_back(mixing); // At the halfway point between the means, we should get P(m|z)=0.5 double midway = mu1 - mu0; - auto pMid = SolveHBN(hbn, midway); + auto pMid = SolveHBN(gmm, midway); EXPECT(assert_equal(DiscreteConditional(m, "60/40"), pMid)); // Everywhere else, the result should be a sigmoid. @@ -96,7 +97,7 @@ TEST(GaussianMixture, GaussianMixtureModel) { const double expected = prob_m_z(mu0, mu1, sigma, sigma, z); // Workflow 1: convert HBN to HFG and solve - auto posterior1 = SolveHBN(hbn, z); + auto posterior1 = SolveHBN(gmm, z); EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8); // Workflow 2: directly specify HFG and solve @@ -117,16 +118,17 @@ TEST(GaussianMixture, GaussianMixtureModel2) { double mu0 = 1.0, mu1 = 3.0; double sigma0 = 8.0, sigma1 = 4.0; - HybridBayesNet hbn; + // Create a Gaussian mixture model p(z|m) with same sigma. + HybridBayesNet gmm; std::vector> parameters{{Vector1(mu0), sigma0}, {Vector1(mu1), sigma1}}; - hbn.emplace_shared(m, Z(0), parameters); - hbn.push_back(mixing); + gmm.emplace_shared(m, Z(0), parameters); + gmm.push_back(mixing); // We get zMax=3.1333 by finding the maximum value of the function, at which // point the mode m==1 is about twice as probable as m==0. double zMax = 3.133; - auto pMax = SolveHBN(hbn, zMax); + auto pMax = SolveHBN(gmm, zMax); EXPECT(assert_equal(DiscreteConditional(m, "42/58"), pMax, 1e-4)); // Everywhere else, the result should be a bell curve like function. @@ -135,7 +137,7 @@ TEST(GaussianMixture, GaussianMixtureModel2) { const double expected = prob_m_z(mu0, mu1, sigma0, sigma1, z); // Workflow 1: convert HBN to HFG and solve - auto posterior1 = SolveHBN(hbn, z); + auto posterior1 = SolveHBN(gmm, z); EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8); // Workflow 2: directly specify HFG and solve