From 3d8383089bb19e0f60ce1afa545bf6e27de1e70e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Aug 2024 22:34:57 -0400 Subject: [PATCH 1/4] don't check if GaussianFactor has no contKeys since it may not (e.g. GaussianMixtureModel) --- gtsam/hybrid/GaussianMixtureFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 8471cef33..94bc09407 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -66,7 +66,7 @@ void GaussianMixtureFactor::print(const std::string &s, [&](const sharedFactor &gf) -> std::string { RedirectCout rd; std::cout << ":\n"; - if (gf && !gf->empty()) { + if (gf) { gf->print("", formatter); return rd.str(); } else { From 1f373fd136e1ac46a3e06b1f568e3bff1737989f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 00:48:32 -0400 Subject: [PATCH 2/4] add examples of Hybrid FGs from HBNs as unit tests --- .../tests/testGaussianMixtureFactor.cpp | 233 +++++++++++++++++- 1 file changed, 229 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index dfafb923b..0910d2f40 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -200,6 +200,228 @@ TEST(GaussianMixtureFactor, Error) { 4.0, mixtureFactor.error({continuousValues, discreteValues}), 1e-9); } +/* ************************************************************************* */ +/** + * Test a simple Gaussian Mixture Model represented as P(m)P(z|m) + * where m is a discrete variable and z is a continuous variable. + * m is binary and depending on m, we have 2 different means + * μ1 and μ2 for the Gaussian distribution around which we sample z. + * + * The resulting factor graph should eliminate to a Bayes net + * which represents a sigmoid function. + */ +TEST(GaussianMixtureFactor, GaussianMixtureModel) { + double mu0 = 1.0, mu1 = 3.0; + double sigma = 2.0; + auto model = noiseModel::Isotropic::Sigma(1, sigma); + + DiscreteKey m(M(0), 2); + Key z = Z(0); + + auto c0 = make_shared(z, Vector1(mu0), I_1x1, model), + c1 = make_shared(z, Vector1(mu1), I_1x1, model); + + auto gm = new GaussianMixture({z}, {}, {m}, {c0, c1}); + auto mixing = new DiscreteConditional(m, "0.5/0.5"); + + HybridBayesNet hbn; + hbn.emplace_back(gm); + hbn.emplace_back(mixing); + + // The result should be a sigmoid. + // So should be m = 0.5 at z=3.0 - 1.0=2.0 + VectorValues given; + given.insert(z, Vector1(mu1 - mu0)); + + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + HybridBayesNet expected; + expected.emplace_back(new DiscreteConditional(m, "0.5/0.5")); + + EXPECT(assert_equal(expected, *bn)); +} + +/* ************************************************************************* */ +/** + * Test a simple Gaussian Mixture Model represented as P(m)P(z|m) + * where m is a discrete variable and z is a continuous variable. + * m is binary and depending on m, we have 2 different means + * and covariances each for the + * Gaussian distribution around which we sample z. + * + * The resulting factor graph should eliminate to a Bayes net + * which represents a sigmoid function leaning towards + * the tighter covariance Gaussian. + */ +TEST(GaussianMixtureFactor, GaussianMixtureModel2) { + double mu0 = 1.0, mu1 = 3.0; + auto model0 = noiseModel::Isotropic::Sigma(1, 8.0); + auto model1 = noiseModel::Isotropic::Sigma(1, 4.0); + + DiscreteKey m(M(0), 2); + Key z = Z(0); + + auto c0 = make_shared(z, Vector1(mu0), I_1x1, model0), + c1 = make_shared(z, Vector1(mu1), I_1x1, model1); + + auto gm = new GaussianMixture({z}, {}, {m}, {c0, c1}); + auto mixing = new DiscreteConditional(m, "0.5/0.5"); + + HybridBayesNet hbn; + hbn.emplace_back(gm); + hbn.emplace_back(mixing); + + // The result should be a sigmoid leaning towards model1 + // since it has the tighter covariance. + // So should be m = 0.34/0.66 at z=3.0 - 1.0=2.0 + VectorValues given; + given.insert(z, Vector1(mu1 - mu0)); + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + HybridBayesNet expected; + expected.emplace_back( + new DiscreteConditional(m, "0.338561851224/0.661438148776")); + + EXPECT(assert_equal(expected, *bn)); +} + +/* ************************************************************************* */ +/** + * Test a model P(x0)P(z0|x0)p(x1|m1)p(z1|x1)p(m1). + * + * p(x1|m1) has different means and same covariance. + * + * Converting to a factor graph gives us + * P(x0)ϕ(x0)P(x1|m1)ϕ(x1)P(m1) + * + * If we only have a measurement on z0, then + * the probability of x1 should be 0.5/0.5. + * Getting a measurement on z1 gives use more information. + */ +TEST(GaussianMixtureFactor, TwoStateModel) { + double mu0 = 1.0, mu1 = 3.0; + auto model = noiseModel::Isotropic::Sigma(1, 2.0); + + DiscreteKey m1(M(1), 2); + Key z0 = Z(0), z1 = Z(1), x0 = X(0), x1 = X(1); + + auto c0 = make_shared(x1, Vector1(mu0), I_1x1, model), + c1 = make_shared(x1, Vector1(mu1), I_1x1, model); + + auto p_x0 = new GaussianConditional(x0, Vector1(0.0), I_1x1, + noiseModel::Isotropic::Sigma(1, 1.0)); + auto p_z0x0 = new GaussianConditional(z0, Vector1(0.0), I_1x1, x0, -I_1x1, + noiseModel::Isotropic::Sigma(1, 1.0)); + auto p_x1m1 = new GaussianMixture({x1}, {}, {m1}, {c0, c1}); + auto p_z1x1 = new GaussianConditional(z1, Vector1(0.0), I_1x1, x1, -I_1x1, + noiseModel::Isotropic::Sigma(1, 3.0)); + auto p_m1 = new DiscreteConditional(m1, "0.5/0.5"); + + HybridBayesNet hbn; + hbn.emplace_back(p_x0); + hbn.emplace_back(p_z0x0); + hbn.emplace_back(p_x1m1); + hbn.emplace_back(p_m1); + + VectorValues given; + given.insert(z0, Vector1(0.5)); + + { + // Start with no measurement on x1, only on x0 + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + // Since no measurement on x1, we hedge our bets + DiscreteConditional expected(m1, "0.5/0.5"); + + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()))); + } + + { + // Now we add a measurement z1 on x1 + hbn.emplace_back(p_z1x1); + + given.insert(z1, Vector1(2.2)); + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + // Since we have a measurement on z2, we get a definite result + DiscreteConditional expected(m1, "0.4923083/0.5076917"); + + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 1e-6)); + } +} + +/* ************************************************************************* */ +/** + * Test a model P(x0)P(z0|x0)p(x1|m1)p(z1|x1)p(m1). + * + * p(x1|m1) has different means and different covariances. + * + * Converting to a factor graph gives us + * P(x0)ϕ(x0)P(x1|m1)ϕ(x1)P(m1) + * + * If we only have a measurement on z0, then + * the probability of x1 should be the ratio of covariances. + * Getting a measurement on z1 gives use more information. + */ +TEST(GaussianMixtureFactor, TwoStateModel2) { + double mu0 = 1.0, mu1 = 3.0; + auto model0 = noiseModel::Isotropic::Sigma(1, 6.0); + auto model1 = noiseModel::Isotropic::Sigma(1, 4.0); + + DiscreteKey m1(M(1), 2); + Key z0 = Z(0), z1 = Z(1), x0 = X(0), x1 = X(1); + + auto c0 = make_shared(x1, Vector1(mu0), I_1x1, model0), + c1 = make_shared(x1, Vector1(mu1), I_1x1, model1); + + auto p_x0 = new GaussianConditional(x0, Vector1(0.0), I_1x1, + noiseModel::Isotropic::Sigma(1, 1.0)); + auto p_z0x0 = new GaussianConditional(z0, Vector1(0.0), I_1x1, x0, -I_1x1, + noiseModel::Isotropic::Sigma(1, 1.0)); + auto p_x1m1 = new GaussianMixture({x1}, {}, {m1}, {c0, c1}); + auto p_z1x1 = new GaussianConditional(z1, Vector1(0.0), I_1x1, x1, -I_1x1, + noiseModel::Isotropic::Sigma(1, 3.0)); + auto p_m1 = new DiscreteConditional(m1, "0.5/0.5"); + + HybridBayesNet hbn; + hbn.emplace_back(p_x0); + hbn.emplace_back(p_z0x0); + hbn.emplace_back(p_x1m1); + hbn.emplace_back(p_m1); + + VectorValues given; + given.insert(z0, Vector1(0.5)); + + { + // Start with no measurement on x1, only on x0 + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + // Since no measurement on x1, we get the ratio of covariances. + DiscreteConditional expected(m1, "0.6/0.4"); + + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()))); + } + + { + // Now we add a measurement z1 on x1 + hbn.emplace_back(p_z1x1); + + given.insert(z1, Vector1(2.2)); + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + // Since we have a measurement on z2, we get a definite result + DiscreteConditional expected(m1, "0.52706646/0.47293354"); + + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 1e-6)); + } +} + /** * @brief Helper function to specify a Hybrid Bayes Net * {P(X1) P(Z1 | X1, X2, M1)} and convert it to a Hybrid Factor Graph @@ -271,11 +493,12 @@ HybridGaussianFactorGraph GetFactorGraphFromBayesNet( * * We specify a hybrid Bayes network P(Z | X, M) =p(X1)p(Z1 | X1, X2, M1), * which is then converted to a factor graph by specifying Z1. - * - * p(Z1 | X1, X2, M1) has 2 factors each for the binary mode m1, with only the - * means being different. + * This is a different case since now we have a hybrid factor + * with 2 continuous variables ϕ(x1, x2, m1). + * p(Z1 | X1, X2, M1) has 2 factors each for the binary + * mode m1, with only the means being different. */ -TEST(GaussianMixtureFactor, DifferentMeansHBN) { +TEST(GaussianMixtureFactor, DifferentMeans) { DiscreteKey m1(M(1), 2); Values values; @@ -355,6 +578,8 @@ TEST(GaussianMixtureFactor, DifferentMeansHBN) { /** * @brief Test components with differing covariances * but with a Bayes net P(Z|X, M) converted to a FG. + * Same as the DifferentMeans example but in this case, + * we keep the means the same and vary the covariances. */ TEST(GaussianMixtureFactor, DifferentCovariances) { DiscreteKey m1(M(1), 2); From 7358effe038d0cb82c9b378a21c8afabfa0ea269 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 00:49:28 -0400 Subject: [PATCH 3/4] allow for GaussianMixtureFactors with 0 continuous keys --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index bf11a50fc..cb8ceed20 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -242,6 +242,18 @@ discreteElimination(const HybridGaussianFactorGraph &factors, for (auto &f : factors) { if (auto df = dynamic_pointer_cast(f)) { dfg.push_back(df); + } else if (auto gmf = dynamic_pointer_cast(f)) { + // Case where we have a GaussianMixtureFactor with no continuous keys. + // In this case, compute discrete probabilities. + auto probability = + [&](const GaussianFactor::shared_ptr &factor) -> double { + if (!factor) return 0.0; + return exp(-factor->error(VectorValues())); + }; + dfg.emplace_shared( + gmf->discreteKeys(), + DecisionTree(gmf->factors(), probability)); + } else if (auto orphan = dynamic_pointer_cast(f)) { // Ignore orphaned clique. // TODO(dellaert): is this correct? If so explain here. From 88c2ad55bea7d23148029f895cec4f453ca3b8ad Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 00:49:36 -0400 Subject: [PATCH 4/4] better docstring --- gtsam/hybrid/GaussianMixtureFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index b1be6ef1d..2459af259 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -80,8 +80,8 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { * @param continuousKeys A vector of keys representing continuous variables. * @param discreteKeys A vector of keys representing discrete variables and * their cardinalities. - * @param factors The decision tree of Gaussian factors stored as the mixture - * density. + * @param factors The decision tree of Gaussian factors stored + * as the mixture density. */ GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,