testSymbolicBayesTree.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 testSymbolicBayesTree.cpp
14  * @date sept 15, 2012
15  * @author Frank Dellaert
16  * @author Michael Kaess
17  * @author Viorela Ila
18  */
19 
20 #include <gtsam/inference/Symbol.h>
24 
27 #include <iterator>
28 #include <type_traits>
29 
30 using namespace std;
31 using namespace gtsam;
32 using namespace gtsam::symbol_shorthand;
33 
34 static bool debug = false;
35 
36 // Given a vector of shared pointers infer the type of the pointed-to objects
37 template<typename T>
38 using PointedToType = std::decay_t<decltype(**declval<T>().begin())>;
39 
40 // Given a vector of shared pointers infer the type of the pointed-to objects
41 template<typename T>
42 using ValuesVector = std::vector<PointedToType<T>>;
43 
44 // Return a vector of dereferenced values
45 template<typename T>
48  for (auto& t : v)
49  result.push_back(*t);
50  return result;
51 }
52 
53 
54 /* ************************************************************************* */
56  SymbolicBayesTree bayesTree = asiaBayesTree;
57  bayesTree.clear();
58 
60 
61  // Check whether cleared BayesTree is equal to a new BayesTree
62  CHECK(assert_equal(expected, bayesTree));
63 }
64 
65 /* ************************************************************************* */
66 TEST(SymbolicBayesTree, clique_structure) {
67  // l1 l2
68  // / | / |
69  // x1 --- x2 --- x3 --- x4 --- x5
70  // \ |
71  // l3
73  graph.emplace_shared<SymbolicFactor>(X(1), L(1));
74  graph.emplace_shared<SymbolicFactor>(X(1), X(2));
75  graph.emplace_shared<SymbolicFactor>(X(2), L(1));
76  graph.emplace_shared<SymbolicFactor>(X(2), X(3));
77  graph.emplace_shared<SymbolicFactor>(X(3), X(4));
78  graph.emplace_shared<SymbolicFactor>(X(4), L(2));
79  graph.emplace_shared<SymbolicFactor>(X(4), X(5));
80  graph.emplace_shared<SymbolicFactor>(L(2), X(5));
81  graph.emplace_shared<SymbolicFactor>(X(4), L(3));
82  graph.emplace_shared<SymbolicFactor>(X(5), L(3));
83 
85  expected.insertRoot(
86  NodeClique(Keys(X(2))(X(3)), 2,
87  Children(NodeClique(
88  Keys(X(4))(X(3)), 1,
89  Children(NodeClique(
90  Keys(X(5))(L(2))(X(4)), 2,
91  Children(LeafClique(Keys(L(3))(X(4))(X(5)), 1))))))(
92  LeafClique(Keys(X(1))(L(1))(X(2)), 2))));
93 
94  Ordering order{X(1), L(3), L(1), X(5), X(2), L(2), X(4), X(3)};
95 
96  SymbolicBayesTree actual = *graph.eliminateMultifrontal(order);
97 
98  EXPECT(assert_equal(expected, actual));
99 }
100 
101 /* ************************************************************************* *
102 Bayes Tree for testing conversion to a forest of orphans needed for incremental.
103  A,B
104  C|A E|B
105  D|C F|E
106  */
107 /* ************************************************************************* */
108 TEST(BayesTree, removePath) {
109  const Key _A_ = A(0), _B_ = B(0), _C_ = C(0), _D_ = D(0), _E_ = E(0),
110  _F_ = F(0);
111 
112  SymbolicBayesTree bayesTreeOrig;
113  auto left = NodeClique(Keys(_C_)(_A_), 1, {LeafClique(Keys(_D_)(_C_), 1)});
114  auto right = NodeClique(Keys(_E_)(_B_), 1, {LeafClique(Keys(_F_)(_E_), 1)});
115  bayesTreeOrig.insertRoot(NodeClique(Keys(_A_)(_B_), 2, {left, right}));
116 
117  SymbolicBayesTree bayesTree = bayesTreeOrig;
118 
119  // remove C, expected outcome: factor graph with ABC,
120  // Bayes Tree now contains two orphan trees: D|C and E|B,F|E
122  expected.emplace_shared<SymbolicFactor>(_A_, _B_);
123  expected.emplace_shared<SymbolicFactor>(_C_, _A_);
124  SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_D_], bayesTree[_E_]};
125 
126  SymbolicBayesNet bn;
127  SymbolicBayesTree::Cliques orphans;
128  bayesTree.removePath(bayesTree[_C_], &bn, &orphans);
130  CHECK(assert_equal(expected, factors));
131  CHECK(assert_container_equal(deref(expectedOrphans), deref(orphans)));
132 
133  bayesTree = bayesTreeOrig;
134 
135  // remove E: factor graph with EB; E|B removed from second orphan tree
136  SymbolicFactorGraph expected2;
137  expected2.emplace_shared<SymbolicFactor>(_A_, _B_);
138  expected2.emplace_shared<SymbolicFactor>(_E_, _B_);
139  SymbolicBayesTree::Cliques expectedOrphans2{bayesTree[_F_], bayesTree[_C_]};
140 
141  SymbolicBayesNet bn2;
142  SymbolicBayesTree::Cliques orphans2;
143  bayesTree.removePath(bayesTree[_E_], &bn2, &orphans2);
144  SymbolicFactorGraph factors2(bn2);
145  CHECK(assert_equal(expected2, factors2));
146  CHECK(assert_container_equal(deref(expectedOrphans2), deref(orphans2)));
147 }
148 
149 /* ************************************************************************* */
150 TEST(BayesTree, removePath2) {
151  SymbolicBayesTree bayesTree = asiaBayesTree;
152 
153  // Call remove-path with clique B
154  SymbolicBayesNet bn;
155  SymbolicBayesTree::Cliques orphans;
156  bayesTree.removePath(bayesTree[_B_], &bn, &orphans);
158 
159  // Check expected outcome
161  expected.emplace_shared<SymbolicFactor>(_E_, _L_, _B_);
162  CHECK(assert_equal(expected, factors));
163  SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_S_], bayesTree[_T_],
164  bayesTree[_X_]};
165  CHECK(assert_container_equal(deref(expectedOrphans), deref(orphans)));
166 }
167 
168 /* ************************************************************************* */
169 TEST(BayesTree, removePath3) {
170  SymbolicBayesTree bayesTree = asiaBayesTree;
171 
172  // Call remove-path with clique T
173  SymbolicBayesNet bn;
174  SymbolicBayesTree::Cliques orphans;
175  bayesTree.removePath(bayesTree[_T_], &bn, &orphans);
177 
178  // Check expected outcome
180  expected.emplace_shared<SymbolicFactor>(_E_, _L_, _B_);
181  expected.emplace_shared<SymbolicFactor>(_T_, _E_, _L_);
182  CHECK(assert_equal(expected, factors));
183  SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_S_], bayesTree[_X_]};
184  CHECK(assert_container_equal(deref(expectedOrphans), deref(orphans)));
185 }
186 
187 void getAllCliques(const SymbolicBayesTree::sharedClique& subtree,
188  SymbolicBayesTree::Cliques& cliques) {
189  // Check if subtree exists
190  if (subtree) {
191  cliques.push_back(subtree);
192  // Recursive call over all child cliques
193  for (SymbolicBayesTree::sharedClique& childClique : subtree->children) {
194  getAllCliques(childClique, cliques);
195  }
196  }
197 }
198 
199 /* ************************************************************************* */
200 TEST(BayesTree, shortcutCheck) {
201  const Key _A_ = 6, _B_ = 5, _C_ = 4, _D_ = 3, _E_ = 2, _F_ = 1, _G_ = 0;
202  auto chain = SymbolicFactorGraph(SymbolicFactor(_A_)) //
203  (SymbolicFactor(_B_, _A_)) //
204  (SymbolicFactor(_C_, _A_)) //
205  (SymbolicFactor(_D_, _C_)) //
206  (SymbolicFactor(_E_, _B_)) //
207  (SymbolicFactor(_F_, _E_)) //
208  (SymbolicFactor(_G_, _F_));
209  Ordering ordering{_G_, _F_, _E_, _D_, _C_, _B_, _A_};
210  SymbolicBayesTree bayesTree = *chain.eliminateMultifrontal(ordering);
211 
212  // bayesTree.saveGraph("BT1.dot");
213 
214  SymbolicBayesTree::sharedClique rootClique = bayesTree.roots().front();
215  // rootClique->printTree();
216  SymbolicBayesTree::Cliques allCliques;
217  getAllCliques(rootClique, allCliques);
218 
219  for (SymbolicBayesTree::sharedClique& clique : allCliques) {
220  // clique->print("Clique#");
221  SymbolicBayesNet bn = clique->shortcut(rootClique);
222  // bn.print("Shortcut:\n");
223  // cout << endl;
224  }
225 
226  // Check if all the cached shortcuts are cleared
227  rootClique->deleteCachedShortcuts();
228  for (SymbolicBayesTree::sharedClique& clique : allCliques) {
229  bool notCleared = clique->cachedSeparatorMarginal().has_value();
230  CHECK(notCleared == false);
231  }
232  EXPECT_LONGS_EQUAL(0, (long)rootClique->numCachedSeparatorMarginals());
233 
234  // for(SymbolicBayesTree::sharedClique& clique: allCliques) {
235  // clique->print("Clique#");
236  // if(clique->cachedShortcut()){
237  // bn = clique->cachedShortcut().get();
238  // bn.print("Shortcut:\n");
239  // }
240  // else
241  // cout << "Not Initialized" << endl;
242  // cout << endl;
243  // }
244 }
245 
246 /* ************************************************************************* */
247 TEST(BayesTree, removeTop) {
248  SymbolicBayesTree bayesTree = asiaBayesTree;
249 
250  // create a new factor to be inserted
251  // std::shared_ptr<IndexFactor> newFactor(new IndexFactor(_S_,_B_));
252 
253  // Remove the contaminated part of the Bayes tree
254  SymbolicBayesNet bn;
255  SymbolicBayesTree::Cliques orphans;
256  bayesTree.removeTop(Keys(_B_)(_S_), &bn, &orphans);
257 
258  // Check expected outcome
260  expected.add(SymbolicConditional::FromKeys<KeyVector>(Keys(_E_)(_L_)(_B_), 3));
261  expected.add(SymbolicConditional::FromKeys<KeyVector>(Keys(_S_)(_B_)(_L_), 1));
262  CHECK(assert_equal(expected, bn));
263 
264  SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_T_], bayesTree[_X_]};
265  CHECK(assert_container_equal(deref(expectedOrphans), deref(orphans)));
266 
267  // Try removeTop again with a factor that should not change a thing
268  // std::shared_ptr<IndexFactor> newFactor2(new IndexFactor(_B_));
269  SymbolicBayesNet bn2;
270  SymbolicBayesTree::Cliques orphans2;
271  bayesTree.removeTop(Keys(_B_), &bn2, &orphans2);
272  SymbolicFactorGraph factors2(bn2);
273  SymbolicFactorGraph expected2;
274  CHECK(assert_equal(expected2, factors2));
275  SymbolicBayesTree::Cliques expectedOrphans2;
276  CHECK(assert_container_equal(deref(expectedOrphans2), deref(orphans2)));
277 }
278 
279 /* ************************************************************************* */
280 TEST(BayesTree, removeTop2) {
281  SymbolicBayesTree bayesTree = asiaBayesTree;
282 
283  // create two factors to be inserted
284  // SymbolicFactorGraph newFactors;
285  // newFactors.push_factor(_B_);
286  // newFactors.push_factor(_S_);
287 
288  // Remove the contaminated part of the Bayes tree
289  SymbolicBayesNet bn;
290  SymbolicBayesTree::Cliques orphans;
291  bayesTree.removeTop(Keys(_T_), &bn, &orphans);
292 
293  // Check expected outcome
294  auto expected = SymbolicBayesNet(
295  SymbolicConditional::FromKeys<KeyVector>(Keys(_E_)(_L_)(_B_), 3))(
296  SymbolicConditional::FromKeys<KeyVector>(Keys(_T_)(_E_)(_L_), 1));
298 
299  SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_S_], bayesTree[_X_]};
300  CHECK(assert_container_equal(deref(expectedOrphans), deref(orphans)));
301 }
302 
303 /* ************************************************************************* */
304 TEST(BayesTree, removeTop3) {
306  X(4), L(5)))(SymbolicFactor(X(2), X(4)))(SymbolicFactor(X(3), X(2)));
307  Ordering ordering{X(3), X(2), X(4), L(5)};
308  SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering);
309 
310  // remove all
311  SymbolicBayesNet bn;
312  SymbolicBayesTree::Cliques orphans;
313  bayesTree.removeTop(Keys(L(5))(X(4))(X(2))(X(3)), &bn, &orphans);
314 
315  auto expectedBn = SymbolicBayesNet(
316  SymbolicConditional::FromKeys<KeyVector>(Keys(X(4))(L(5)), 2))(
317  SymbolicConditional(X(2), X(4)))(SymbolicConditional(X(3), X(2)));
318  EXPECT(assert_equal(expectedBn, bn));
319  EXPECT(orphans.empty());
320 }
321 
322 /* ************************************************************************* */
323 TEST(BayesTree, removeTop4) {
325  X(4), L(5)))(SymbolicFactor(X(2), X(4)))(SymbolicFactor(X(3), X(2)));
326  Ordering ordering{X(3), X(2), X(4), L(5)};
327  SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering);
328 
329  // remove all
330  SymbolicBayesNet bn;
331  SymbolicBayesTree::Cliques orphans;
332  bayesTree.removeTop(Keys(X(2))(L(5))(X(4))(X(3)), &bn, &orphans);
333 
334  auto expectedBn = SymbolicBayesNet(
335  SymbolicConditional::FromKeys<KeyVector>(Keys(X(4))(L(5)), 2))(
336  SymbolicConditional(X(2), X(4)))(SymbolicConditional(X(3), X(2)));
337  EXPECT(assert_equal(expectedBn, bn));
338  EXPECT(orphans.empty());
339 }
340 
341 /* ************************************************************************* */
342 TEST(BayesTree, removeTop5) {
343  // Remove top called with variables that are not in the Bayes tree
345  X(4), L(5)))(SymbolicFactor(X(2), X(4)))(SymbolicFactor(X(3), X(2)));
346  Ordering ordering{X(3), X(2), X(4), L(5)};
347  SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering);
348 
349  // Remove nonexistant
350  SymbolicBayesNet bn;
351  SymbolicBayesTree::Cliques orphans;
352  bayesTree.removeTop(Keys(X(10)), &bn, &orphans);
353 
354  SymbolicBayesNet expectedBn;
355  EXPECT(assert_equal(expectedBn, bn));
356  EXPECT(orphans.empty());
357 }
358 
359 /* ************************************************************************* */
361  // create a thin-tree Bayes net, a la Jean-Guillaume
362  SymbolicBayesNet bayesNet;
363  bayesNet.emplace_shared<SymbolicConditional>(14);
364 
365  bayesNet.emplace_shared<SymbolicConditional>(13, 14);
366  bayesNet.emplace_shared<SymbolicConditional>(12, 14);
367 
368  bayesNet.emplace_shared<SymbolicConditional>(11, 13, 14);
369  bayesNet.emplace_shared<SymbolicConditional>(10, 13, 14);
370  bayesNet.emplace_shared<SymbolicConditional>(9, 12, 14);
371  bayesNet.emplace_shared<SymbolicConditional>(8, 12, 14);
372 
373  bayesNet.emplace_shared<SymbolicConditional>(7, 11, 13);
374  bayesNet.emplace_shared<SymbolicConditional>(6, 11, 13);
375  bayesNet.emplace_shared<SymbolicConditional>(5, 10, 13);
376  bayesNet.emplace_shared<SymbolicConditional>(4, 10, 13);
377 
378  bayesNet.emplace_shared<SymbolicConditional>(3, 9, 12);
379  bayesNet.emplace_shared<SymbolicConditional>(2, 9, 12);
380  bayesNet.emplace_shared<SymbolicConditional>(1, 8, 12);
381  bayesNet.emplace_shared<SymbolicConditional>(0, 8, 12);
382 
383  if (debug) {
384  GTSAM_PRINT(bayesNet);
385  bayesNet.saveGraph("/tmp/symbolicBayesNet.dot");
386  }
387 
388  // create a BayesTree out of a Bayes net
389  SymbolicBayesTree bayesTree =
391  if (debug) {
392  GTSAM_PRINT(bayesTree);
393  bayesTree.saveGraph("/tmp/SymbolicBayesTree.dot");
394  }
395 
396  SymbolicBayesTree::Clique::shared_ptr R = bayesTree.roots().front();
397 
398  {
399  // check shortcut P(S9||R) to root
401  SymbolicBayesNet shortcut = c->shortcut(R);
402  auto expected = SymbolicBayesNet(SymbolicConditional(14, 11, 13));
403  EXPECT(assert_equal(expected, shortcut));
404  }
405 
406  {
407  // check shortcut P(S8||R) to root
409  SymbolicBayesNet shortcut = c->shortcut(R);
411  SymbolicConditional(14, 11, 13));
412  EXPECT(assert_equal(expected, shortcut));
413  }
414 
415  {
416  // check shortcut P(S4||R) to root
418  SymbolicBayesNet shortcut = c->shortcut(R);
419  auto expected = SymbolicBayesNet(SymbolicConditional(10, 11, 13));
420  EXPECT(assert_equal(expected, shortcut));
421  }
422 
423  {
424  // check shortcut P(S2||R) to root
426  SymbolicBayesNet shortcut = c->shortcut(R);
427  auto expected = SymbolicBayesNet(SymbolicConditional(9, 11, 12, 13))(
428  SymbolicConditional(12, 11, 13));
429  EXPECT(assert_equal(expected, shortcut));
430  }
431 
432  {
433  // check shortcut P(S0||R) to root
435  SymbolicBayesNet shortcut = c->shortcut(R);
436  auto expected = SymbolicBayesNet(SymbolicConditional(8, 11, 12, 13))(
437  SymbolicConditional(12, 11, 13));
438  EXPECT(assert_equal(expected, shortcut));
439  }
440 
441  SymbolicBayesNet::shared_ptr actualJoint;
442 
443  // Check joint P(8,2)
444  if (false) { // TODO, not disjoint
445  actualJoint = bayesTree.jointBayesNet(8, 2);
447  expected.emplace_shared<SymbolicConditional>(8);
448  expected.emplace_shared<SymbolicConditional>(2, 8);
449  EXPECT(assert_equal(expected, *actualJoint));
450  }
451 
452  // Check joint P(1,2)
453  if (false) { // TODO, not disjoint
454  actualJoint = bayesTree.jointBayesNet(1, 2);
456  expected.emplace_shared<SymbolicConditional>(2);
457  expected.emplace_shared<SymbolicConditional>(1, 2);
458  EXPECT(assert_equal(expected, *actualJoint));
459  }
460 
461  // Check joint P(2,6)
462  if (true) {
463  actualJoint = bayesTree.jointBayesNet(2, 6);
465  expected.emplace_shared<SymbolicConditional>(2, 6);
466  expected.emplace_shared<SymbolicConditional>(6);
467  EXPECT(assert_equal(expected, *actualJoint));
468  }
469 
470  // Check joint P(4,6)
471  if (false) { // TODO, not disjoint
472  actualJoint = bayesTree.jointBayesNet(4, 6);
474  expected.emplace_shared<SymbolicConditional>(6);
475  expected.emplace_shared<SymbolicConditional>(4, 6);
476  EXPECT(assert_equal(expected, *actualJoint));
477  }
478 }
479 
480 /* ************************************************************************* */
481 TEST(SymbolicBayesTree, forest_joint) {
482  // Create forest
483  sharedClique root1 = LeafClique(Keys(1), 1);
484  sharedClique root2 = LeafClique(Keys(2), 1);
485  SymbolicBayesTree bayesTree;
486  bayesTree.insertRoot(root1);
487  bayesTree.insertRoot(root2);
488 
489  // Check joint
490  auto expected =
492  SymbolicBayesNet actual = *bayesTree.jointBayesNet(1, 2);
493 
494  EXPECT(assert_equal(expected, actual));
495 }
496 
497 /* ************************************************************************* *
498  Bayes tree for smoother with "natural" ordering:
499  C1 5 6
500  C2 4 : 5
501  C3 3 : 4
502  C4 2 : 3
503  C5 1 : 2
504  C6 0 : 1
505  **************************************************************************** */
506 
507 TEST(SymbolicBayesTree, linear_smoother_shortcuts) {
508  // Create smoother with 7 nodes
509  SymbolicFactorGraph smoother;
510  smoother.push_factor(0);
511  smoother.push_factor(0, 1);
512  smoother.push_factor(1, 2);
513  smoother.push_factor(2, 3);
514  smoother.push_factor(3, 4);
515  smoother.push_factor(4, 5);
516  smoother.push_factor(5, 6);
517 
518  // Eliminate in numerical order 0..6
519  Ordering ordering(smoother.keys());
520  SymbolicBayesNet bayesNet = *smoother.eliminateSequential(ordering);
521 
522  if (debug) {
523  GTSAM_PRINT(bayesNet);
524  bayesNet.saveGraph("/tmp/symbolicBayesNet.dot");
525  }
526 
527  // create a BayesTree
528  SymbolicBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
529  if (debug) {
530  GTSAM_PRINT(bayesTree);
531  bayesTree.saveGraph("/tmp/SymbolicBayesTree.dot");
532  }
533 
534  SymbolicBayesTree::Clique::shared_ptr R = bayesTree.roots().front();
535 
536  {
537  // check shortcut P(S2||R) to root
539  bayesTree[4]; // 4 is frontal in C2
540  SymbolicBayesNet shortcut = c->shortcut(R);
542  EXPECT(assert_equal(expected, shortcut));
543  }
544 
545  {
546  // check shortcut P(S3||R) to root
548  bayesTree[3]; // 3 is frontal in C3
549  SymbolicBayesNet shortcut = c->shortcut(R);
551  EXPECT(assert_equal(expected, shortcut));
552  }
553 
554  {
555  // check shortcut P(S4||R) to root
557  bayesTree[2]; // 2 is frontal in C4
558  SymbolicBayesNet shortcut = c->shortcut(R);
560  EXPECT(assert_equal(expected, shortcut));
561  }
562 }
563 
564 /* ************************************************************************* */
565 // from testSymbolicJunctionTree, which failed at one point
566 TEST(SymbolicBayesTree, complicatedMarginal) {
567  // Create the conditionals to go in the BayesTree
568  sharedClique cur;
569  sharedClique root = LeafClique(Keys(11)(12), 2);
570  cur = root;
571 
572  root->children.push_back(LeafClique(Keys(9)(10)(11)(12), 2));
573  root->children.back()->parent_ = root;
574 
575  root->children.push_back(LeafClique(Keys(7)(8)(11), 2));
576  root->children.back()->parent_ = root;
577  cur = root->children.back();
578 
579  cur->children.push_back(LeafClique(Keys(5)(6)(7)(8), 2));
580  cur->children.back()->parent_ = cur;
581  cur = cur->children.back();
582 
583  cur->children.push_back(LeafClique(Keys(3)(4)(6), 2));
584  cur->children.back()->parent_ = cur;
585 
586  cur->children.push_back(LeafClique(Keys(1)(2)(5), 2));
587  cur->children.back()->parent_ = cur;
588 
589  // Create Bayes Tree
591  bt.insertRoot(root);
592  if (debug) {
593  GTSAM_PRINT(bt);
594  bt.saveGraph("/tmp/SymbolicBayesTree.dot");
595  }
596 
597  // Shortcut on 9
598  {
600  SymbolicBayesNet shortcut = c->shortcut(root);
601  EXPECT(assert_equal(SymbolicBayesNet(), shortcut));
602  }
603 
604  // Shortcut on 7
605  {
607  SymbolicBayesNet shortcut = c->shortcut(root);
608  EXPECT(assert_equal(SymbolicBayesNet(), shortcut));
609  }
610 
611  // Shortcut on 5
612  {
614  SymbolicBayesNet shortcut = c->shortcut(root);
616  SymbolicConditional(8, 11));
617  EXPECT(assert_equal(expected, shortcut));
618  }
619 
620  // Shortcut on 3
621  {
623  SymbolicBayesNet shortcut = c->shortcut(root);
625  EXPECT(assert_equal(expected, shortcut));
626  }
627 
628  // Shortcut on 1
629  {
631  SymbolicBayesNet shortcut = c->shortcut(root);
633  EXPECT(assert_equal(expected, shortcut));
634  }
635 
636  // Marginal on 5
637  {
639  EXPECT(assert_equal(SymbolicFactor(5), *actual, 1e-1));
640  }
641 
642  // Shortcut on 6
643  {
645  EXPECT(assert_equal(SymbolicFactor(6), *actual, 1e-1));
646  }
647 }
648 
649 /* ************************************************************************* */
650 TEST(SymbolicBayesTree, COLAMDvsMETIS) {
651  // create circular graph
653  sfg.push_factor(0, 1);
654  sfg.push_factor(1, 2);
655  sfg.push_factor(2, 3);
656  sfg.push_factor(3, 4);
657  sfg.push_factor(4, 5);
658  sfg.push_factor(0, 5);
659 
660  // COLAMD
661  {
662  Ordering ordering = Ordering::Create(Ordering::COLAMD, sfg);
663  EXPECT(assert_equal(Ordering{0, 5, 1, 4, 2, 3}, ordering));
664 
665  // - P( 4 2 3)
666  // | - P( 1 | 2 4)
667  // | | - P( 5 | 1 4)
668  // | | | - P( 0 | 1 5)
670  expected.insertRoot( //
671  NodeClique(
672  Keys(4)(2)(3), 3,
673  Children( //
674  NodeClique(
675  Keys(1)(2)(4), 1,
676  Children( //
677  NodeClique(Keys(5)(1)(4), 1,
678  Children( //
679  LeafClique(Keys(0)(1)(5), 1))))))));
680 
681  SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering);
682  EXPECT(assert_equal(expected, actual));
683  }
684 
685 #ifdef GTSAM_SUPPORT_NESTED_DISSECTION
686  // METIS
687  {
688  Ordering ordering = Ordering::Create(Ordering::METIS, sfg);
689 // Linux and Mac split differently when using Metis
690 #if defined(__APPLE__)
691  EXPECT(assert_equal(Ordering{5, 4, 2, 1, 0, 3}, ordering));
692 #elif defined(_WIN32)
693  EXPECT(assert_equal(Ordering{4, 3, 1, 0, 5, 2}, ordering));
694 #else
695  EXPECT(assert_equal(Ordering{3, 2, 5, 0, 4, 1}, ordering));
696 #endif
697 
698  // - P( 1 0 3)
699  // | - P( 4 | 0 3)
700  // | | - P( 5 | 0 4)
701  // | - P( 2 | 1 3)
703 #if defined(__APPLE__)
704  expected.insertRoot(
705  NodeClique(Keys(1)(0)(3), 3,
706  Children( //
707  NodeClique(Keys(4)(0)(3), 1, //
708  {LeafClique(Keys(5)(0)(4), 1)}))(
709  LeafClique(Keys(2)(1)(3), 1))));
710 #elif defined(_WIN32)
711  expected.insertRoot(
712  NodeClique(Keys(3)(5)(2), 3,
713  Children( //
714  NodeClique(Keys(4)(3)(5), 1, //
715  {LeafClique(Keys(0)(2)(5), 1)}))(
716  LeafClique(Keys(1)(0)(2), 1))));
717 #else
718  expected.insertRoot(
719  NodeClique(Keys(2)(4)(1), 3,
720  Children( //
721  NodeClique(Keys(0)(1)(4), 1, //
722  {LeafClique(Keys(5)(0)(4), 1)}))(
723  LeafClique(Keys(3)(2)(4), 1))));
724 #endif
725  SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering);
726  EXPECT(assert_equal(expected, actual));
727  }
728 #endif
729 }
730 
731 /* ************************************************************************* */
732 int main() {
733  TestResult tr;
734  return TestRegistry::runAllTests(tr);
735 }
736 /* ************************************************************************* */
void removePath(sharedClique clique, BayesNetType *bn, Cliques *orphans)
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
Provides additional testing facilities for common data structures.
int main()
#define CHECK(condition)
Definition: Test.h:108
void removeTop(const KeyVector &keys, BayesNetType *bn, Cliques *orphans)
std::decay_t< decltype(**declval< T >().begin())> PointedToType
KeySet keys() const
static const Key _C_
Key F(std::uint64_t j)
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
static int runAllTests(TestResult &result)
std::shared_ptr< This > shared_ptr
ValuesVector< T > deref(const T &v)
Matrix expected
Definition: testMatrix.cpp:971
static bool debug
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
Rot2 R(Rot2::fromAngle(0.1))
const GaussianFactorGraph factors
MatrixXd L
Definition: LLT_example.cpp:6
Definition: BFloat16.h:88
static const Key _L_
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
NonlinearFactorGraph graph
static enum @1107 ordering
void insertRoot(const sharedClique &subtree)
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:214
static char left
std::vector< PointedToType< T > > ValuesVector
Values result
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
output to file with graphviz format.
#define EXPECT(condition)
Definition: Test.h:150
Array< int, Dynamic, 1 > v
std::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Array< double, 1, 3 > e(1./3., 0.5, 2.)
bool assert_container_equal(const std::map< V1, V2 > &expected, const std::map< V1, V2 > &actual, double tol=1e-9)
static char right
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
static const Key _B_
traits
Definition: chartTesting.h:28
DiscreteKey E(5, 2)
#define GTSAM_PRINT(x)
Definition: Testable.h:43
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
void getAllCliques(const SymbolicBayesTree::sharedClique &subtree, SymbolicBayesTree::Cliques &cliques)
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
output to file with graphviz format.
Definition: BayesNet-inst.h:84
std::shared_ptr< This > shared_ptr
const Roots & roots() const
Definition: BayesTree.h:152
std::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
std::shared_ptr< This > shared_ptr
TEST(SymbolicBayesTree, clear)
sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
static const Key _A_
#define X
Definition: icosphere.cpp:20
sharedConditional marginalFactor(Key j, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
Point2 t(10, 10)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:43