From a4a0a2a4247bb6ba31b2b6b47d744e457c3b8e1d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 25 Sep 2024 18:48:01 -0700 Subject: [PATCH] Test different workflows --- gtsam/hybrid/tests/testGaussianMixture.cpp | 122 +++++++++++---------- 1 file changed, 67 insertions(+), 55 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index e976994f88..c6263c44dc 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -10,56 +10,35 @@ * -------------------------------------------------------------------------- */ /** - * @file testHybridGaussianFactor.cpp - * @brief Unit tests for HybridGaussianFactor + * @file testGaussianMixture.cpp + * @brief test hybrid elimination with a simple mixture model * @author Varun Agrawal - * @author Fan Jiang * @author Frank Dellaert - * @date December 2021 + * @date September 2024 */ -#include -#include #include -#include #include -#include -#include -#include -#include #include -#include -#include -#include -#include // Include for test suite #include -#include - -using namespace std; using namespace gtsam; using symbol_shorthand::M; -using symbol_shorthand::X; using symbol_shorthand::Z; -/** - * Closed form computation of P(m=1|z). - * If sigma0 == sigma1, it simplifies to a sigmoid function. - */ -static double prob_m_z(double mu0, double mu1, double sigma0, double sigma1, - double z) { - double x1 = ((z - mu0) / sigma0), x2 = ((z - mu1) / sigma1); - double d = sigma1 / sigma0; - double e = d * std::exp(-0.5 * (x1 * x1 - x2 * x2)); - return 1 / (1 + e); -}; - // Define mode key and an assignment m==1 static const DiscreteKey m(M(0), 2); static const DiscreteValues m1Assignment{{M(0), 1}}; +// Define a 50/50 prior on the mode +DiscreteConditional::shared_ptr mixing = + std::make_shared(m, "60/40"); + +// define Continuous keys +static const KeyVector continuousKeys{Z(0)}; + /** * 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. @@ -68,30 +47,45 @@ static const DiscreteValues m1Assignment{{M(0), 1}}; */ static HybridBayesNet GetGaussianMixtureModel(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 = make_shared(Z(0), Vector1(mu0), I_1x1, model0), - c1 = make_shared(Z(0), Vector1(mu1), I_1x1, model1); - - HybridBayesNet hbn; - hbn.emplace_shared(KeyVector{Z(0)}, KeyVector{}, m, + 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(continuousKeys, KeyVector{}, m, std::vector{c0, c1}); - - auto mixing = make_shared(m, "50/50"); hbn.push_back(mixing); - return hbn; } -/// Given p(z,m) and z, use eliminate to obtain P(m|z). -static DiscreteConditional solveForMeasurement(const HybridBayesNet &hbn, - double z) { - VectorValues given; - given.insert(Z(0), Vector1(z)); +/// 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); +}; + +/** + * Closed form computation of P(m=1|z). + * If sigma0 == sigma1, it simplifies to a sigmoid function. + * Hardcodes 60/40 prior on mode. + */ +static double prob_m_z(double mu0, double mu1, double sigma0, double sigma1, + double z) { + const double p0 = 0.6 * Gaussian(mu0, sigma0, z); + const double p1 = 0.4 * Gaussian(mu1, sigma1, z); + return p1 / (p0 + p1); +}; + +/// Given \phi(m;z)\phi(m) use eliminate to obtain P(m|z). +static DiscreteConditional solveHFG(const HybridGaussianFactorGraph &hfg) { + return *hfg.eliminateSequential()->at(0)->asDiscrete(); +} - HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - return *gfg.eliminateSequential()->at(0)->asDiscrete(); +/// Given p(z,m) and z, convert to HFG and solve. +static DiscreteConditional solveHBN(const HybridBayesNet &hbn, double z) { + VectorValues given{{Z(0), Vector1(z)}}; + return solveHFG(hbn.toFactorGraph(given)); } /* @@ -106,16 +100,25 @@ TEST(HybridGaussianFactor, GaussianMixtureModel) { // At the halfway point between the means, we should get P(m|z)=0.5 double midway = mu1 - mu0; - auto pMid = solveForMeasurement(hbn, midway); - EXPECT(assert_equal(DiscreteConditional(m, "50/50"), pMid)); + auto pMid = solveHBN(hbn, midway); + EXPECT(assert_equal(DiscreteConditional(m, "60/40"), pMid)); // Everywhere else, the result should be a sigmoid. for (const double shift : {-4, -2, 0, 2, 4}) { const double z = midway + shift; const double expected = prob_m_z(mu0, mu1, sigma, sigma, z); - auto posterior = solveForMeasurement(hbn, z); - EXPECT_DOUBLES_EQUAL(expected, posterior(m1Assignment), 1e-8); + // Workflow 1: convert HBN to HFG and solve + auto posterior1 = solveHBN(hbn, z); + EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8); + + // Workflow 2: directly specify HFG and solve + HybridGaussianFactorGraph hfg1; + hfg1.emplace_shared( + m, std::vector{Gaussian(mu0, sigma, z), Gaussian(mu1, sigma, z)}); + hfg1.push_back(mixing); + auto posterior2 = solveHFG(hfg1); + EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8); } } @@ -132,16 +135,25 @@ TEST(HybridGaussianFactor, GaussianMixtureModel2) { // 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 = solveForMeasurement(hbn, zMax); - EXPECT(assert_equal(DiscreteConditional(m, "32.56/67.44"), pMax, 1e-5)); + auto pMax = solveHBN(hbn, zMax); + EXPECT(assert_equal(DiscreteConditional(m, "42/58"), pMax, 1e-4)); // Everywhere else, the result should be a bell curve like function. for (const double shift : {-4, -2, 0, 2, 4}) { const double z = zMax + shift; const double expected = prob_m_z(mu0, mu1, sigma0, sigma1, z); - auto posterior = solveForMeasurement(hbn, z); - EXPECT_DOUBLES_EQUAL(expected, posterior(m1Assignment), 1e-8); + // Workflow 1: convert HBN to HFG and solve + auto posterior1 = solveHBN(hbn, z); + EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8); + + // Workflow 2: directly specify HFG and solve + HybridGaussianFactorGraph hfg; + hfg.emplace_shared( + m, std::vector{Gaussian(mu0, sigma0, z), Gaussian(mu1, sigma1, z)}); + hfg.push_back(mixing); + auto posterior2 = solveHFG(hfg); + EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8); } }