From 958a298fee629e1ad960db71c36da9eace09dc11 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 12 Sep 2024 15:37:30 -0700 Subject: [PATCH 1/8] Clean up small things --- gtsam/hybrid/tests/testGaussianMixture.cpp | 1 - .../tests/testGaussianMixtureFactor.cpp | 32 +++++++------------ 2 files changed, 11 insertions(+), 22 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index 4da61912e1..7c241acb9c 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -31,7 +31,6 @@ #include using namespace gtsam; -using noiseModel::Isotropic; using symbol_shorthand::M; using symbol_shorthand::X; using symbol_shorthand::Z; diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index fcd9dd08f8..22426305ed 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -35,8 +35,6 @@ using namespace std; using namespace gtsam; -using noiseModel::Isotropic; -using symbol_shorthand::F; using symbol_shorthand::M; using symbol_shorthand::X; using symbol_shorthand::Z; @@ -387,40 +385,38 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel2) { namespace test_two_state_estimation { +DiscreteKey m1(M(1), 2); + /// Create Two State Bayes Network with measurements static HybridBayesNet CreateBayesNet(double mu0, double mu1, double sigma0, double sigma1, bool add_second_measurement = false, double prior_sigma = 1e-3, double measurement_sigma = 3.0) { - DiscreteKey m1(M(1), 2); - Key z0 = Z(0), z1 = Z(1); - Key x0 = X(0), x1 = X(1); - HybridBayesNet hbn; auto measurement_model = noiseModel::Isotropic::Sigma(1, measurement_sigma); // Add measurement P(z0 | x0) auto p_z0 = std::make_shared( - z0, Vector1(0.0), -I_1x1, x0, I_1x1, measurement_model); + Z(0), Vector1(0.0), -I_1x1, X(0), I_1x1, measurement_model); hbn.push_back(p_z0); // Add hybrid motion model auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); - auto c0 = make_shared(x1, Vector1(mu0), I_1x1, x0, + auto c0 = make_shared(X(1), Vector1(mu0), I_1x1, X(0), -I_1x1, model0), - c1 = make_shared(x1, Vector1(mu1), I_1x1, x0, + c1 = make_shared(X(1), Vector1(mu1), I_1x1, X(0), -I_1x1, model1); auto motion = std::make_shared( - KeyVector{x1}, KeyVector{x0}, DiscreteKeys{m1}, std::vector{c0, c1}); + KeyVector{X(1)}, KeyVector{X(0)}, DiscreteKeys{m1}, std::vector{c0, c1}); hbn.push_back(motion); if (add_second_measurement) { // Add second measurement auto p_z1 = std::make_shared( - z1, Vector1(0.0), -I_1x1, x1, I_1x1, measurement_model); + Z(1), Vector1(0.0), -I_1x1, X(1), I_1x1, measurement_model); hbn.push_back(p_z1); } @@ -452,14 +448,11 @@ TEST(GaussianMixtureFactor, TwoStateModel) { double mu0 = 1.0, mu1 = 3.0; double sigma = 2.0; - DiscreteKey m1(M(1), 2); - Key z0 = Z(0), z1 = Z(1); - // Start with no measurement on x1, only on x0 HybridBayesNet hbn = CreateBayesNet(mu0, mu1, sigma, sigma, false); VectorValues given; - given.insert(z0, Vector1(0.5)); + given.insert(Z(0), Vector1(0.5)); { HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); @@ -476,7 +469,7 @@ TEST(GaussianMixtureFactor, TwoStateModel) { // If we see z1=2.6 (> 2.5 which is the halfway point), // discrete mode should say m1=1 - given.insert(z1, Vector1(2.6)); + given.insert(Z(1), Vector1(2.6)); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); @@ -508,14 +501,11 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); - DiscreteKey m1(M(1), 2); - Key z0 = Z(0), z1 = Z(1); - // Start with no measurement on x1, only on x0 HybridBayesNet hbn = CreateBayesNet(mu0, mu1, sigma0, sigma1, false); VectorValues given; - given.insert(z0, Vector1(0.5)); + given.insert(Z(0), Vector1(0.5)); { // Start with no measurement on x1, only on x0 @@ -552,7 +542,7 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { // Now we add a measurement z1 on x1 hbn = CreateBayesNet(mu0, mu1, sigma0, sigma1, true); - given.insert(z1, Vector1(2.2)); + given.insert(Z(1), Vector1(2.2)); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); { From 2d58a5bac068b7ad10b22e34ac50377b2081e581 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 12 Sep 2024 18:10:42 -0700 Subject: [PATCH 2/8] Importance sampling --- .../tests/testGaussianMixtureFactor.cpp | 168 ++++++++++++------ 1 file changed, 117 insertions(+), 51 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 22426305ed..81a68826a0 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -18,7 +18,9 @@ * @date December 2021 */ +#include #include +#include #include #include #include @@ -33,6 +35,8 @@ // Include for test suite #include +#include + using namespace std; using namespace gtsam; using symbol_shorthand::M; @@ -387,46 +391,89 @@ namespace test_two_state_estimation { DiscreteKey m1(M(1), 2); -/// Create Two State Bayes Network with measurements -static HybridBayesNet CreateBayesNet(double mu0, double mu1, double sigma0, - double sigma1, - bool add_second_measurement = false, - double prior_sigma = 1e-3, - double measurement_sigma = 3.0) { - HybridBayesNet hbn; - - auto measurement_model = noiseModel::Isotropic::Sigma(1, measurement_sigma); - // Add measurement P(z0 | x0) - auto p_z0 = std::make_shared( - Z(0), Vector1(0.0), -I_1x1, X(0), I_1x1, measurement_model); - hbn.push_back(p_z0); - - // Add hybrid motion model +/// Create hybrid motion model p(x1 | x0, m1) +static GaussianMixture::shared_ptr CreateHybridMotionModel(double mu0, + double mu1, + double sigma0, + double sigma1) { auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); auto c0 = make_shared(X(1), Vector1(mu0), I_1x1, X(0), -I_1x1, model0), c1 = make_shared(X(1), Vector1(mu1), I_1x1, X(0), -I_1x1, model1); - - auto motion = std::make_shared( + return std::make_shared( KeyVector{X(1)}, KeyVector{X(0)}, DiscreteKeys{m1}, std::vector{c0, c1}); - hbn.push_back(motion); +} + +/// Create two state Bayes network with 1 or two measurement models +HybridBayesNet CreateBayesNet( + const GaussianMixture::shared_ptr& hybridMotionModel, + bool add_second_measurement = false) { + HybridBayesNet hbn; + // Add measurement model p(z0 | x0) + const double measurement_sigma = 3.0; + auto measurement_model = noiseModel::Isotropic::Sigma(1, measurement_sigma); + hbn.emplace_shared(Z(0), Vector1(0.0), I_1x1, X(0), + -I_1x1, measurement_model); + + // Optionally add second measurement model p(z1 | x1) if (add_second_measurement) { - // Add second measurement - auto p_z1 = std::make_shared( - Z(1), Vector1(0.0), -I_1x1, X(1), I_1x1, measurement_model); - hbn.push_back(p_z1); + hbn.emplace_shared(Z(1), Vector1(0.0), I_1x1, X(1), + -I_1x1, measurement_model); } + // Add hybrid motion model + hbn.push_back(hybridMotionModel); + // Discrete uniform prior. - auto p_m1 = std::make_shared(m1, "0.5/0.5"); - hbn.push_back(p_m1); + hbn.emplace_shared(m1, "0.5/0.5"); return hbn; } +/// Create importance sampling network p(x1| x0, m1) p(x0) P(m1), +/// using Q(x0) = N(z0, sigma_Q) to sample from p(x0) +HybridBayesNet CreateProposalNet( + const GaussianMixture::shared_ptr& hybridMotionModel, double z0, + double sigma_Q) { + HybridBayesNet hbn; + + // Add hybrid motion model + hbn.push_back(hybridMotionModel); + + // Add proposal Q(x0) for x0 + auto measurement_model = noiseModel::Isotropic::Sigma(1, sigma_Q); + hbn.emplace_shared( + GaussianConditional::FromMeanAndStddev(X(0), Vector1(z0), sigma_Q)); + + // Discrete uniform prior. + hbn.emplace_shared(m1, "0.5/0.5"); + + return hbn; +} + +/// Approximate the discrete marginal P(m1) using importance sampling +/// Not typically called as expensive, but values are used in the tests. +void approximateDiscreteMarginal(const HybridBayesNet& hbn, + const HybridBayesNet& proposalNet, + const VectorValues& given) { + // Do importance sampling + double w0 = 0.0, w1 = 0.0; + std::mt19937_64 rng(44); + for (int i = 0; i < 50000; i++) { + HybridValues sample = proposalNet.sample(&rng); + sample.insert(given); + double weight = hbn.evaluate(sample) / proposalNet.evaluate(sample); + (sample.atDiscrete(m1.first) == 0) ? w0 += weight : w1 += weight; + } + double sumWeights = w0 + w1; + double pm1 = w1 / sumWeights; + std::cout << "p(m0) ~ " << 1.0 - pm1 << std::endl; + std::cout << "p(m1) ~ " << pm1 << std::endl; +} + } // namespace test_two_state_estimation /* ************************************************************************* */ @@ -446,37 +493,48 @@ TEST(GaussianMixtureFactor, TwoStateModel) { using namespace test_two_state_estimation; double mu0 = 1.0, mu1 = 3.0; - double sigma = 2.0; + double sigma = 0.5; + auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma, sigma); // Start with no measurement on x1, only on x0 - HybridBayesNet hbn = CreateBayesNet(mu0, mu1, sigma, sigma, false); + double z0 = 0.5; VectorValues given; - given.insert(Z(0), Vector1(0.5)); + given.insert(Z(0), Vector1(z0)); + + // Create proposal network for importance sampling + auto proposalNet = CreateProposalNet(hybridMotionModel, z0, 3.0); + EXPECT_LONGS_EQUAL(3, proposalNet.size()); { + HybridBayesNet hbn = CreateBayesNet(hybridMotionModel); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); // Since no measurement on x1, we hedge our bets + // Importance sampling run with 50k samples gives 0.49934/0.50066 + // approximateDiscreteMarginal(hbn, proposalNet, given); DiscreteConditional expected(m1, "0.5/0.5"); EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()))); } { // Now we add a measurement z1 on x1 - hbn = CreateBayesNet(mu0, mu1, sigma, sigma, true); + HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); - // If we see z1=2.6 (> 2.5 which is the halfway point), + // If we see z1=4.5 (>> 2.5 which is the halfway point), // discrete mode should say m1=1 - given.insert(Z(1), Vector1(2.6)); + const double z1 = 4.5; + given.insert(Z(1), Vector1(z1)); + 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.49772729/0.50227271"); - // regression - EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 1e-6)); + // Since we have a measurement on x1, we get a definite result + // Values taken from an importance sampling run with 50k samples: + // approximateDiscreteMarginal(hbn, proposalNet, given); + DiscreteConditional expected(m1, "0.446629/0.553371"); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.01)); } } @@ -498,22 +556,24 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { double mu0 = 1.0, mu1 = 3.0; double sigma0 = 6.0, sigma1 = 4.0; - auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); - auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); + auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1); // Start with no measurement on x1, only on x0 - HybridBayesNet hbn = CreateBayesNet(mu0, mu1, sigma0, sigma1, false); - + double z0 = 0.5; VectorValues given; - given.insert(Z(0), Vector1(0.5)); + given.insert(Z(0), Vector1(z0)); + + // Create proposal network for importance sampling + // uncomment this and the approximateDiscreteMarginal calls to run + // auto proposalNet = CreateProposalNet(hybridMotionModel, z0, 3.0); { - // Start with no measurement on x1, only on x0 + HybridBayesNet hbn = CreateBayesNet(hybridMotionModel); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); { VectorValues vv{ - {X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}, {Z(0), Vector1(0.5)}}; + {X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}, {Z(0), Vector1(z0)}}; HybridValues hv0(vv, DiscreteValues{{M(1), 0}}), hv1(vv, DiscreteValues{{M(1), 1}}); EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), @@ -521,7 +581,7 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { } { VectorValues vv{ - {X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}, {Z(0), Vector1(0.5)}}; + {X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}, {Z(0), Vector1(z0)}}; HybridValues hv0(vv, DiscreteValues{{M(1), 0}}), hv1(vv, DiscreteValues{{M(1), 1}}); EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), @@ -530,6 +590,9 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + // Importance sampling run with 50k samples gives 0.49934/0.50066 + // approximateDiscreteMarginal(hbn, proposalNet, given); + // Since no measurement on x1, we a 50/50 probability auto p_m = bn->at(2)->asDiscrete(); EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()(DiscreteValues{{m1.first, 0}}), @@ -540,16 +603,18 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { { // Now we add a measurement z1 on x1 - hbn = CreateBayesNet(mu0, mu1, sigma0, sigma1, true); + HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); + + double z1 = 2.2; + given.insert(Z(1), Vector1(z1)); - given.insert(Z(1), Vector1(2.2)); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); { VectorValues vv{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}, - {Z(0), Vector1(0.5)}, - {Z(1), Vector1(2.2)}}; + {Z(0), Vector1(z0)}, + {Z(1), Vector1(z1)}}; HybridValues hv0(vv, DiscreteValues{{M(1), 0}}), hv1(vv, DiscreteValues{{M(1), 1}}); EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), @@ -558,8 +623,8 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { { VectorValues vv{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}, - {Z(0), Vector1(0.5)}, - {Z(1), Vector1(2.2)}}; + {Z(0), Vector1(z0)}, + {Z(1), Vector1(z1)}}; HybridValues hv0(vv, DiscreteValues{{M(1), 0}}), hv1(vv, DiscreteValues{{M(1), 1}}); EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), @@ -569,9 +634,10 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); // Since we have a measurement on z2, we get a definite result - DiscreteConditional expected(m1, "0.44744586/0.55255414"); - // regression - EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 1e-6)); + // Values taken from an importance sampling run with 50k samples: + // approximateDiscreteMarginal(hbn, proposalNet, given); + DiscreteConditional expected(m1, "0.446345/0.553655"); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.01)); } } From e59b3afc2931a6a2c6a6e9e2a277c870754f9c6a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 12 Sep 2024 18:32:27 -0700 Subject: [PATCH 3/8] Add second test --- .../tests/testGaussianMixtureFactor.cpp | 23 ++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 81a68826a0..049986fdbf 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -555,7 +555,7 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { using namespace test_two_state_estimation; double mu0 = 1.0, mu1 = 3.0; - double sigma0 = 6.0, sigma1 = 4.0; + double sigma0 = 0.5, sigma1 = 2.0; auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1); // Start with no measurement on x1, only on x0 @@ -605,7 +605,7 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { // Now we add a measurement z1 on x1 HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); - double z1 = 2.2; + double z1 = 4.0; // favors m==1 given.insert(Z(1), Vector1(z1)); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); @@ -636,7 +636,24 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { // Since we have a measurement on z2, we get a definite result // Values taken from an importance sampling run with 50k samples: // approximateDiscreteMarginal(hbn, proposalNet, given); - DiscreteConditional expected(m1, "0.446345/0.553655"); + DiscreteConditional expected(m1, "0.481793/0.518207"); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.01)); + } + + { + // Add a different measurement z1 on that favors m==0 + HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); + + double z1 = 1.1; + given.insert_or_assign(Z(1), Vector1(z1)); + + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + // Since we have a measurement on z2, we get a definite result + // Values taken from an importance sampling run with 50k samples: + // approximateDiscreteMarginal(hbn, proposalNet, given); + DiscreteConditional expected(m1, "0.554485/0.445515"); EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.01)); } } From 07b4c236eb49b53c65c3b709c0433f7d2be1b533 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 12 Sep 2024 23:20:05 -0700 Subject: [PATCH 4/8] Finishing touches --- .../tests/testGaussianMixtureFactor.cpp | 110 ++++++++---------- 1 file changed, 46 insertions(+), 64 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 049986fdbf..c0a012e79f 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include @@ -433,20 +434,20 @@ HybridBayesNet CreateBayesNet( return hbn; } -/// Create importance sampling network p(x1| x0, m1) p(x0) P(m1), -/// using Q(x0) = N(z0, sigma_Q) to sample from p(x0) +/// Create importance sampling network q(x0,x1,m) = p(x1|x0,m1) q(x0) P(m1), +/// using q(x0) = N(z0, sigma_Q) to sample x0. HybridBayesNet CreateProposalNet( - const GaussianMixture::shared_ptr& hybridMotionModel, double z0, + const GaussianMixture::shared_ptr& hybridMotionModel, const Vector1& z0, double sigma_Q) { HybridBayesNet hbn; // Add hybrid motion model hbn.push_back(hybridMotionModel); - // Add proposal Q(x0) for x0 + // Add proposal q(x0) for x0 auto measurement_model = noiseModel::Isotropic::Sigma(1, sigma_Q); hbn.emplace_shared( - GaussianConditional::FromMeanAndStddev(X(0), Vector1(z0), sigma_Q)); + GaussianConditional::FromMeanAndStddev(X(0), z0, sigma_Q)); // Discrete uniform prior. hbn.emplace_shared(m1, "0.5/0.5"); @@ -466,7 +467,7 @@ void approximateDiscreteMarginal(const HybridBayesNet& hbn, HybridValues sample = proposalNet.sample(&rng); sample.insert(given); double weight = hbn.evaluate(sample) / proposalNet.evaluate(sample); - (sample.atDiscrete(m1.first) == 0) ? w0 += weight : w1 += weight; + (sample.atDiscrete(M(1)) == 0) ? w0 += weight : w1 += weight; } double sumWeights = w0 + w1; double pm1 = w1 / sumWeights; @@ -478,15 +479,14 @@ void approximateDiscreteMarginal(const HybridBayesNet& hbn, /* ************************************************************************* */ /** - * Test a model P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1). + * Test a model p(z0|x0)p(z1|x1)p(x1|x0,m1)P(m1). * - * P(f01|x1,x0,m1) has different means and same covariance. + * p(x1|x0,m1) has mode-dependent mean but same covariance. * - * Converting to a factor graph gives us - * ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1) + * Converting to a factor graph gives us ϕ(x0;z0)ϕ(x1;z1)ϕ(x1,x0,m1)P(m1) * - * If we only have a measurement on z0, then - * the probability of m1 should be 0.5/0.5. + * If we only have a measurement on x0, then + * the posterior probability of m1 should be 0.5/0.5. * Getting a measurement on z1 gives use more information. */ TEST(GaussianMixtureFactor, TwoStateModel) { @@ -497,10 +497,10 @@ TEST(GaussianMixtureFactor, TwoStateModel) { auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma, sigma); // Start with no measurement on x1, only on x0 - double z0 = 0.5; + const Vector1 z0(0.5); VectorValues given; - given.insert(Z(0), Vector1(z0)); + given.insert(Z(0), z0); // Create proposal network for importance sampling auto proposalNet = CreateProposalNet(hybridMotionModel, z0, 3.0); @@ -522,10 +522,10 @@ TEST(GaussianMixtureFactor, TwoStateModel) { // Now we add a measurement z1 on x1 HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); - // If we see z1=4.5 (>> 2.5 which is the halfway point), - // discrete mode should say m1=1 - const double z1 = 4.5; - given.insert(Z(1), Vector1(z1)); + // If we set z1=4.5 (>> 2.5 which is the halfway point), + // probability of discrete mode should be leaning to m1==1. + const Vector1 z1(4.5); + given.insert(Z(1), z1); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); @@ -534,7 +534,7 @@ TEST(GaussianMixtureFactor, TwoStateModel) { // Values taken from an importance sampling run with 50k samples: // approximateDiscreteMarginal(hbn, proposalNet, given); DiscreteConditional expected(m1, "0.446629/0.553371"); - EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.01)); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); } } @@ -559,9 +559,9 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1); // Start with no measurement on x1, only on x0 - double z0 = 0.5; + const Vector1 z0(0.5); VectorValues given; - given.insert(Z(0), Vector1(z0)); + given.insert(Z(0), z0); // Create proposal network for importance sampling // uncomment this and the approximateDiscreteMarginal calls to run @@ -571,19 +571,13 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { HybridBayesNet hbn = CreateBayesNet(hybridMotionModel); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - { - VectorValues vv{ - {X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}, {Z(0), Vector1(z0)}}; - HybridValues hv0(vv, DiscreteValues{{M(1), 0}}), - hv1(vv, DiscreteValues{{M(1), 1}}); - EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), - gfg.error(hv1) / hbn.error(hv1), 1e-9); - } - { - VectorValues vv{ - {X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}, {Z(0), Vector1(z0)}}; - HybridValues hv0(vv, DiscreteValues{{M(1), 0}}), - hv1(vv, DiscreteValues{{M(1), 1}}); + // Check that ratio of Bayes net and factor graph for different modes is + // equal for several values of {x0,x1}. + for (VectorValues vv : + {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, + VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { + vv.insert(given); // add measurements for HBN + HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}}); EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), gfg.error(hv1) / hbn.error(hv1), 1e-9); } @@ -595,66 +589,54 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { // Since no measurement on x1, we a 50/50 probability auto p_m = bn->at(2)->asDiscrete(); - EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()(DiscreteValues{{m1.first, 0}}), - 1e-9); - EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()(DiscreteValues{{m1.first, 1}}), - 1e-9); + EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()(DiscreteValues{{M(1), 0}}), 1e-9); + EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()(DiscreteValues{{M(1), 1}}), 1e-9); } { // Now we add a measurement z1 on x1 HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); - double z1 = 4.0; // favors m==1 - given.insert(Z(1), Vector1(z1)); + const Vector1 z1(4.0); // favors m==1 + given.insert(Z(1), z1); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - { - VectorValues vv{{X(0), Vector1(0.0)}, - {X(1), Vector1(1.0)}, - {Z(0), Vector1(z0)}, - {Z(1), Vector1(z1)}}; - HybridValues hv0(vv, DiscreteValues{{M(1), 0}}), - hv1(vv, DiscreteValues{{M(1), 1}}); - EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), - gfg.error(hv1) / hbn.error(hv1), 1e-9); - } - { - VectorValues vv{{X(0), Vector1(0.5)}, - {X(1), Vector1(3.0)}, - {Z(0), Vector1(z0)}, - {Z(1), Vector1(z1)}}; - HybridValues hv0(vv, DiscreteValues{{M(1), 0}}), - hv1(vv, DiscreteValues{{M(1), 1}}); + // Check that ratio of Bayes net and factor graph for different modes is + // equal for several values of {x0,x1}. + for (VectorValues vv : + {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, + VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { + vv.insert(given); // add measurements for HBN + HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}}); EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), gfg.error(hv1) / hbn.error(hv1), 1e-9); } HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - // Since we have a measurement on z2, we get a definite result + // Since we have a measurement z1 on x1, we get a definite result // Values taken from an importance sampling run with 50k samples: // approximateDiscreteMarginal(hbn, proposalNet, given); DiscreteConditional expected(m1, "0.481793/0.518207"); - EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.01)); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.001)); } { - // Add a different measurement z1 on that favors m==0 + // Add a different measurement z1 on x1 that favors m==0 HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); - double z1 = 1.1; - given.insert_or_assign(Z(1), Vector1(z1)); + const Vector1 z1(1.1); + given.insert_or_assign(Z(1), z1); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - // Since we have a measurement on z2, we get a definite result + // Since we have a measurement z1 on x1, we get a definite result // Values taken from an importance sampling run with 50k samples: // approximateDiscreteMarginal(hbn, proposalNet, given); DiscreteConditional expected(m1, "0.554485/0.445515"); - EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.01)); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.001)); } } From 70651e2cc501d75b764e038c8623b492535bcfda Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 12 Sep 2024 23:45:32 -0700 Subject: [PATCH 5/8] Inline q, use 100k --- .../tests/testGaussianMixtureFactor.cpp | 113 +++++++----------- 1 file changed, 43 insertions(+), 70 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index c0a012e79f..ab8453fba5 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -235,7 +235,7 @@ static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, hbn.emplace_shared(KeyVector{z}, KeyVector{}, DiscreteKeys{m}, std::vector{c0, c1}); - auto mixing = make_shared(m, "0.5/0.5"); + auto mixing = make_shared(m, "50/50"); hbn.push_back(mixing); return hbn; @@ -281,7 +281,7 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel) { // At the halfway point between the means, we should get P(m|z)=0.5 HybridBayesNet expected; - expected.emplace_shared(m, "0.5/0.5"); + expected.emplace_shared(m, "50/50"); EXPECT(assert_equal(expected, *bn)); } @@ -429,50 +429,37 @@ HybridBayesNet CreateBayesNet( hbn.push_back(hybridMotionModel); // Discrete uniform prior. - hbn.emplace_shared(m1, "0.5/0.5"); - - return hbn; -} - -/// Create importance sampling network q(x0,x1,m) = p(x1|x0,m1) q(x0) P(m1), -/// using q(x0) = N(z0, sigma_Q) to sample x0. -HybridBayesNet CreateProposalNet( - const GaussianMixture::shared_ptr& hybridMotionModel, const Vector1& z0, - double sigma_Q) { - HybridBayesNet hbn; - - // Add hybrid motion model - hbn.push_back(hybridMotionModel); - - // Add proposal q(x0) for x0 - auto measurement_model = noiseModel::Isotropic::Sigma(1, sigma_Q); - hbn.emplace_shared( - GaussianConditional::FromMeanAndStddev(X(0), z0, sigma_Q)); - - // Discrete uniform prior. - hbn.emplace_shared(m1, "0.5/0.5"); + hbn.emplace_shared(m1, "50/50"); return hbn; } /// Approximate the discrete marginal P(m1) using importance sampling /// Not typically called as expensive, but values are used in the tests. -void approximateDiscreteMarginal(const HybridBayesNet& hbn, - const HybridBayesNet& proposalNet, - const VectorValues& given) { +void approximateDiscreteMarginal( + const HybridBayesNet& hbn, + const GaussianMixture::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, sigma_Q) to sample x0. + HybridBayesNet q; + q.push_back(hybridMotionModel); // Add hybrid motion model + q.emplace_shared(GaussianConditional::FromMeanAndStddev( + X(0), given.at(Z(0)), /* sigma_Q = */ 3.0)); // Add proposal q(x0) for x0 + q.emplace_shared(m1, "50/50"); // Discrete prior. + // Do importance sampling double w0 = 0.0, w1 = 0.0; - std::mt19937_64 rng(44); - for (int i = 0; i < 50000; i++) { - HybridValues sample = proposalNet.sample(&rng); + std::mt19937_64 rng(42); + for (int i = 0; i < N; i++) { + HybridValues sample = q.sample(&rng); sample.insert(given); - double weight = hbn.evaluate(sample) / proposalNet.evaluate(sample); + double weight = hbn.evaluate(sample) / q.evaluate(sample); (sample.atDiscrete(M(1)) == 0) ? w0 += weight : w1 += weight; } - double sumWeights = w0 + w1; - double pm1 = w1 / sumWeights; - std::cout << "p(m0) ~ " << 1.0 - pm1 << std::endl; - std::cout << "p(m1) ~ " << pm1 << std::endl; + double pm1 = w1 / (w0 + w1); + std::cout << "p(m0) = " << 100 * (1.0 - pm1) << std::endl; + std::cout << "p(m1) = " << 100 * pm1 << std::endl; } } // namespace test_two_state_estimation @@ -502,38 +489,32 @@ TEST(GaussianMixtureFactor, TwoStateModel) { VectorValues given; given.insert(Z(0), z0); - // Create proposal network for importance sampling - auto proposalNet = CreateProposalNet(hybridMotionModel, z0, 3.0); - EXPECT_LONGS_EQUAL(3, proposalNet.size()); - { HybridBayesNet hbn = CreateBayesNet(hybridMotionModel); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); // Since no measurement on x1, we hedge our bets - // Importance sampling run with 50k samples gives 0.49934/0.50066 - // approximateDiscreteMarginal(hbn, proposalNet, given); - DiscreteConditional expected(m1, "0.5/0.5"); + // Importance sampling run with 100k samples gives 50.051/49.949 + // approximateDiscreteMarginal(hbn, hybridMotionModel, given); + DiscreteConditional expected(m1, "50/50"); EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()))); } { - // Now we add a measurement z1 on x1 - HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); - // If we set z1=4.5 (>> 2.5 which is the halfway point), // probability of discrete mode should be leaning to m1==1. const Vector1 z1(4.5); given.insert(Z(1), z1); + HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); // Since we have a measurement on x1, we get a definite result - // Values taken from an importance sampling run with 50k samples: - // approximateDiscreteMarginal(hbn, proposalNet, given); - DiscreteConditional expected(m1, "0.446629/0.553371"); + // Values taken from an importance sampling run with 100k samples: + // approximateDiscreteMarginal(hbn, hybridMotionModel, given); + DiscreteConditional expected(m1, "44.3854/55.6146"); EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); } } @@ -563,10 +544,6 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { VectorValues given; given.insert(Z(0), z0); - // Create proposal network for importance sampling - // uncomment this and the approximateDiscreteMarginal calls to run - // auto proposalNet = CreateProposalNet(hybridMotionModel, z0, 3.0); - { HybridBayesNet hbn = CreateBayesNet(hybridMotionModel); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); @@ -584,22 +561,21 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - // Importance sampling run with 50k samples gives 0.49934/0.50066 - // approximateDiscreteMarginal(hbn, proposalNet, given); + // Importance sampling run with 100k samples gives 50.095/49.905 + // approximateDiscreteMarginal(hbn, hybridMotionModel, given); // Since no measurement on x1, we a 50/50 probability auto p_m = bn->at(2)->asDiscrete(); - EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()(DiscreteValues{{M(1), 0}}), 1e-9); - EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()(DiscreteValues{{M(1), 1}}), 1e-9); + EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 0}}), 1e-9); + EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 1}}), 1e-9); } { // Now we add a measurement z1 on x1 - HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); - const Vector1 z1(4.0); // favors m==1 given.insert(Z(1), z1); + HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); // Check that ratio of Bayes net and factor graph for different modes is @@ -615,28 +591,25 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - // Since we have a measurement z1 on x1, we get a definite result - // Values taken from an importance sampling run with 50k samples: - // approximateDiscreteMarginal(hbn, proposalNet, given); - DiscreteConditional expected(m1, "0.481793/0.518207"); - EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.001)); + // Values taken from an importance sampling run with 100k samples: + // approximateDiscreteMarginal(hbn, hybridMotionModel, given); + DiscreteConditional expected(m1, "48.3158/51.6842"); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); } { // Add a different measurement z1 on x1 that favors m==0 - HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); - const Vector1 z1(1.1); given.insert_or_assign(Z(1), z1); + HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - // Since we have a measurement z1 on x1, we get a definite result - // Values taken from an importance sampling run with 50k samples: - // approximateDiscreteMarginal(hbn, proposalNet, given); - DiscreteConditional expected(m1, "0.554485/0.445515"); - EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.001)); + // Values taken from an importance sampling run with 100k samples: + // approximateDiscreteMarginal(hbn, hybridMotionModel, given); + DiscreteConditional expected(m1, "55.396/44.604"); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); } } From 8fbabf5c2417738a561c1e6193dac56e2cdd8fb9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 13 Sep 2024 01:08:33 -0700 Subject: [PATCH 6/8] Extreme example --- .../tests/testGaussianMixtureFactor.cpp | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index ab8453fba5..7947779c89 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -613,6 +613,33 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { } } +/* ************************************************************************* */ +/** + * Same model, P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1), but now with very informative + * measurements and vastly different motion model: either stand still or move + * far. This yields a very informative posterior. + */ +TEST(GaussianMixtureFactor, TwoStateModel3) { + using namespace test_two_state_estimation; + + double mu0 = 0.0, mu1 = 10.0; + double sigma0 = 0.2, sigma1 = 5.0; + auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1); + + // We only check the 2-measurement case + const Vector1 z0(0.0), z1(10.0); + VectorValues given{{Z(0), z0}, {Z(1), z1}}; + + HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + // Values taken from an importance sampling run with 100k samples: + // approximateDiscreteMarginal(hbn, hybridMotionModel, given); + DiscreteConditional expected(m1, "8.91527/91.0847"); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); +} + /* ************************************************************************* */ int main() { TestResult tr; From bf00ca891d103b183aa44d624e1181b3a6b788ea Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 13 Sep 2024 01:18:05 -0700 Subject: [PATCH 7/8] Small improvements --- .../tests/testGaussianMixtureFactor.cpp | 32 +++++++++++++------ 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 7947779c89..0ccb6d7bce 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -392,6 +392,12 @@ namespace test_two_state_estimation { DiscreteKey m1(M(1), 2); +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); +} + /// Create hybrid motion model p(x1 | x0, m1) static GaussianMixture::shared_ptr CreateHybridMotionModel(double mu0, double mu1, @@ -414,15 +420,11 @@ HybridBayesNet CreateBayesNet( HybridBayesNet hbn; // Add measurement model p(z0 | x0) - const double measurement_sigma = 3.0; - auto measurement_model = noiseModel::Isotropic::Sigma(1, measurement_sigma); - hbn.emplace_shared(Z(0), Vector1(0.0), I_1x1, X(0), - -I_1x1, measurement_model); + addMeasurement(hbn, Z(0), X(0), 3.0); // Optionally add second measurement model p(z1 | x1) if (add_second_measurement) { - hbn.emplace_shared(Z(1), Vector1(0.0), I_1x1, X(1), - -I_1x1, measurement_model); + addMeasurement(hbn, Z(1), X(1), 3.0); } // Add hybrid motion model @@ -434,18 +436,27 @@ HybridBayesNet CreateBayesNet( return hbn; } +/** + * @brief Approximates the discrete marginal P(m1) using importance sampling. + * @note Not typically called as expensive, but values are used in the tests. + * + * @param hbn The hybrid Bayesian network. + * @param hybridMotionModel The hybrid motion model. + * @param given Observed values for variables. + * @param N Number of samples for importance sampling. + * @return std::pair Probabilities for m1 = 0 and m1 = 1. + */ /// Approximate the discrete marginal P(m1) using importance sampling -/// Not typically called as expensive, but values are used in the tests. -void approximateDiscreteMarginal( +std::pair approximateDiscreteMarginal( const HybridBayesNet& hbn, const GaussianMixture::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, sigma_Q) to sample x0. + /// using q(x0) = N(z0, sigmaQ) to sample x0. HybridBayesNet q; q.push_back(hybridMotionModel); // Add hybrid motion model q.emplace_shared(GaussianConditional::FromMeanAndStddev( - X(0), given.at(Z(0)), /* sigma_Q = */ 3.0)); // Add proposal q(x0) for x0 + X(0), given.at(Z(0)), /* sigmaQ = */ 3.0)); // Add proposal q(x0) for x0 q.emplace_shared(m1, "50/50"); // Discrete prior. // Do importance sampling @@ -460,6 +471,7 @@ void approximateDiscreteMarginal( double pm1 = w1 / (w0 + w1); std::cout << "p(m0) = " << 100 * (1.0 - pm1) << std::endl; std::cout << "p(m1) = " << 100 * pm1 << std::endl; + return {1.0 - pm1, pm1}; } } // namespace test_two_state_estimation From fcc26e553d394782380955e2075e616bb3e550b6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 13 Sep 2024 01:21:39 -0700 Subject: [PATCH 8/8] Remove redundant doc --- gtsam/hybrid/tests/testGaussianMixtureFactor.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 0ccb6d7bce..dab11039f1 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -436,16 +436,6 @@ HybridBayesNet CreateBayesNet( return hbn; } -/** - * @brief Approximates the discrete marginal P(m1) using importance sampling. - * @note Not typically called as expensive, but values are used in the tests. - * - * @param hbn The hybrid Bayesian network. - * @param hybridMotionModel The hybrid motion model. - * @param given Observed values for variables. - * @param N Number of samples for importance sampling. - * @return std::pair Probabilities for m1 = 0 and m1 = 1. - */ /// Approximate the discrete marginal P(m1) using importance sampling std::pair approximateDiscreteMarginal( const HybridBayesNet& hbn,