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 
25 
27 
28 using namespace std;
29 using namespace gtsam;
30 
31 /* ************************************************************************* */
33  KeySet expected{0, 1, 2, 3, 4};
34  KeySet actual = simpleTestGraph1.keys();
35  EXPECT(expected == actual);
36 }
37 
38 /* ************************************************************************* */
40  KeySet expected{0, 1, 2, 3, 4, 5};
41  KeySet actual = simpleTestGraph2.keys();
42  EXPECT(expected == actual);
43 }
44 
45 /* ************************************************************************* */
46 TEST(SymbolicFactorGraph, eliminateFullSequential) {
47  // Test with simpleTestGraph1
48  Ordering order{0, 1, 2, 3, 4};
49  SymbolicBayesNet actual1 = *simpleTestGraph1.eliminateSequential(order);
50  EXPECT(assert_equal(simpleTestGraph1BayesNet, actual1));
51 
52  // Test with Asia graph
53  SymbolicBayesNet actual2 = *asiaGraph.eliminateSequential(asiaOrdering);
54  EXPECT(assert_equal(asiaBayesNet, actual2));
55 }
56 
57 /* ************************************************************************* */
58 TEST(SymbolicFactorGraph, eliminatePartialSequential) {
59  // Eliminate 0 and 1
60  const Ordering order{0, 1};
61 
62  const auto expectedBayesNet = SymbolicBayesNet(SymbolicConditional(0, 1, 2))(
63  SymbolicConditional(1, 2, 3, 4));
64 
65  const auto expectedSfg = SymbolicFactorGraph(SymbolicFactor(2, 3))(
66  SymbolicFactor(4, 5))(SymbolicFactor(2, 3, 4));
67 
68  const auto [actualBayesNet, actualSfg] =
69  simpleTestGraph2.eliminatePartialSequential(Ordering{0, 1});
70 
71  EXPECT(assert_equal(expectedSfg, *actualSfg));
72  EXPECT(assert_equal(expectedBayesNet, *actualBayesNet));
73 
74  const auto [actualBayesNet2, actualSfg2] =
75  simpleTestGraph2.eliminatePartialSequential(Ordering{0, 1});
76 
77  EXPECT(assert_equal(expectedSfg, *actualSfg2));
78  EXPECT(assert_equal(expectedBayesNet, *actualBayesNet2));
79 }
80 
81 /* ************************************************************************* */
82 TEST(SymbolicFactorGraph, eliminateFullMultifrontal) {
83  Ordering ordering{0, 1, 2, 3};
84  SymbolicBayesTree actual1 = *simpleChain.eliminateMultifrontal(ordering);
85  EXPECT(assert_equal(simpleChainBayesTree, actual1));
86 
87  SymbolicBayesTree actual2 = *asiaGraph.eliminateMultifrontal(asiaOrdering);
88  EXPECT(assert_equal(asiaBayesTree, actual2));
89 }
90 
91 /* ************************************************************************* */
92 TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) {
93  SymbolicBayesTree expectedBayesTree;
95  std::make_shared<SymbolicConditional>(
96  SymbolicConditional::FromKeys(KeyVector{4, 5, 1}, 2));
97  expectedBayesTree.insertRoot(
98  std::make_shared<SymbolicBayesTreeClique>(root));
99 
100  const auto expectedFactorGraph =
102  SymbolicFactor(1, 3))(SymbolicFactor(2, 3))(SymbolicFactor(1));
103 
104  const auto [actualBayesTree, actualFactorGraph] =
105  simpleTestGraph2.eliminatePartialMultifrontal(Ordering{4, 5});
106 
107  EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph));
108  EXPECT(assert_equal(expectedBayesTree, *actualBayesTree));
109 
110  SymbolicBayesTree expectedBayesTree2;
112  std::make_shared<SymbolicBayesTreeClique>(
113  std::make_shared<SymbolicConditional>(4, 1));
114  root2->children.push_back(std::make_shared<SymbolicBayesTreeClique>(
115  std::make_shared<SymbolicConditional>(5, 4)));
116  expectedBayesTree2.insertRoot(root2);
117 
118  const auto [actualBayesTree2, actualFactorGraph2] =
119  simpleTestGraph2.eliminatePartialMultifrontal(KeyVector{4, 5});
120 
121  EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2));
122  EXPECT(assert_equal(expectedBayesTree2, *actualBayesTree2));
123 }
124 
125 /* ************************************************************************* */
126 TEST(SymbolicFactorGraph, MarginalMultifrontalBayesNetOrdering) {
127  SymbolicBayesNet actual =
128  *simpleTestGraph2.marginalMultifrontalBayesNet(Ordering{0, 1, 2, 3});
129  auto expectedBayesNet = SymbolicBayesNet({0, 1, 2})({1, 2, 3})({2, 3})({3});
130  EXPECT(assert_equal(expectedBayesNet, actual));
131 }
132 
133 TEST(SymbolicFactorGraph, MarginalMultifrontalBayesNetKeyVector) {
134  SymbolicBayesNet actual =
135  *simpleTestGraph2.marginalMultifrontalBayesNet(KeyVector{0, 1, 2, 3});
136  // Since we use KeyVector, the variable ordering will be determined by COLAMD:
137  auto expectedBayesNet = SymbolicBayesNet({0, 1, 2})({2, 1, 3})({1, 3})({3});
138  EXPECT(assert_equal(expectedBayesNet, actual));
139 }
140 
141 TEST(SymbolicFactorGraph, MarginalMultifrontalBayesNetOrderingPlus) {
142  const Ordering orderedVariables{0, 3},
143  marginalizedVariableOrdering{1, 2, 4, 5};
144  SymbolicBayesNet actual = *simpleTestGraph2.marginalMultifrontalBayesNet(
145  orderedVariables, marginalizedVariableOrdering);
146  auto expectedBayesNet = SymbolicBayesNet(SymbolicConditional{0, 3})({3});
147  EXPECT(assert_equal(expectedBayesNet, actual));
148 }
149 
150 TEST(SymbolicFactorGraph, MarginalMultifrontalBayesNetKeyVectorPlus) {
151  const KeyVector variables{0, 1, 3};
152  const Ordering marginalizedVariableOrdering{2, 4, 5};
153  SymbolicBayesNet actual = *simpleTestGraph2.marginalMultifrontalBayesNet(
154  variables, marginalizedVariableOrdering);
155  // Since we use KeyVector, the variable ordering will be determined by COLAMD:
156  auto expectedBayesNet = SymbolicBayesNet({0, 1, 3})({3, 1})({1});
157  EXPECT(assert_equal(expectedBayesNet, actual));
158 }
159 
160 /* ************************************************************************* */
161 TEST(SymbolicFactorGraph, MarginalMultifrontalBayesTreeOrdering) {
162  auto expectedBayesTree =
163  *simpleTestGraph2.eliminatePartialMultifrontal(Ordering{4, 5})
164  .second->eliminateMultifrontal(Ordering{0, 1, 2, 3});
165 
166  SymbolicBayesTree actual =
167  *simpleTestGraph2.marginalMultifrontalBayesTree(Ordering{0, 1, 2, 3});
168  EXPECT(assert_equal(expectedBayesTree, actual));
169 }
170 
171 TEST(SymbolicFactorGraph, MarginalMultifrontalBayesTreeKeyVector) {
172  // Same: KeyVector variant will use COLAMD:
173  auto expectedBayesTree =
174  *simpleTestGraph2.eliminatePartialMultifrontal(Ordering{4, 5})
175  .second->eliminateMultifrontal(Ordering::OrderingType::COLAMD);
176 
177  SymbolicBayesTree actual =
178  *simpleTestGraph2.marginalMultifrontalBayesTree(KeyVector{0, 1, 2, 3});
179  EXPECT(assert_equal(expectedBayesTree, actual));
180 }
181 
182 TEST(SymbolicFactorGraph, MarginalMultifrontalBayesTreeOrderingPlus) {
183  const Ordering orderedVariables{0, 3},
184  marginalizedVariableOrdering{1, 2, 4, 5};
185  auto expectedBayesTree =
186  *simpleTestGraph2
187  .eliminatePartialMultifrontal(marginalizedVariableOrdering)
188  .second->eliminateMultifrontal(orderedVariables);
189 
190  SymbolicBayesTree actual = *simpleTestGraph2.marginalMultifrontalBayesTree(
191  orderedVariables, marginalizedVariableOrdering);
192  EXPECT(assert_equal(expectedBayesTree, actual));
193 }
194 
195 TEST(SymbolicFactorGraph, MarginalMultifrontalBayesTreeKeyVectorPlus) {
196  // Again: KeyVector variant will use COLAMD:
197  const Ordering marginalizedVariableOrdering{2, 4, 5};
198  auto expectedBayesTree =
199  *simpleTestGraph2
200  .eliminatePartialMultifrontal(marginalizedVariableOrdering)
201  .second->eliminateMultifrontal(Ordering::OrderingType::COLAMD);
202 
203  const KeyVector variables{0, 1, 3};
204  SymbolicBayesTree actual = *simpleTestGraph2.marginalMultifrontalBayesTree(
205  variables, marginalizedVariableOrdering);
206  EXPECT(assert_equal(expectedBayesTree, actual));
207 }
208 
209 /* ************************************************************************* */
210 TEST(SymbolicFactorGraph, eliminate_disconnected_graph) {
212  fg.push_factor(0, 1);
213  fg.push_factor(0, 2);
214  fg.push_factor(1, 2);
215  fg.push_factor(3, 4);
216 
217  // create expected Chordal bayes Net
219  expected.emplace_shared<SymbolicConditional>(0, 1, 2);
220  expected.emplace_shared<SymbolicConditional>(1, 2);
221  expected.emplace_shared<SymbolicConditional>(2);
222  expected.emplace_shared<SymbolicConditional>(3, 4);
223  expected.emplace_shared<SymbolicConditional>(4);
224 
225  const Ordering order{0, 1, 2, 3, 4};
226  SymbolicBayesNet actual = *fg.eliminateSequential(order);
227 
228  EXPECT(assert_equal(expected, actual));
229 }
230 
231 /* ************************************************************************* */
233  // Create factor graph
235  fg.push_factor(0, 1);
236  fg.push_factor(0, 2);
237  fg.push_factor(1, 4);
238  fg.push_factor(2, 4);
239  fg.push_factor(3, 4);
240 
241  // eliminate
242  Ordering ord{3, 4, 2, 1, 0};
243  auto actual = fg.eliminateSequential(ord);
245  expected.emplace_shared<SymbolicConditional>(3, 4);
246  expected.emplace_shared<SymbolicConditional>(4, 1, 2);
247  expected.emplace_shared<SymbolicConditional>(2, 0, 1);
248  expected.emplace_shared<SymbolicConditional>(1, 0);
249  expected.emplace_shared<SymbolicConditional>(0);
250  EXPECT(assert_equal(expected, *actual));
251 
252  {
253  // jointBayesNet
254  Ordering ord{0, 4, 3};
255  auto actual = fg.eliminatePartialSequential(ord);
256  SymbolicBayesNet expectedBN;
257  expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2);
258  expectedBN.emplace_shared<SymbolicConditional>(4, 1, 2, 3);
259  expectedBN.emplace_shared<SymbolicConditional>(3, 1, 2);
260  EXPECT(assert_equal(expectedBN, *(actual.first)));
261  }
262 
263  {
264  // jointBayesNet
265  Ordering ord{0, 2, 3};
266  auto actual = fg.eliminatePartialSequential(ord);
267  SymbolicBayesNet expectedBN;
268  expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2);
269  expectedBN.emplace_shared<SymbolicConditional>(2, 1, 4);
270  expectedBN.emplace_shared<SymbolicConditional>(3, 4);
271  EXPECT(assert_equal(expectedBN, *(actual.first)));
272  }
273 
274  {
275  // conditionalBayesNet
276  Ordering ord{0, 2};
277  auto actual = fg.eliminatePartialSequential(ord);
278  SymbolicBayesNet expectedBN;
279  expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2);
280  expectedBN.emplace_shared<SymbolicConditional>(2, 1, 4);
281  EXPECT(assert_equal(expectedBN, *(actual.first)));
282  }
283 }
284 
285 /* ************************************************************************* */
286 TEST(SymbolicFactorGraph, constructFromBayesNet) {
287  // create expected factor graph
289  expected.push_factor(0, 1, 2);
290  expected.push_factor(1, 2);
291  expected.push_factor(1);
292 
293  // create Bayes Net
298 
299  // create actual factor graph from a Bayes Net
301 
302  CHECK(assert_equal(expected, actual));
303 }
304 
305 /* ************************************************************************* */
306 TEST(SymbolicFactorGraph, constructFromBayesTree) {
307  // create expected factor graph
309  expected.push_factor(_E_, _L_, _B_);
310  expected.push_factor(_S_, _B_, _L_);
311  expected.push_factor(_T_, _E_, _L_);
312  expected.push_factor(_X_, _E_);
313 
314  // create actual factor graph
315  SymbolicFactorGraph actual(asiaBayesTree);
316 
317  CHECK(assert_equal(expected, actual));
318 }
319 
320 /* ************************************************************************* */
322  // Create two factor graphs and expected combined graph
323  SymbolicFactorGraph fg1, fg2, expected;
324 
325  fg1.push_factor(1);
326  fg1.push_factor(0, 1);
327 
328  fg2.push_factor(1, 2);
329  fg2.push_factor(0, 2);
330 
331  expected.push_factor(1);
332  expected.push_factor(0, 1);
333  expected.push_factor(1, 2);
334  expected.push_factor(0, 2);
335 
336  // combine
337  SymbolicFactorGraph actual;
338  actual.push_back(fg1);
339  actual.push_back(fg2);
340  CHECK(assert_equal(expected, actual));
341 
342  // combine in second way
343  SymbolicFactorGraph actual2 = fg1;
344  actual2.push_back(fg2);
345  CHECK(assert_equal(expected, actual2));
346 }
347 
348 /* ************************************************************************* */
349 TEST(SymbolicFactorGraph, add_factors) {
351  fg1.push_factor(10);
352  fg1.push_back(SymbolicFactor::shared_ptr()); // empty slot!
353  fg1.push_factor(11);
354 
356  fg2.push_factor(1);
357  fg2.push_factor(2);
358 
360  expected.push_factor(10);
361  expected.push_factor(1);
362  expected.push_factor(11);
363  expected.push_factor(2);
364  const FactorIndices expectedIndices{1, 3};
365  const FactorIndices actualIndices = fg1.add_factors(fg2, true);
366 
368  EXPECT(assert_container_equality(expectedIndices, actualIndices));
369 
370  expected.push_factor(1);
371  expected.push_factor(2);
372  const FactorIndices expectedIndices2{4, 5};
373  const FactorIndices actualIndices2 = fg1.add_factors(fg2, false);
374 
376  EXPECT(assert_container_equality(expectedIndices2, actualIndices2));
377 }
378 
379 /* ************************************************************************* */
380 int main() {
381  TestResult tr;
382  return TestRegistry::runAllTests(tr);
383 }
384 /* ************************************************************************* */
gtsam::SymbolicFactorGraph::push_factor
void push_factor(Key key)
Definition: SymbolicFactorGraph.cpp:42
gtsam::EliminateableFactorGraph::eliminatePartialMultifrontal
std::pair< std::shared_ptr< BayesTreeType >, std::shared_ptr< FactorGraphType > > eliminatePartialMultifrontal(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:190
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::FactorGraph::add_factors
FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false)
Definition: FactorGraph-inst.h:109
gtsam::SymbolicFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: SymbolicFactor.h:47
gtsam::EliminateableFactorGraph::eliminateSequential
std::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:29
gtsam::BayesTree::insertRoot
void insertRoot(const sharedClique &subtree)
Definition: BayesTree-inst.h:299
gtsam::SymbolicConditional::shared_ptr
std::shared_ptr< This > shared_ptr
Typedef to the conditional base class.
Definition: SymbolicConditional.h:44
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
SymbolicBayesTree.h
gtsam::FastSet< Key >
gtsam::SymbolicBayesTree
Definition: SymbolicBayesTree.h:48
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
SymbolicConditional.h
TestableAssertions.h
Provides additional testing facilities for common data structures.
different_sigmas::bayesNet
const HybridBayesNet bayesNet
Definition: testHybridBayesNet.cpp:242
gtsam::EliminateableFactorGraph::eliminatePartialSequential
std::pair< std::shared_ptr< BayesNetType >, std::shared_ptr< FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:151
gtsam::SymbolicFactorGraph
Definition: SymbolicFactorGraph.h:61
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
_L_
static const Key _L_
Definition: testSymbolicBayesNet.cpp:31
SymbolicFactorGraph.h
ordering
static enum @1096 ordering
TestResult
Definition: TestResult.h:26
gtsam::SymbolicConditional
Definition: SymbolicConditional.h:36
gtsam
traits
Definition: SFMdata.h:40
gtsam::SymbolicBayesTreeClique::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: SymbolicBayesTree.h:39
_B_
static const Key _B_
Definition: testSymbolicBayesNet.cpp:33
symbolicExampleGraphs.h
gtsam::SymbolicFactor
Definition: SymbolicFactor.h:38
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
CHECK
#define CHECK(condition)
Definition: Test.h:108
std
Definition: BFloat16.h:88
main
int main()
Definition: testSymbolicFactorGraph.cpp:380
gtsam::assert_container_equality
bool assert_container_equality(const std::map< size_t, V2 > &expected, const std::map< size_t, V2 > &actual)
Definition: TestableAssertions.h:237
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
SymbolicBayesNet.h
TEST
TEST(SymbolicFactorGraph, keys1)
Definition: testSymbolicFactorGraph.cpp:32
marginals
Marginals marginals(graph, result)
gtsam::SymbolicBayesNet
Definition: SymbolicBayesNet.h:32
gtsam::Ordering
Definition: inference/Ordering.h:33
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::HybridBayesNet::emplace_shared
void emplace_shared(Args &&...args)
Definition: HybridBayesNet.h:116
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:37


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:08:45