testDiscreteMarginals.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 
12 /*
13  * @file testDiscreteMarginals.cpp
14  * @date Jun 7, 2012
15  * @author Abhijit Kundu
16  * @author Richard Roberts
17  * @author Frank Dellaert
18  */
19 
21 
22 #include <boost/assign/std/vector.hpp>
23 using namespace boost::assign;
24 
26 
27 using namespace std;
28 using namespace gtsam;
29 
30 /* ************************************************************************* */
32  size_t nrStates = 2;
33  DiscreteKey Cathy(1, nrStates), Heather(2, nrStates), Mark(3, nrStates),
34  Allison(4, nrStates);
36 
37  // add node potentials
38  graph.add(Cathy, "1 3");
39  graph.add(Heather, "9 1");
40  graph.add(Mark, "1 3");
41  graph.add(Allison, "9 1");
42 
43  // add edge potentials
44  graph.add(Cathy & Heather, "2 1 1 2");
45  graph.add(Heather & Mark, "2 1 1 2");
46  graph.add(Mark & Allison, "2 1 1 2");
47 
49  DiscreteFactor::shared_ptr actualC = marginals(Cathy.first);
51 
52  values[Cathy.first] = 0;
53  EXPECT_DOUBLES_EQUAL( 0.359631, (*actualC)(values), 1e-6);
54 
55  Vector actualCvector = marginals.marginalProbabilities(Cathy);
56  EXPECT(assert_equal(Vector2(0.359631, 0.640369), actualCvector, 1e-6));
57 
58  actualCvector = marginals.marginalProbabilities(Mark);
59  EXPECT(assert_equal(Vector2(0.48628, 0.51372), actualCvector, 1e-6));
60 }
61 
62 /* ************************************************************************* */
64 
65  const int nrNodes = 10;
66  const size_t nrStates = 7;
67 
68  // define variables
69  vector<DiscreteKey> key;
70  for (int i = 0; i < nrNodes; i++) {
71  DiscreteKey key_i(i, nrStates);
72  key.push_back(key_i);
73  }
74 
75  // create graph
77 
78  // add node potentials
79  graph.add(key[0], ".3 .6 .1 0 0 0 0");
80  for (int i = 1; i < nrNodes; i++)
81  graph.add(key[i], "1 1 1 1 1 1 1");
82 
83  const std::string edgePotential = ".08 .9 .01 0 0 0 .01 "
84  ".03 .95 .01 0 0 0 .01 "
85  ".06 .06 .75 .05 .05 .02 .01 "
86  "0 0 0 .3 .6 .09 .01 "
87  "0 0 0 .02 .95 .02 .01 "
88  "0 0 0 .01 .01 .97 .01 "
89  "0 0 0 0 0 0 1";
90 
91  // add edge potentials
92  for (int i = 0; i < nrNodes - 1; i++)
93  graph.add(key[i] & key[i + 1], edgePotential);
94 
96  DiscreteFactor::shared_ptr actualC = marginals(key[2].first);
98 
99  values[key[2].first] = 0;
100  EXPECT_DOUBLES_EQUAL( 0.03426, (*actualC)(values), 1e-4);
101 }
102 
103 /* ************************************************************************* */
105 
106  const int nrNodes = 5;
107  const size_t nrStates = 2;
108 
109  // define variables
110  vector<DiscreteKey> key;
111  for (int i = 0; i < nrNodes; i++) {
112  DiscreteKey key_i(i, nrStates);
113  key.push_back(key_i);
114  }
115 
116  // create graph and add three truss potentials
118  graph.add(key[0] & key[2] & key[4],"2 2 2 2 1 1 1 1");
119  graph.add(key[1] & key[3] & key[4],"1 1 1 1 2 2 2 2");
120  graph.add(key[2] & key[3] & key[4],"1 1 1 1 1 1 1 1");
122 // bayesTree->print("Bayes Tree");
123  typedef DiscreteBayesTreeClique Clique;
124 
125  Clique expected0(boost::make_shared<DiscreteConditional>((key[0] | key[2], key[4]) = "2/1 2/1 2/1 2/1"));
126  Clique::shared_ptr actual0 = (*bayesTree)[0];
127 // EXPECT(assert_equal(expected0, *actual0)); // TODO, correct but fails
128 
129  Clique expected1(boost::make_shared<DiscreteConditional>((key[1] | key[3], key[4]) = "1/2 1/2 1/2 1/2"));
130  Clique::shared_ptr actual1 = (*bayesTree)[1];
131 // EXPECT(assert_equal(expected1, *actual1)); // TODO, correct but fails
132 
133  // Create Marginals instance
135 
136  // test 0
137  DecisionTreeFactor expectedM0(key[0],"0.666667 0.333333");
139  EXPECT(assert_equal(expectedM0, *boost::dynamic_pointer_cast<DecisionTreeFactor>(actualM0),1e-5));
140 
141  // test 1
142  DecisionTreeFactor expectedM1(key[1],"0.333333 0.666667");
144  EXPECT(assert_equal(expectedM1, *boost::dynamic_pointer_cast<DecisionTreeFactor>(actualM1),1e-5));
145 }
146 
147 /* ************************************************************************* */
148 // Second truss example with non-trivial factors
150  const int nrNodes = 5;
151  const size_t nrStates = 2;
152 
153  // define variables
154  vector<DiscreteKey> key;
155  for (int i = 0; i < nrNodes; i++) {
156  DiscreteKey key_i(i, nrStates);
157  key.push_back(key_i);
158  }
159 
160  // create graph and add three truss potentials
162  graph.add(key[0] & key[2] & key[4], "1 2 3 4 5 6 7 8");
163  graph.add(key[1] & key[3] & key[4], "1 2 3 4 5 6 7 8");
164  graph.add(key[2] & key[3] & key[4], "1 2 3 4 5 6 7 8");
165 
166  // Calculate the marginals by brute force
167  vector<DiscreteFactor::Values> allPosbValues =
168  cartesianProduct(key[0] & key[1] & key[2] & key[3] & key[4]);
169  Vector T = Z_5x1, F = Z_5x1;
170  for (size_t i = 0; i < allPosbValues.size(); ++i) {
171  DiscreteFactor::Values x = allPosbValues[i];
172  double px = graph(x);
173  for (size_t j = 0; j < 5; j++)
174  if (x[j])
175  T[j] += px;
176  else
177  F[j] += px;
178  }
179 
180  // Check all marginals given by a sequential solver and Marginals
181  // DiscreteSequentialSolver solver(graph);
183  for (size_t j = 0; j < 5; j++) {
184  double sum = T[j] + F[j];
185  T[j] /= sum;
186  F[j] /= sum;
187 
188  // Marginals
189  vector<double> table;
190  table += F[j], T[j];
191  DecisionTreeFactor expectedM(key[j], table);
194  expectedM, *boost::dynamic_pointer_cast<DecisionTreeFactor>(actualM)));
195  }
196 }
197 
198 /* ************************************************************************* */
199 int main() {
200  TestResult tr;
201  return TestRegistry::runAllTests(tr);
202 }
203 /* ************************************************************************* */
204 
int main()
std::vector< Assignment< L > > cartesianProduct(const std::vector< std::pair< L, size_t > > &keys)
Get Cartesian product consisting all possible configurations.
Definition: Assignment.h:62
void add(const DiscreteKey &j, SOURCE table)
TEST_UNSAFE(DiscreteMarginals, UGM_small)
static int runAllTests(TestResult &result)
Vector marginalProbabilities(const DiscreteKey &key) const
boost::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType orderingType=boost::none, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
leaf::MyValues values
Definition: Half.h:150
NonlinearFactorGraph graph
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:162
Signature::Row F
Definition: Signature.cpp:53
constexpr int first(int i)
Implementation details for constexpr functions.
Eigen::VectorXd Vector
Definition: Vector.h:38
std::pair< Key, size_t > DiscreteKey
Definition: DiscreteKey.h:34
#define EXPECT(condition)
Definition: Test.h:151
RealScalar RealScalar * px
Array< double, 1, 3 > e(1./3., 0.5, 2.)
const mpreal sum(const mpreal tab[], const unsigned long int n, int &status, mp_rnd_t mode=mpreal::get_default_rnd())
Definition: mpreal.h:2381
A class for computing marginals in a DiscreteFactorGraph.
boost::shared_ptr< DiscreteFactor > shared_ptr
shared_ptr to this class
traits
Definition: chartTesting.h:28
boost::shared_ptr< This > shared_ptr
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
Eigen::Vector2d Vector2
Definition: Vector.h:42
ArrayXXf table(10, 4)
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Marginals marginals(graph, result)
std::ptrdiff_t j


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:46:26