testHybridMotionModel.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
21 #include <gtsam/base/Testable.h>
22 #include <gtsam/base/TestableAssertions.h>
23 #include <gtsam/discrete/DiscreteConditional.h>
24 #include <gtsam/discrete/DiscreteValues.h>
25 #include <gtsam/hybrid/HybridBayesNet.h>
26 #include <gtsam/hybrid/HybridGaussianConditional.h>
27 #include <gtsam/hybrid/HybridGaussianFactor.h>
28 #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
29 #include <gtsam/hybrid/HybridValues.h>
30 #include <gtsam/inference/Symbol.h>
31 #include <gtsam/linear/GaussianFactorGraph.h>
32 #include <gtsam/linear/VectorValues.h>
33 #include <gtsam/nonlinear/PriorFactor.h>
34 #include <gtsam/slam/BetweenFactor.h>
35 
36 // Include for test suite
37 #include <CppUnitLite/TestHarness.h>
38 
39 #include <memory>
40 
41 using namespace std;
42 using namespace gtsam;
43 using symbol_shorthand::M;
44 using symbol_shorthand::X;
45 using symbol_shorthand::Z;
46 
47 DiscreteKey m1(M(1), 2);
48 
49 void addMeasurement(HybridBayesNet &hbn, Key z_key, Key x_key, double sigma) {
50  auto measurement_model = noiseModel::Isotropic::Sigma(1, sigma);
51  hbn.emplace_shared<GaussianConditional>(z_key, Vector1(0.0), I_1x1, x_key,
52  -I_1x1, measurement_model);
53 }
54 
56 static HybridGaussianConditional::shared_ptr CreateHybridMotionModel(
57  double mu0, double mu1, double sigma0, double sigma1) {
58  std::vector<std::pair<Vector, double>> motionModels{{Vector1(mu0), sigma0},
59  {Vector1(mu1), sigma1}};
60  return std::make_shared<HybridGaussianConditional>(m1, X(1), I_1x1, X(0),
61  motionModels);
62 }
63 
65 HybridBayesNet CreateBayesNet(
66  const HybridGaussianConditional::shared_ptr &hybridMotionModel,
67  bool add_second_measurement = false) {
68  HybridBayesNet hbn;
69 
70  // Add measurement model p(z0 | x0)
71  addMeasurement(hbn, Z(0), X(0), 3.0);
72 
73  // Optionally add second measurement model p(z1 | x1)
74  if (add_second_measurement) {
75  addMeasurement(hbn, Z(1), X(1), 3.0);
76  }
77 
78  // Add hybrid motion model
79  hbn.push_back(hybridMotionModel);
80 
81  // Discrete uniform prior.
82  hbn.emplace_shared<DiscreteConditional>(m1, "50/50");
83 
84  return hbn;
85 }
86 
88 std::pair<double, double> approximateDiscreteMarginal(
89  const HybridBayesNet &hbn,
90  const HybridGaussianConditional::shared_ptr &hybridMotionModel,
91  const VectorValues &given, size_t N = 100000) {
94  HybridBayesNet q;
95  q.push_back(hybridMotionModel); // Add hybrid motion model
96  q.emplace_shared<GaussianConditional>(GaussianConditional::FromMeanAndStddev(
97  X(0), given.at(Z(0)), /* sigmaQ = */ 3.0)); // Add proposal q(x0) for x0
98  q.emplace_shared<DiscreteConditional>(m1, "50/50"); // Discrete prior.
99 
100  // Do importance sampling
101  double w0 = 0.0, w1 = 0.0;
102  std::mt19937_64 rng(42);
103  for (int i = 0; i < N; i++) {
104  HybridValues sample = q.sample(&rng);
105  sample.insert(given);
106  double weight = hbn.evaluate(sample) / q.evaluate(sample);
107  (sample.atDiscrete(M(1)) == 0) ? w0 += weight : w1 += weight;
108  }
109  double pm1 = w1 / (w0 + w1);
110  std::cout << "p(m0) = " << 100 * (1.0 - pm1) << std::endl;
111  std::cout << "p(m1) = " << 100 * pm1 << std::endl;
112  return {1.0 - pm1, pm1};
113 }
114 
115 /* ************************************************************************* */
127 TEST(HybridGaussianFactor, TwoStateModel) {
128  double mu0 = 1.0, mu1 = 3.0;
129  double sigma = 0.5;
130  auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma, sigma);
131 
132  // Start with no measurement on x1, only on x0
133  const Vector1 z0(0.5);
134 
135  VectorValues given;
136  given.insert(Z(0), z0);
137 
138  {
139  HybridBayesNet hbn = CreateBayesNet(hybridMotionModel);
140  HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
141  HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
142 
143  // Since no measurement on x1, we hedge our bets
144  // Importance sampling run with 100k samples gives 50.051/49.949
145  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
146  DiscreteConditional expected(m1, "50/50");
147  EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete())));
148  }
149 
150  {
151  // If we set z1=4.5 (>> 2.5 which is the halfway point),
152  // probability of discrete mode should be leaning to m1==1.
153  const Vector1 z1(4.5);
154  given.insert(Z(1), z1);
155 
156  HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
157  HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
158  HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
159 
160  // Since we have a measurement on x1, we get a definite result
161  // Values taken from an importance sampling run with 100k samples:
162  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
163  DiscreteConditional expected(m1, "44.3854/55.6146");
164  EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
165  }
166 }
167 
168 /* ************************************************************************* */
181 TEST(HybridGaussianFactor, TwoStateModel2) {
182  double mu0 = 1.0, mu1 = 3.0;
183  double sigma0 = 0.5, sigma1 = 2.0;
184  auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1);
185 
186  // Start with no measurement on x1, only on x0
187  const Vector1 z0(0.5);
188  VectorValues given;
189  given.insert(Z(0), z0);
190 
191  {
192  HybridBayesNet hbn = CreateBayesNet(hybridMotionModel);
193  HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
194 
195  HybridBayesNet::shared_ptr eliminated = gfg.eliminateSequential();
196 
197  for (VectorValues vv :
198  {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
199  VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
200  vv.insert(given); // add measurements for HBN
201  const auto& expectedDiscretePosterior = hbn.discretePosterior(vv);
202 
203  // Equality of posteriors asserts that the factor graph is correct (same
204  // ratios for all modes)
205  EXPECT(
206  assert_equal(expectedDiscretePosterior, gfg.discretePosterior(vv)));
207 
208  // This one asserts that HBN resulting from elimination is correct.
209  EXPECT(assert_equal(expectedDiscretePosterior,
210  eliminated->discretePosterior(vv)));
211  }
212 
213  // Importance sampling run with 100k samples gives 50.095/49.905
214  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
215 
216  // Since no measurement on x1, we a 50/50 probability
217  auto p_m = eliminated->at(2)->asDiscrete();
218  EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 0}}), 1e-9);
219  EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 1}}), 1e-9);
220  }
221 
222  {
223  // Now we add a measurement z1 on x1
224  const Vector1 z1(4.0); // favors m==1
225  given.insert(Z(1), z1);
226 
227  HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
228  HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
229  HybridBayesNet::shared_ptr eliminated = gfg.eliminateSequential();
230 
231  // Check that ratio of Bayes net and factor graph for different modes is
232  // equal for several values of {x0,x1}.
233  for (VectorValues vv :
234  {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
235  VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
236  vv.insert(given); // add measurements for HBN
237  const auto& expectedDiscretePosterior = hbn.discretePosterior(vv);
238 
239  // Equality of posteriors asserts that the factor graph is correct (same
240  // ratios for all modes)
241  EXPECT(
242  assert_equal(expectedDiscretePosterior, gfg.discretePosterior(vv)));
243 
244  // This one asserts that HBN resulting from elimination is correct.
245  EXPECT(assert_equal(expectedDiscretePosterior,
246  eliminated->discretePosterior(vv)));
247  }
248 
249  // Values taken from an importance sampling run with 100k samples:
250  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
251  DiscreteConditional expected(m1, "48.3158/51.6842");
252  EXPECT(assert_equal(expected, *(eliminated->at(2)->asDiscrete()), 0.002));
253  }
254 
255  {
256  // Add a different measurement z1 on x1 that favors m==0
257  const Vector1 z1(1.1);
258  given.insert_or_assign(Z(1), z1);
259 
260  HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
261  HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
262  HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
263 
264  // Values taken from an importance sampling run with 100k samples:
265  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
266  DiscreteConditional expected(m1, "55.396/44.604");
267  EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
268  }
269 }
270 
271 /* ************************************************************************* */
284 TEST(HybridGaussianFactor, TwoStateModel3) {
285  double mu = 1.0;
286  double sigma0 = 0.5, sigma1 = 2.0;
287  auto hybridMotionModel = CreateHybridMotionModel(mu, mu, sigma0, sigma1);
288 
289  // Start with no measurement on x1, only on x0
290  const Vector1 z0(0.5);
291  VectorValues given;
292  given.insert(Z(0), z0);
293 
294  {
295  HybridBayesNet hbn = CreateBayesNet(hybridMotionModel);
296  HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
297 
298  // Check that ratio of Bayes net and factor graph for different modes is
299  // equal for several values of {x0,x1}.
300  for (VectorValues vv :
301  {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
302  VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
303  vv.insert(given); // add measurements for HBN
304  HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
305  EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
306  gfg.error(hv1) / hbn.error(hv1), 1e-9);
307  }
308 
309  HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
310 
311  // Importance sampling run with 100k samples gives 50.095/49.905
312  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
313 
314  // Since no measurement on x1, we a 50/50 probability
315  auto p_m = bn->at(2)->asDiscrete();
316  EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 0}}), 1e-9);
317  EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 1}}), 1e-9);
318  }
319 
320  {
321  // Now we add a measurement z1 on x1
322  const Vector1 z1(4.0); // favors m==1
323  given.insert(Z(1), z1);
324 
325  HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
326  HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
327 
328  // Check that ratio of Bayes net and factor graph for different modes is
329  // equal for several values of {x0,x1}.
330  for (VectorValues vv :
331  {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
332  VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
333  vv.insert(given); // add measurements for HBN
334  HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
335  EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
336  gfg.error(hv1) / hbn.error(hv1), 1e-9);
337  }
338 
339  HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
340 
341  // Values taken from an importance sampling run with 100k samples:
342  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
343  DiscreteConditional expected(m1, "51.7762/48.2238");
344  EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
345  }
346 
347  {
348  // Add a different measurement z1 on x1 that favors m==1
349  const Vector1 z1(7.0);
350  given.insert_or_assign(Z(1), z1);
351 
352  HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
353  HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
354  HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
355 
356  // Values taken from an importance sampling run with 100k samples:
357  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
358  DiscreteConditional expected(m1, "49.0762/50.9238");
359  EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.005));
360  }
361 }
362 
363 /* ************************************************************************* */
369 TEST(HybridGaussianFactor, TwoStateModel4) {
370  double mu0 = 0.0, mu1 = 10.0;
371  double sigma0 = 0.2, sigma1 = 5.0;
372  auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1);
373 
374  // We only check the 2-measurement case
375  const Vector1 z0(0.0), z1(10.0);
376  VectorValues given{{Z(0), z0}, {Z(1), z1}};
377 
378  HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
379  HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
380  HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
381 
382  // Values taken from an importance sampling run with 100k samples:
383  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
384  DiscreteConditional expected(m1, "8.91527/91.0847");
385  EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
386 }
387 
388 /* ************************************************************************* */
389 int main() {
390  TestResult tr;
391  return TestRegistry::runAllTests(tr);
392 }
393 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::EliminateableFactorGraph::eliminateSequential
std::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:29
simple_graph::sigma1
double sigma1
Definition: testJacobianFactor.cpp:193
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::HybridValues
Definition: HybridValues.h:37
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
gtsam::HybridGaussianConditional::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: HybridGaussianConditional.h:59
TEST
TEST(HybridGaussianFactor, TwoStateModel)
Definition: testHybridMotionModel.cpp:127
HybridGaussianConditional.h
A hybrid conditional in the Conditional Linear Gaussian scheme.
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::FactorGraph::error
double error(const HybridValues &values) const
Definition: FactorGraph-inst.h:66
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
gtsam::HybridBayesNet
Definition: HybridBayesNet.h:37
mu
double mu
Definition: testBoundingConstraint.cpp:37
gtsam::HybridBayesNet::evaluate
double evaluate(const HybridValues &values) const
Evaluate hybrid probability density for given HybridValues.
Definition: HybridBayesNet.cpp:210
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:88
z1
Point2 z1
Definition: testTriangulationFactor.cpp:45
gtsam::VectorValues::at
Vector & at(Key j)
Definition: VectorValues.h:142
m1
DiscreteKey m1(M(1), 2)
CreateHybridMotionModel
static HybridGaussianConditional::shared_ptr CreateHybridMotionModel(double mu0, double mu1, double sigma0, double sigma1)
Create hybrid motion model p(x1 | x0, m1)
Definition: testHybridMotionModel.cpp:56
HybridBayesNet.h
A Bayes net of Gaussian Conditionals indexed by discrete keys.
X
#define X
Definition: icosphere.cpp:20
vv
static const VectorValues vv
Definition: testHybridGaussianConditional.cpp:44
DiscreteConditional.h
HybridGaussianFactor.h
A set of GaussianFactors, indexed by a set of discrete keys.
approximateDiscreteMarginal
std::pair< double, double > approximateDiscreteMarginal(const HybridBayesNet &hbn, const HybridGaussianConditional::shared_ptr &hybridMotionModel, const VectorValues &given, size_t N=100000)
Approximate the discrete marginal P(m1) using importance sampling.
Definition: testHybridMotionModel.cpp:88
TestableAssertions.h
Provides additional testing facilities for common data structures.
sampling::sigma
static const double sigma
Definition: testGaussianBayesNet.cpp:170
addMeasurement
void addMeasurement(HybridBayesNet &hbn, Key z_key, Key x_key, double sigma)
Definition: testHybridMotionModel.cpp:49
Vector1
Eigen::Matrix< double, 1, 1 > Vector1
Definition: testEvent.cpp:33
PriorFactor.h
gtsam::VectorValues
Definition: VectorValues.h:74
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
BetweenFactor.h
main
int main()
Definition: testHybridMotionModel.cpp:389
gtsam::GaussianConditional
Definition: GaussianConditional.h:40
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::HybridGaussianFactorGraph
Definition: HybridGaussianFactorGraph.h:105
Symbol.h
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
VectorValues.h
Factor Graph Values.
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
TestResult
Definition: TestResult.h:26
gtsam::HybridBayesNet::discretePosterior
AlgebraicDecisionTree< Key > discretePosterior(const VectorValues &continuousValues) const
Compute normalized posterior P(M|X=x) and return as a tree.
Definition: HybridBayesNet.cpp:201
gtsam::HybridBayesNet::push_back
void push_back(std::shared_ptr< HybridConditional > conditional)
Add a hybrid conditional using a shared_ptr.
Definition: HybridBayesNet.h:76
gtsam::DiscreteConditional
Definition: DiscreteConditional.h:37
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
gtsam
traits
Definition: SFMdata.h:40
gtsam::HybridValues::atDiscrete
size_t & atDiscrete(Key j)
Definition: HybridValues.cpp:132
DiscreteValues.h
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Definition: DiscreteKey.h:38
std
Definition: BFloat16.h:88
gtsam::HybridGaussianFactor
Implementation of a discrete-conditioned hybrid factor. Implements a joint discrete-continuous factor...
Definition: HybridGaussianFactor.h:60
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
hv0
static const HybridValues hv0
Definition: testHybridGaussianConditional.cpp:46
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
N
#define N
Definition: igam.h:9
gtsam::HybridBayesNet::shared_ptr
std::shared_ptr< HybridBayesNet > shared_ptr
Definition: HybridBayesNet.h:42
gtsam::HybridGaussianFactorGraph::discretePosterior
AlgebraicDecisionTree< Key > discretePosterior(const VectorValues &continuousValues) const
Computer posterior P(M|X=x) when all continuous values X are given. This is efficient as this simply ...
Definition: HybridGaussianFactorGraph.cpp:548
gtsam::HybridBayesNet::toFactorGraph
HybridGaussianFactorGraph toFactorGraph(const VectorValues &measurements) const
Definition: HybridBayesNet.cpp:215
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
hv1
static const HybridValues hv1
Definition: testHybridGaussianConditional.cpp:47
Z
#define Z
Definition: icosphere.cpp:21
CreateBayesNet
HybridBayesNet CreateBayesNet(const HybridGaussianConditional::shared_ptr &hybridMotionModel, bool add_second_measurement=false)
Create two state Bayes network with 1 or two measurement models.
Definition: testHybridMotionModel.cpp:65
HybridValues.h
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::HybridBayesNet::emplace_shared
void emplace_shared(Args &&...args)
Definition: HybridBayesNet.h:116
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51

gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:40:14