From 80d9a5a65fc4fdf0ef45921a971597c73ef3c14f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Sep 2024 10:27:43 -0400 Subject: [PATCH] remove duplicate test and focus only on direct specification --- .../hybrid/tests/testHybridGaussianFactor.cpp | 161 +----------------- 1 file changed, 7 insertions(+), 154 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index dff120855..8bf508022 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -741,152 +741,6 @@ TEST(HybridGaussianFactor, TwoStateModel4) { EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); } -/** - * @brief Helper function to specify a Hybrid Bayes Net - * P(X1)P(Z1 | X1, X2, M1) and convert it to a Hybrid Factor Graph - * ϕ(X1)ϕ(X1, X2, M1; Z1) by converting to likelihoods given Z1. - * - * We can specify either different means or different sigmas, - * or both for each hybrid factor component. - * - * @param values Initial values for linearization. - * @param means The mean values for the conditional components. - * @param sigmas Noise model sigma values (standard deviation). - * @param m1 The discrete mode key. - * @param z1 The measurement value. - * @return HybridGaussianFactorGraph - */ -static HybridGaussianFactorGraph GetFactorGraphFromBayesNet( - const gtsam::Values &values, const std::vector &means, - const std::vector &sigmas, DiscreteKey &m1, double z1 = 0.0) { - // Noise models - auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]); - auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]); - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); - - // HybridGaussianFactor component factors - auto f0 = - std::make_shared>(X(0), X(1), means[0], model0); - auto f1 = - std::make_shared>(X(0), X(1), means[1], model1); - - /// Get terms for each p^m(z1 | x1, x2) - Matrix H0_1, H0_2, H1_1, H1_2; - double x1 = values.at(X(0)), x2 = values.at(X(1)); - Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2); - std::vector> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/}, - // - {X(0), H0_1 /*Sp1*/}, - {X(1), H0_2 /*Tp2*/}}; - - Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2); - std::vector> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/}, - // - {X(0), H1_1 /*Sp1*/}, - {X(1), H1_2 /*Tp2*/}}; - // Create conditional P(Z1 | X1, X2, M1) - auto conditionals = std::vector{ - 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); - - // Create FG via toFactorGraph - gtsam::VectorValues measurements; - measurements.insert(Z(1), gtsam::I_1x1 * z1); // Set Z1 - HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements); - - // Linearized prior factor on X1 - auto prior = PriorFactor(X(0), x1, prior_noise).linearize(values); - mixture_fg.push_back(prior); - - return mixture_fg; -} - -/* ************************************************************************* */ -/** - * @brief Test Hybrid Factor Graph. - * - * 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. - * This is different from the TwoStateModel version since - * we use a factor with 2 continuous variables ϕ(x1, x2, m1) - * directly instead of a conditional. - * This serves as a good sanity check. - * - * P(Z1 | X1, X2, M1) has 2 conditionals each for the binary - * mode m1. - */ -TEST(HybridGaussianFactor, FactorGraphFromBayesNet) { - DiscreteKey m1(M(1), 2); - - Values values; - double x1 = 0.0, x2 = 1.75; - values.insert(X(0), x1); - values.insert(X(1), x2); - - // Different means, same sigma - std::vector means{0.0, 2.0}, sigmas{1e-0, 1e-0}; - - HybridGaussianFactorGraph hfg = - GetFactorGraphFromBayesNet(values, means, sigmas, m1, 0.0); - - { - // With no measurement on X2, each mode should be equally likely - auto bn = hfg.eliminateSequential(); - HybridValues actual = bn->optimize(); - - HybridValues expected( - VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(-1.75)}}, - DiscreteValues{{M(1), 0}}); - - EXPECT(assert_equal(expected, actual)); - - DiscreteValues dv0{{M(1), 0}}; - VectorValues cont0 = bn->optimize(dv0); - double error0 = bn->error(HybridValues(cont0, dv0)); - // regression - EXPECT_DOUBLES_EQUAL(0.69314718056, error0, 1e-9); - - DiscreteValues dv1{{M(1), 1}}; - VectorValues cont1 = bn->optimize(dv1); - double error1 = bn->error(HybridValues(cont1, dv1)); - EXPECT_DOUBLES_EQUAL(error0, error1, 1e-9); - } - { - // If we add a measurement on X2, we have more information to work with. - // Add a measurement on X2 - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); - GaussianConditional meas_z2(Z(2), Vector1(2.0), I_1x1, X(1), I_1x1, - prior_noise); - auto prior_x2 = meas_z2.likelihood(Vector1(x2)); - - hfg.push_back(prior_x2); - - auto bn = hfg.eliminateSequential(); - HybridValues actual = bn->optimize(); - - // regression - HybridValues expected( - VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(0.25)}}, - DiscreteValues{{M(1), 1}}); - EXPECT(assert_equal(expected, actual)); - - DiscreteValues dv0{{M(1), 0}}; - VectorValues cont0 = bn->optimize(dv0); - // regression - EXPECT_DOUBLES_EQUAL(2.12692448787, bn->error(HybridValues(cont0, dv0)), - 1e-9); - - DiscreteValues dv1{{M(1), 1}}; - VectorValues cont1 = bn->optimize(dv1); - // regression - EXPECT_DOUBLES_EQUAL(0.126928487854, bn->error(HybridValues(cont1, dv1)), - 1e-9); - } -} - namespace test_direct_factor_graph { /** * @brief Create a Factor Graph by directly specifying all @@ -902,10 +756,11 @@ namespace test_direct_factor_graph { */ static HybridGaussianFactorGraph CreateFactorGraph( const gtsam::Values &values, const std::vector &means, - const std::vector &sigmas, DiscreteKey &m1) { + const std::vector &sigmas, DiscreteKey &m1, + double measurement_noise = 1e-3) { auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]); auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]); - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); + auto prior_noise = noiseModel::Isotropic::Sigma(1, measurement_noise); auto f0 = std::make_shared>(X(0), X(1), means[0], model0) @@ -917,10 +772,10 @@ static HybridGaussianFactorGraph CreateFactorGraph( // Create HybridGaussianFactor std::vector factors{ {f0, ComputeLogNormalizer(model0)}, {f1, ComputeLogNormalizer(model1)}}; - HybridGaussianFactor mixtureFactor({X(0), X(1)}, {m1}, factors); + HybridGaussianFactor motionFactor({X(0), X(1)}, m1, factors); HybridGaussianFactorGraph hfg; - hfg.push_back(mixtureFactor); + hfg.push_back(motionFactor); hfg.push_back(PriorFactor(X(0), values.at(X(0)), prior_noise) .linearize(values)); @@ -1025,10 +880,8 @@ TEST(HybridGaussianFactor, DifferentCovariancesFG) { std::vector means = {0.0, 0.0}, sigmas = {1e2, 1e-2}; // Create FG with HybridGaussianFactor and prior on X1 - HybridGaussianFactorGraph mixture_fg = - CreateFactorGraph(values, means, sigmas, m1); - - auto hbn = mixture_fg.eliminateSequential(); + HybridGaussianFactorGraph fg = CreateFactorGraph(values, means, sigmas, m1); + auto hbn = fg.eliminateSequential(); VectorValues cv; cv.insert(X(0), Vector1(0.0));