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  // Check that ratio of Bayes net and factor graph for different modes is
196  // equal for several values of {x0,x1}.
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  HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
202  EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
203  gfg.error(hv1) / hbn.error(hv1), 1e-9);
204  }
205 
206  HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
207 
208  // Importance sampling run with 100k samples gives 50.095/49.905
209  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
210 
211  // Since no measurement on x1, we a 50/50 probability
212  auto p_m = bn->at(2)->asDiscrete();
213  EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 0}}), 1e-9);
214  EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 1}}), 1e-9);
215  }
216 
217  {
218  // Now we add a measurement z1 on x1
219  const Vector1 z1(4.0); // favors m==1
220  given.insert(Z(1), z1);
221 
222  HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
223  HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
224 
225  // Check that ratio of Bayes net and factor graph for different modes is
226  // equal for several values of {x0,x1}.
227  for (VectorValues vv :
228  {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
229  VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
230  vv.insert(given); // add measurements for HBN
231  HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
232  EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
233  gfg.error(hv1) / hbn.error(hv1), 1e-9);
234  }
235 
236  HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
237 
238  // Values taken from an importance sampling run with 100k samples:
239  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
240  DiscreteConditional expected(m1, "48.3158/51.6842");
241  EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
242  }
243 
244  {
245  // Add a different measurement z1 on x1 that favors m==0
246  const Vector1 z1(1.1);
247  given.insert_or_assign(Z(1), z1);
248 
249  HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
250  HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
251  HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
252 
253  // Values taken from an importance sampling run with 100k samples:
254  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
255  DiscreteConditional expected(m1, "55.396/44.604");
256  EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
257  }
258 }
259 
260 /* ************************************************************************* */
273 TEST(HybridGaussianFactor, TwoStateModel3) {
274  double mu = 1.0;
275  double sigma0 = 0.5, sigma1 = 2.0;
276  auto hybridMotionModel = CreateHybridMotionModel(mu, mu, sigma0, sigma1);
277 
278  // Start with no measurement on x1, only on x0
279  const Vector1 z0(0.5);
280  VectorValues given;
281  given.insert(Z(0), z0);
282 
283  {
284  HybridBayesNet hbn = CreateBayesNet(hybridMotionModel);
285  HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
286 
287  // Check that ratio of Bayes net and factor graph for different modes is
288  // equal for several values of {x0,x1}.
289  for (VectorValues vv :
290  {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
291  VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
292  vv.insert(given); // add measurements for HBN
293  HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
294  EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
295  gfg.error(hv1) / hbn.error(hv1), 1e-9);
296  }
297 
298  HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
299 
300  // Importance sampling run with 100k samples gives 50.095/49.905
301  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
302 
303  // Since no measurement on x1, we a 50/50 probability
304  auto p_m = bn->at(2)->asDiscrete();
305  EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 0}}), 1e-9);
306  EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 1}}), 1e-9);
307  }
308 
309  {
310  // Now we add a measurement z1 on x1
311  const Vector1 z1(4.0); // favors m==1
312  given.insert(Z(1), z1);
313 
314  HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
315  HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
316 
317  // Check that ratio of Bayes net and factor graph for different modes is
318  // equal for several values of {x0,x1}.
319  for (VectorValues vv :
320  {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
321  VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
322  vv.insert(given); // add measurements for HBN
323  HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
324  EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
325  gfg.error(hv1) / hbn.error(hv1), 1e-9);
326  }
327 
328  HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
329 
330  // Values taken from an importance sampling run with 100k samples:
331  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
332  DiscreteConditional expected(m1, "51.7762/48.2238");
333  EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
334  }
335 
336  {
337  // Add a different measurement z1 on x1 that favors m==1
338  const Vector1 z1(7.0);
339  given.insert_or_assign(Z(1), z1);
340 
341  HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
342  HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
343  HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
344 
345  // Values taken from an importance sampling run with 100k samples:
346  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
347  DiscreteConditional expected(m1, "49.0762/50.9238");
348  EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.005));
349  }
350 }
351 
352 /* ************************************************************************* */
358 TEST(HybridGaussianFactor, TwoStateModel4) {
359  double mu0 = 0.0, mu1 = 10.0;
360  double sigma0 = 0.2, sigma1 = 5.0;
361  auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1);
362 
363  // We only check the 2-measurement case
364  const Vector1 z0(0.0), z1(10.0);
365  VectorValues given{{Z(0), z0}, {Z(1), z1}};
366 
367  HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
368  HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
369  HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
370 
371  // Values taken from an importance sampling run with 100k samples:
372  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
373  DiscreteConditional expected(m1, "8.91527/91.0847");
374  EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
375 }
376 
377 /* ************************************************************************* */
378 int main() {
379  TestResult tr;
380  return TestRegistry::runAllTests(tr);
381 }
382 /* ************************************************************************* */
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:58
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:36
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:357
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:41
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:378
gtsam::GaussianConditional
Definition: GaussianConditional.h:40
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::HybridGaussianFactorGraph
Definition: HybridGaussianFactorGraph.h:104
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::push_back
void push_back(std::shared_ptr< HybridConditional > conditional)
Add a hybrid conditional using a shared_ptr.
Definition: HybridBayesNet.h:75
gtsam::DiscreteConditional
Definition: DiscreteConditional.h:37
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
gtsam
traits
Definition: chartTesting.h:28
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:59
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:43
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:41
gtsam::HybridBayesNet::toFactorGraph
HybridGaussianFactorGraph toFactorGraph(const VectorValues &measurements) const
Definition: HybridBayesNet.cpp:362
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
hv1
static const HybridValues hv1
Definition: testHybridGaussianConditional.cpp:44
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:120
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51

gtsam
Author(s):
autogenerated on Fri Oct 4 2024 03:08:41