testSymbolicFactorGraph.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 
19 
25 
27 
28 #include <boost/assign/std/set.hpp>
29 
30 using namespace std;
31 using namespace gtsam;
32 using namespace boost::assign;
33 
34 /* ************************************************************************* */
37  expected += 0, 1, 2, 3, 4;
38  KeySet actual = simpleTestGraph1.keys();
39  EXPECT(expected == actual);
40 }
41 
42 /* ************************************************************************* */
45  expected += 0, 1, 2, 3, 4, 5;
46  KeySet actual = simpleTestGraph2.keys();
47  EXPECT(expected == actual);
48 }
49 
50 /* ************************************************************************* */
51 TEST(SymbolicFactorGraph, eliminateFullSequential) {
52  // Test with simpleTestGraph1
53  Ordering order;
54  order += 0, 1, 2, 3, 4;
55  SymbolicBayesNet actual1 = *simpleTestGraph1.eliminateSequential(order);
56  EXPECT(assert_equal(simpleTestGraph1BayesNet, actual1));
57 
58  // Test with Asia graph
59  SymbolicBayesNet actual2 = *asiaGraph.eliminateSequential(asiaOrdering);
60  EXPECT(assert_equal(asiaBayesNet, actual2));
61 }
62 
63 /* ************************************************************************* */
64 TEST(SymbolicFactorGraph, eliminatePartialSequential) {
65  // Eliminate 0 and 1
66  const Ordering order = list_of(0)(1);
67 
68  const SymbolicBayesNet expectedBayesNet =
69  list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3, 4));
70 
71  const SymbolicFactorGraph expectedSfg = list_of(SymbolicFactor(2, 3))(
72  SymbolicFactor(4, 5))(SymbolicFactor(2, 3, 4));
73 
74  SymbolicBayesNet::shared_ptr actualBayesNet;
76  boost::tie(actualBayesNet, actualSfg) =
77  simpleTestGraph2.eliminatePartialSequential(Ordering(list_of(0)(1)));
78 
79  EXPECT(assert_equal(expectedSfg, *actualSfg));
80  EXPECT(assert_equal(expectedBayesNet, *actualBayesNet));
81 
82  SymbolicBayesNet::shared_ptr actualBayesNet2;
84  boost::tie(actualBayesNet2, actualSfg2) =
85  simpleTestGraph2.eliminatePartialSequential(
86  list_of(0)(1).convert_to_container<KeyVector>());
87 
88  EXPECT(assert_equal(expectedSfg, *actualSfg2));
89  EXPECT(assert_equal(expectedBayesNet, *actualBayesNet2));
90 }
91 
92 /* ************************************************************************* */
93 TEST(SymbolicFactorGraph, eliminateFullMultifrontal) {
95  ordering += 0, 1, 2, 3;
96  SymbolicBayesTree actual1 = *simpleChain.eliminateMultifrontal(ordering);
97  EXPECT(assert_equal(simpleChainBayesTree, actual1));
98 
99  SymbolicBayesTree actual2 = *asiaGraph.eliminateMultifrontal(asiaOrdering);
100  EXPECT(assert_equal(asiaBayesTree, actual2));
101 }
102 
103 /* ************************************************************************* */
104 TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) {
105  SymbolicBayesTree expectedBayesTree;
107  boost::make_shared<SymbolicConditional>(
108  SymbolicConditional::FromKeys(list_of(4)(5)(1), 2));
109  expectedBayesTree.insertRoot(
110  boost::make_shared<SymbolicBayesTreeClique>(root));
111 
112  SymbolicFactorGraph expectedFactorGraph =
113  list_of(SymbolicFactor(0, 1))(SymbolicFactor(0, 2))(SymbolicFactor(1, 3))(
114  SymbolicFactor(2, 3))(SymbolicFactor(1));
115 
116  SymbolicBayesTree::shared_ptr actualBayesTree;
117  SymbolicFactorGraph::shared_ptr actualFactorGraph;
118  boost::tie(actualBayesTree, actualFactorGraph) =
119  simpleTestGraph2.eliminatePartialMultifrontal(Ordering(list_of(4)(5)));
120 
121  EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph));
122  EXPECT(assert_equal(expectedBayesTree, *actualBayesTree));
123 
124  SymbolicBayesTree expectedBayesTree2;
126  boost::make_shared<SymbolicBayesTreeClique>(
127  boost::make_shared<SymbolicConditional>(4, 1));
128  root2->children.push_back(boost::make_shared<SymbolicBayesTreeClique>(
129  boost::make_shared<SymbolicConditional>(5, 4)));
130  expectedBayesTree2.insertRoot(root2);
131 
132  SymbolicBayesTree::shared_ptr actualBayesTree2;
133  SymbolicFactorGraph::shared_ptr actualFactorGraph2;
134  boost::tie(actualBayesTree2, actualFactorGraph2) =
135  simpleTestGraph2.eliminatePartialMultifrontal(
136  list_of<Key>(4)(5).convert_to_container<KeyVector>());
137 
138  EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2));
139  EXPECT(assert_equal(expectedBayesTree2, *actualBayesTree2));
140 }
141 
142 /* ************************************************************************* */
143 TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) {
144  SymbolicBayesNet expectedBayesNet =
145  list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3))(
147 
148  SymbolicBayesNet actual1 = *simpleTestGraph2.marginalMultifrontalBayesNet(
149  Ordering(list_of(0)(1)(2)(3)));
150  EXPECT(assert_equal(expectedBayesNet, actual1));
151 }
152 
153 /* ************************************************************************* */
154 TEST(SymbolicFactorGraph, eliminate_disconnected_graph) {
156  fg.push_factor(0, 1);
157  fg.push_factor(0, 2);
158  fg.push_factor(1, 2);
159  fg.push_factor(3, 4);
160 
161  // create expected Chordal bayes Net
163  expected.push_back(boost::make_shared<SymbolicConditional>(0, 1, 2));
164  expected.push_back(boost::make_shared<SymbolicConditional>(1, 2));
165  expected.push_back(boost::make_shared<SymbolicConditional>(2));
166  expected.push_back(boost::make_shared<SymbolicConditional>(3, 4));
167  expected.push_back(boost::make_shared<SymbolicConditional>(4));
168 
169  Ordering order;
170  order += 0, 1, 2, 3, 4;
171  SymbolicBayesNet actual = *fg.eliminateSequential(order);
172 
173  EXPECT(assert_equal(expected, actual));
174 }
175 
176 /* ************************************************************************* */
178  // Create factor graph
180  fg.push_factor(0, 1);
181  fg.push_factor(0, 2);
182  fg.push_factor(1, 4);
183  fg.push_factor(2, 4);
184  fg.push_factor(3, 4);
185 
186  // eliminate
187  Ordering ord(list_of(3)(4)(2)(1)(0));
188  auto actual = fg.eliminateSequential(ord);
190  expected.emplace_shared<SymbolicConditional>(3, 4);
191  expected.emplace_shared<SymbolicConditional>(4, 1, 2);
192  expected.emplace_shared<SymbolicConditional>(2, 0, 1);
193  expected.emplace_shared<SymbolicConditional>(1, 0);
194  expected.emplace_shared<SymbolicConditional>(0);
195  EXPECT(assert_equal(expected, *actual));
196 
197  {
198  // jointBayesNet
199  Ordering ord(list_of(0)(4)(3));
200  auto actual = fg.eliminatePartialSequential(ord);
201  SymbolicBayesNet expectedBN;
202  expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2);
203  expectedBN.emplace_shared<SymbolicConditional>(4, 1, 2, 3);
204  expectedBN.emplace_shared<SymbolicConditional>(3, 1, 2);
205  EXPECT(assert_equal(expectedBN, *(actual.first)));
206  }
207 
208  {
209  // jointBayesNet
210  Ordering ord(list_of(0)(2)(3));
211  auto actual = fg.eliminatePartialSequential(ord);
212  SymbolicBayesNet expectedBN;
213  expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2);
214  expectedBN.emplace_shared<SymbolicConditional>(2, 1, 4);
215  expectedBN.emplace_shared<SymbolicConditional>(3, 4);
216  EXPECT(assert_equal(expectedBN, *(actual.first)));
217  }
218 
219  {
220  // conditionalBayesNet
221  Ordering ord(list_of(0)(2));
222  auto actual = fg.eliminatePartialSequential(ord);
223  SymbolicBayesNet expectedBN;
224  expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2);
225  expectedBN.emplace_shared<SymbolicConditional>(2, 1, 4);
226  EXPECT(assert_equal(expectedBN, *(actual.first)));
227  }
228 }
229 
230 /* ************************************************************************* */
231 TEST(SymbolicFactorGraph, constructFromBayesNet) {
232  // create expected factor graph
234  expected.push_factor(0, 1, 2);
235  expected.push_factor(1, 2);
236  expected.push_factor(1);
237 
238  // create Bayes Net
239  SymbolicBayesNet bayesNet;
240  bayesNet += SymbolicConditional(0, 1, 2);
241  bayesNet += SymbolicConditional(1, 2);
242  bayesNet += SymbolicConditional(1);
243 
244  // create actual factor graph from a Bayes Net
245  SymbolicFactorGraph actual(bayesNet);
246 
247  CHECK(assert_equal(expected, actual));
248 }
249 
250 /* ************************************************************************* */
251 TEST(SymbolicFactorGraph, constructFromBayesTree) {
252  // create expected factor graph
254  expected.push_factor(_E_, _L_, _B_);
255  expected.push_factor(_S_, _B_, _L_);
256  expected.push_factor(_T_, _E_, _L_);
257  expected.push_factor(_X_, _E_);
258 
259  // create actual factor graph
260  SymbolicFactorGraph actual(asiaBayesTree);
261 
262  CHECK(assert_equal(expected, actual));
263 }
264 
265 /* ************************************************************************* */
267  // Create two factor graphs and expected combined graph
268  SymbolicFactorGraph fg1, fg2, expected;
269 
270  fg1.push_factor(1);
271  fg1.push_factor(0, 1);
272 
273  fg2.push_factor(1, 2);
274  fg2.push_factor(0, 2);
275 
276  expected.push_factor(1);
277  expected.push_factor(0, 1);
278  expected.push_factor(1, 2);
279  expected.push_factor(0, 2);
280 
281  // combine
282  SymbolicFactorGraph actual;
283  actual.push_back(fg1);
284  actual.push_back(fg2);
285  CHECK(assert_equal(expected, actual));
286 
287  // combine in second way
288  SymbolicFactorGraph actual2 = fg1;
289  actual2.push_back(fg2);
290  CHECK(assert_equal(expected, actual2));
291 }
292 
293 /* ************************************************************************* */
294 TEST(SymbolicFactorGraph, add_factors) {
296  fg1.push_factor(10);
297  fg1 += SymbolicFactor::shared_ptr(); // empty slot!
298  fg1.push_factor(11);
299 
301  fg2.push_factor(1);
302  fg2.push_factor(2);
303 
305  expected.push_factor(10);
306  expected.push_factor(1);
307  expected.push_factor(11);
308  expected.push_factor(2);
309  const FactorIndices expectedIndices = list_of(1)(3);
310  const FactorIndices actualIndices = fg1.add_factors(fg2, true);
311 
312  EXPECT(assert_equal(expected, fg1));
313  EXPECT(assert_container_equality(expectedIndices, actualIndices));
314 
315  expected.push_factor(1);
316  expected.push_factor(2);
317  const FactorIndices expectedIndices2 = list_of(4)(5);
318  const FactorIndices actualIndices2 = fg1.add_factors(fg2, false);
319 
320  EXPECT(assert_equal(expected, fg1));
321  EXPECT(assert_container_equality(expectedIndices2, actualIndices2));
322 }
323 
324 /* ************************************************************************* */
325 int main() {
326  TestResult tr;
327  return TestRegistry::runAllTests(tr);
328 }
329 /* ************************************************************************* */
std::pair< boost::shared_ptr< BayesTreeType >, boost::shared_ptr< FactorGraphType > > eliminatePartialMultifrontal(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Provides additional testing facilities for common data structures.
#define CHECK(condition)
Definition: Test.h:109
boost::shared_ptr< This > shared_ptr
boost::shared_ptr< This > shared_ptr
static int runAllTests(TestResult &result)
boost::shared_ptr< This > shared_ptr
static enum @843 ordering
Matrix expected
Definition: testMatrix.cpp:974
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:32
FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false)
Definition: Half.h:150
static const Key _L_
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:166
const mpreal root(const mpreal &x, unsigned long int k, mp_rnd_t r=mpreal::get_default_rnd())
Definition: mpreal.h:2194
void insertRoot(const sharedClique &subtree)
bool assert_container_equality(const std::map< size_t, V2 > &expected, const std::map< size_t, V2 > &actual)
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&...args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:172
boost::shared_ptr< This > shared_ptr
Typedef to the conditional base class.
#define EXPECT(condition)
Definition: Test.h:151
std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
boost::shared_ptr< This > shared_ptr
static const Key _B_
traits
Definition: chartTesting.h:28
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
boost::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType orderingType=boost::none, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
TEST(SymbolicFactorGraph, keys1)
Marginals marginals(graph, result)


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:49:57