From ad9fd1969e994ac845d853382ab54b289e059f51 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Sep 2024 23:19:02 -0700 Subject: [PATCH] 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 5e80f802a..998afb1a3 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 79cafece7..7c14f0008 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( - Key x, const DiscreteKey mode, + 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( + 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 f2ff7dde2..1862e8a31 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 7d227f2d7..d6eac9c38 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 e52d611bd..c33ff8454 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 672f47503..25d697451 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 38429ac60..6aef60386 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 1a2a09e15..cbfdc7fe4 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