From de68aecca5e894c3ddedc8f3e691cfc44c70cc1e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Sep 2024 15:45:14 -0400 Subject: [PATCH] fix tests --- .../hybrid/tests/testHybridGaussianFactor.cpp | 52 +++++++++---------- 1 file changed, 25 insertions(+), 27 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 44e60d451..f2248510e 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -221,8 +221,9 @@ double prob_m_z(double mu0, double mu1, double sigma0, double sigma1, return 1 / (1 + e); }; -static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, - double sigma0, double sigma1) { +static HybridBayesNet GetHybridGaussianConditionalModel(double mu0, double mu1, + double sigma0, + double sigma1) { DiscreteKey m(M(0), 2); Key z = Z(0); @@ -254,7 +255,7 @@ static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, * The resulting factor graph should eliminate to a Bayes net * which represents a sigmoid function. */ -TEST(HybridGaussianFactor, GaussianMixtureModel) { +TEST(HybridGaussianFactor, HybridGaussianConditionalModel) { using namespace test_gmm; double mu0 = 1.0, mu1 = 3.0; @@ -263,7 +264,7 @@ TEST(HybridGaussianFactor, GaussianMixtureModel) { DiscreteKey m(M(0), 2); Key z = Z(0); - auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma, sigma); + auto hbn = GetHybridGaussianConditionalModel(mu0, mu1, sigma, sigma); // The result should be a sigmoid. // So should be P(m=1|z) = 0.5 at z=3.0 - 1.0=2.0 @@ -326,7 +327,7 @@ TEST(HybridGaussianFactor, GaussianMixtureModel) { * which represents a Gaussian-like function * where m1>m0 close to 3.1333. */ -TEST(HybridGaussianFactor, GaussianMixtureModel2) { +TEST(HybridGaussianFactor, HybridGaussianConditionalModel2) { using namespace test_gmm; double mu0 = 1.0, mu1 = 3.0; @@ -335,7 +336,7 @@ TEST(HybridGaussianFactor, GaussianMixtureModel2) { DiscreteKey m(M(0), 2); Key z = Z(0); - auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma0, sigma1); + auto hbn = GetHybridGaussianConditionalModel(mu0, mu1, sigma0, sigma1); double m1_high = 3.133, lambda = 4; { @@ -393,7 +394,7 @@ namespace test_two_state_estimation { DiscreteKey m1(M(1), 2); -void addMeasurement(HybridBayesNet& hbn, Key z_key, Key x_key, double sigma) { +void addMeasurement(HybridBayesNet &hbn, Key z_key, Key x_key, double sigma) { auto measurement_model = noiseModel::Isotropic::Sigma(1, sigma); hbn.emplace_shared(z_key, Vector1(0.0), I_1x1, x_key, -I_1x1, measurement_model); @@ -414,7 +415,7 @@ static HybridGaussianConditional::shared_ptr CreateHybridMotionModel( /// Create two state Bayes network with 1 or two measurement models HybridBayesNet CreateBayesNet( - const HybridGaussianConditional::shared_ptr& hybridMotionModel, + const HybridGaussianConditional::shared_ptr &hybridMotionModel, bool add_second_measurement = false) { HybridBayesNet hbn; @@ -437,9 +438,9 @@ HybridBayesNet CreateBayesNet( /// Approximate the discrete marginal P(m1) using importance sampling std::pair approximateDiscreteMarginal( - const HybridBayesNet& hbn, - const HybridGaussianConditional::shared_ptr& hybridMotionModel, - const VectorValues& given, size_t N = 100000) { + const HybridBayesNet &hbn, + const HybridGaussianConditional::shared_ptr &hybridMotionModel, + const VectorValues &given, size_t N = 100000) { /// Create importance sampling network q(x0,x1,m) = p(x1|x0,m1) q(x0) P(m1), /// using q(x0) = N(z0, sigmaQ) to sample x0. HybridBayesNet q; @@ -758,7 +759,7 @@ static HybridGaussianFactorGraph GetFactorGraphFromBayesNet( auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]); auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); - // GaussianMixtureFactor component factors + // HybridGaussianFactor component factors auto f0 = std::make_shared>(X(0), X(1), means[0], model0); auto f1 = @@ -783,8 +784,8 @@ static HybridGaussianFactorGraph GetFactorGraphFromBayesNet( std::make_shared(terms0, 1, -d0, model0), std::make_shared(terms1, 1, -d1, model1)}; gtsam::HybridBayesNet bn; - bn.emplace_shared(KeyVector{Z(1)}, KeyVector{X(0), X(1)}, - DiscreteKeys{m1}, conditionals); + bn.emplace_shared( + KeyVector{Z(1)}, KeyVector{X(0), X(1)}, DiscreteKeys{m1}, conditionals); // Create FG via toFactorGraph gtsam::VectorValues measurements; @@ -812,7 +813,7 @@ static HybridGaussianFactorGraph GetFactorGraphFromBayesNet( * P(Z1 | X1, X2, M1) has 2 conditionals each for the binary * mode m1. */ -TEST(GaussianMixtureFactor, FactorGraphFromBayesNet) { +TEST(HybridGaussianFactor, FactorGraphFromBayesNet) { DiscreteKey m1(M(1), 2); Values values; @@ -889,8 +890,8 @@ namespace test_direct_factor_graph { * then perform linearization. * * @param values Initial values to linearize around. - * @param means The means of the GaussianMixtureFactor components. - * @param sigmas The covariances of the GaussianMixtureFactor components. + * @param means The means of the HybridGaussianFactor components. + * @param sigmas The covariances of the HybridGaussianFactor components. * @param m1 The discrete key. * @return HybridGaussianFactorGraph */ @@ -908,13 +909,10 @@ static HybridGaussianFactorGraph CreateFactorGraph( std::make_shared>(X(0), X(1), means[1], model1) ->linearize(values); - // Create GaussianMixtureFactor - std::vector factors{f0, f1}; - AlgebraicDecisionTree logNormalizers( - {m1}, std::vector{ComputeLogNormalizer(model0), - ComputeLogNormalizer(model1)}); - GaussianMixtureFactor mixtureFactor({X(0), X(1)}, {m1}, factors, - logNormalizers); + // Create HybridGaussianFactor + std::vector factors{ + {f0, ComputeLogNormalizer(model0)}, {f1, ComputeLogNormalizer(model1)}}; + HybridGaussianFactor mixtureFactor({X(0), X(1)}, {m1}, factors); HybridGaussianFactorGraph hfg; hfg.push_back(mixtureFactor); @@ -934,7 +932,7 @@ static HybridGaussianFactorGraph CreateFactorGraph( * | * M1 */ -TEST(GaussianMixtureFactor, DifferentMeansFG) { +TEST(HybridGaussianFactor, DifferentMeansFG) { using namespace test_direct_factor_graph; DiscreteKey m1(M(1), 2); @@ -1009,7 +1007,7 @@ TEST(GaussianMixtureFactor, DifferentMeansFG) { * | * M1 */ -TEST(GaussianMixtureFactor, DifferentCovariancesFG) { +TEST(HybridGaussianFactor, DifferentCovariancesFG) { using namespace test_direct_factor_graph; DiscreteKey m1(M(1), 2); @@ -1021,7 +1019,7 @@ TEST(GaussianMixtureFactor, DifferentCovariancesFG) { std::vector means = {0.0, 0.0}, sigmas = {1e2, 1e-2}; - // Create FG with GaussianMixtureFactor and prior on X1 + // Create FG with HybridGaussianFactor and prior on X1 HybridGaussianFactorGraph mixture_fg = CreateFactorGraph(values, means, sigmas, m1);