diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index c6263c44d..be4ba2ff4 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -11,15 +11,22 @@ /** * @file testGaussianMixture.cpp - * @brief test hybrid elimination with a simple mixture model + * @brief Test hybrid elimination with a simple mixture model * @author Varun Agrawal * @author Frank Dellaert * @date September 2024 */ +#include #include +#include #include +#include +#include +#include #include +#include +#include // Include for test suite #include @@ -29,15 +36,15 @@ using symbol_shorthand::M; using symbol_shorthand::Z; // Define mode key and an assignment m==1 -static const DiscreteKey m(M(0), 2); -static const DiscreteValues m1Assignment{{M(0), 1}}; +const DiscreteKey m(M(0), 2); +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)}; +const KeyVector continuousKeys{Z(0)}; /** * Create a simple Gaussian Mixture Model represented as p(z|m)P(m) @@ -45,8 +52,8 @@ static const KeyVector continuousKeys{Z(0)}; * The "mode" m is binary and depending on m, we have 2 different means * μ1 and μ2 for the Gaussian density p(z|m). */ -static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, - double sigma0, double sigma1) { +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); @@ -70,37 +77,37 @@ double Gaussian(double mu, double sigma, double 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) { +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) { +DiscreteConditional SolveHFG(const HybridGaussianFactorGraph &hfg) { return *hfg.eliminateSequential()->at(0)->asDiscrete(); } /// Given p(z,m) and z, convert to HFG and solve. -static DiscreteConditional solveHBN(const HybridBayesNet &hbn, double z) { +DiscreteConditional SolveHBN(const HybridBayesNet &hbn, double z) { VectorValues given{{Z(0), Vector1(z)}}; - return solveHFG(hbn.toFactorGraph(given)); + return SolveHFG(hbn.toFactorGraph(given)); } /* * Test a Gaussian Mixture Model P(m)p(z|m) with same sigma. * The posterior, as a function of z, should be a sigmoid function. */ -TEST(HybridGaussianFactor, GaussianMixtureModel) { +TEST(GaussianMixture, GaussianMixtureModel) { double mu0 = 1.0, mu1 = 3.0; double sigma = 2.0; - auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma, sigma); + auto hbn = GaussianMixtureModel(mu0, mu1, sigma, sigma); // 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(hbn, midway); EXPECT(assert_equal(DiscreteConditional(m, "60/40"), pMid)); // Everywhere else, the result should be a sigmoid. @@ -109,7 +116,7 @@ TEST(HybridGaussianFactor, 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(hbn, z); EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8); // Workflow 2: directly specify HFG and solve @@ -117,7 +124,7 @@ TEST(HybridGaussianFactor, GaussianMixtureModel) { hfg1.emplace_shared( m, std::vector{Gaussian(mu0, sigma, z), Gaussian(mu1, sigma, z)}); hfg1.push_back(mixing); - auto posterior2 = solveHFG(hfg1); + auto posterior2 = SolveHFG(hfg1); EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8); } } @@ -126,16 +133,16 @@ TEST(HybridGaussianFactor, GaussianMixtureModel) { * Test a Gaussian Mixture Model P(m)p(z|m) with different sigmas. * The posterior, as a function of z, should be a unimodal function. */ -TEST(HybridGaussianFactor, GaussianMixtureModel2) { +TEST(GaussianMixture, GaussianMixtureModel2) { double mu0 = 1.0, mu1 = 3.0; double sigma0 = 8.0, sigma1 = 4.0; - auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma0, sigma1); + auto hbn = GaussianMixtureModel(mu0, mu1, sigma0, sigma1); // 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(hbn, zMax); EXPECT(assert_equal(DiscreteConditional(m, "42/58"), pMax, 1e-4)); // Everywhere else, the result should be a bell curve like function. @@ -144,7 +151,7 @@ TEST(HybridGaussianFactor, 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(hbn, z); EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8); // Workflow 2: directly specify HFG and solve @@ -152,7 +159,7 @@ TEST(HybridGaussianFactor, GaussianMixtureModel2) { hfg.emplace_shared( m, std::vector{Gaussian(mu0, sigma0, z), Gaussian(mu1, sigma1, z)}); hfg.push_back(mixing); - auto posterior2 = solveHFG(hfg); + auto posterior2 = SolveHFG(hfg); EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8); } }