From 8fbabf5c2417738a561c1e6193dac56e2cdd8fb9 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 13 Sep 2024 01:08:33 -0700
Subject: [PATCH] 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 ab8453fba..7947779c8 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;