testDiscreteFactorGraph.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 testDiscreteFactorGraph.cpp
14  * @date Feb 14, 2011
15  * @author Duy-Nguyen Ta
16  */
17 
18 #include <CppUnitLite/TestHarness.h>
19 #include <gtsam/base/TestableAssertions.h>
20 #include <gtsam/discrete/DiscreteBayesTree.h>
21 #include <gtsam/discrete/DiscreteEliminationTree.h>
22 #include <gtsam/discrete/DiscreteFactor.h>
23 #include <gtsam/discrete/DiscreteFactorGraph.h>
24 #include <gtsam/inference/BayesNet.h>
25 #include <gtsam/inference/Symbol.h>
26 
27 using namespace std;
28 using namespace gtsam;
29 
30 using symbol_shorthand::M;
31 
32 /* ************************************************************************* */
33 TEST_UNSAFE(DiscreteFactorGraph, debugScheduler) {
34  DiscreteKey PC(0, 4), ME(1, 4), AI(2, 4), A(3, 3);
35 
36  DiscreteFactorGraph graph;
37  graph.add(AI, "1 0 0 1");
38  graph.add(AI, "1 1 1 0");
39  graph.add(A & AI, "1 1 1 0 1 1 1 1 0 1 1 1");
40  graph.add(ME, "0 1 0 0");
41  graph.add(ME, "1 1 1 0");
42  graph.add(A & ME, "1 1 1 0 1 1 1 1 0 1 1 1");
43  graph.add(PC, "1 0 1 0");
44  graph.add(PC, "1 1 1 0");
45  graph.add(A & PC, "1 1 1 0 1 1 1 1 0 1 1 1");
46  graph.add(ME & AI, "0 1 1 1 1 0 1 1 1 1 0 1 1 1 1 0");
47  graph.add(PC & ME, "0 1 1 1 1 0 1 1 1 1 0 1 1 1 1 0");
48  graph.add(PC & AI, "0 1 1 1 1 0 1 1 1 1 0 1 1 1 1 0");
49 
50  // Check MPE.
51  auto actualMPE = graph.optimize();
52  EXPECT(assert_equal({{0, 2}, {1, 1}, {2, 0}, {3, 0}}, actualMPE));
53 }
54 
55 /* ************************************************************************* */
57 TEST_UNSAFE( DiscreteFactorGraph, DiscreteFactorGraphEvaluationTest) {
58 
59  // Three keys P1 and P2
60  DiscreteKey P1(0,2), P2(1,2), P3(2,3);
61 
62  // Create the DiscreteFactorGraph
63  DiscreteFactorGraph graph;
64  graph.add(P1, "0.9 0.3");
65  graph.add(P2, "0.9 0.6");
66  graph.add(P1 & P2, "4 1 10 4");
67 
68  // Instantiate DiscreteValues
69  DiscreteValues values;
70  values[0] = 1;
71  values[1] = 1;
72 
73  // Check if graph evaluation works ( 0.3*0.6*4 )
74  EXPECT_DOUBLES_EQUAL( .72, graph(values), 1e-9);
75 
76  // Creating a new test with third node and adding unary and ternary factors on it
77  graph.add(P3, "0.9 0.2 0.5");
78  graph.add(P1 & P2 & P3, "1 2 3 4 5 6 7 8 9 10 11 12");
79 
80  // Below values lead to selecting the 8th index in the ternary factor table
81  values[0] = 1;
82  values[1] = 0;
83  values[2] = 1;
84 
85  // Check if graph evaluation works (0.3*0.9*1*0.2*8)
86  EXPECT_DOUBLES_EQUAL( 4.32, graph(values), 1e-9);
87 
88  // Below values lead to selecting the 3rd index in the ternary factor table
89  values[0] = 0;
90  values[1] = 1;
91  values[2] = 0;
92 
93  // Check if graph evaluation works (0.9*0.6*1*0.9*4)
94  EXPECT_DOUBLES_EQUAL( 1.944, graph(values), 1e-9);
95 
96  // Check if graph product works
97  DecisionTreeFactor product = graph.product();
98  EXPECT_DOUBLES_EQUAL( 1.944, product(values), 1e-9);
99 }
100 
101 /* ************************************************************************* */
102 TEST(DiscreteFactorGraph, test) {
103  // Declare keys and ordering
104  DiscreteKey C(0, 2), B(1, 2), A(2, 2);
105 
106  // A simple factor graph (A)-fAC-(C)-fBC-(B)
107  // with smoothness priors
108  DiscreteFactorGraph graph;
109  graph.add(A & C, "3 1 1 3");
110  graph.add(C & B, "3 1 1 3");
111 
112  // Test EliminateDiscrete
113  const Ordering frontalKeys{0};
114  const auto [conditional, newFactorPtr] = EliminateDiscrete(graph, frontalKeys);
115 
116  DecisionTreeFactor newFactor =
117  *std::dynamic_pointer_cast<DecisionTreeFactor>(newFactorPtr);
118 
119  // Normalize newFactor by max for comparison with expected
120  auto normalization = newFactor.max(newFactor.size());
121 
122  newFactor = newFactor / *normalization;
123 
124  // Check Conditional
125  CHECK(conditional);
126  Signature signature((C | B, A) = "9/1 1/1 1/1 1/9");
127  DiscreteConditional expectedConditional(signature);
128  EXPECT(assert_equal(expectedConditional, *conditional));
129 
130  // Check Factor
131  CHECK(&newFactor);
132  DecisionTreeFactor expectedFactor(B & A, "10 6 6 10");
133  // Normalize by max.
134  normalization = expectedFactor.max(expectedFactor.size());
135  // Ensure normalization is correct.
136  expectedFactor = expectedFactor / *normalization;
137  EXPECT(assert_equal(expectedFactor, newFactor));
138 
139  // Test using elimination tree
140  const Ordering ordering{0, 1, 2};
141  DiscreteEliminationTree etree(graph, ordering);
142  const auto [actual, remainingGraph] = etree.eliminate(&EliminateDiscrete);
143 
144  // Check Bayes net
145  DiscreteBayesNet expectedBayesNet;
146  expectedBayesNet.add(signature);
147  expectedBayesNet.add(B | A = "5/3 3/5");
148  expectedBayesNet.add(A % "1/1");
149  EXPECT(assert_equal(expectedBayesNet, *actual));
150 
151  // Test eliminateSequential
152  DiscreteBayesNet::shared_ptr actual2 = graph.eliminateSequential(ordering);
153  EXPECT(assert_equal(expectedBayesNet, *actual2));
154 
155  // Test mpe
156  DiscreteValues mpe { {0, 0}, {1, 0}, {2, 0}};
157  auto actualMPE = graph.optimize();
158  EXPECT(assert_equal(mpe, actualMPE));
159  EXPECT_DOUBLES_EQUAL(9, graph(mpe), 1e-5); // regression
160 
161  // Test sumProduct alias with all orderings:
162  auto mpeProbability = expectedBayesNet(mpe);
163  EXPECT_DOUBLES_EQUAL(0.28125, mpeProbability, 1e-5); // regression
164 
165  // Using custom ordering
166  DiscreteBayesNet bayesNet = graph.sumProduct(ordering);
167  EXPECT_DOUBLES_EQUAL(mpeProbability, bayesNet(mpe), 1e-5);
168 
169  for (Ordering::OrderingType orderingType :
170  {Ordering::COLAMD, Ordering::METIS, Ordering::NATURAL,
171  Ordering::CUSTOM}) {
172  auto bayesNet = graph.sumProduct(orderingType);
173  EXPECT_DOUBLES_EQUAL(mpeProbability, bayesNet(mpe), 1e-5);
174  }
175 }
176 
177 /* ************************************************************************* */
178 TEST_UNSAFE(DiscreteFactorGraph, testMaxProduct) {
179  // Declare a bunch of keys
180  DiscreteKey C(0, 2), A(1, 2), B(2, 2);
181 
182  // Create Factor graph
183  DiscreteFactorGraph graph;
184  graph.add(C & A, "0.2 0.8 0.3 0.7");
185  graph.add(C & B, "0.1 0.9 0.4 0.6");
186 
187  // Created expected MPE
188  DiscreteValues mpe{{0, 0}, {1, 1}, {2, 1}};
189 
190  // Do max-product with different orderings
191  for (Ordering::OrderingType orderingType :
192  {Ordering::COLAMD, Ordering::METIS, Ordering::NATURAL,
193  Ordering::CUSTOM}) {
194  DiscreteLookupDAG dag = graph.maxProduct(orderingType);
195  auto actualMPE = dag.argmax();
196  EXPECT(assert_equal(mpe, actualMPE));
197  auto actualMPE2 = graph.optimize(); // all in one
198  EXPECT(assert_equal(mpe, actualMPE2));
199  }
200 }
201 
202 /* ************************************************************************* */
203 TEST(DiscreteFactorGraph, marginalIsNotMPE) {
204  // Declare 2 keys
205  DiscreteKey A(0, 2), B(1, 2);
206 
207  // Create Bayes net such that marginal on A is bigger for 0 than 1, but the
208  // MPE does not have A=0.
209  DiscreteBayesNet bayesNet;
210  bayesNet.add(B | A = "1/1 1/2");
211  bayesNet.add(A % "10/9");
212 
213  // The expected MPE is A=1, B=1
214  DiscreteValues mpe { {0, 1}, {1, 1} };
215 
216  // Which we verify using max-product:
217  DiscreteFactorGraph graph(bayesNet);
218  auto actualMPE = graph.optimize();
219  EXPECT(assert_equal(mpe, actualMPE));
220  EXPECT_DOUBLES_EQUAL(0.315789, graph(mpe), 1e-5); // regression
221 }
222 
223 /* ************************************************************************* */
224 TEST(DiscreteFactorGraph, testMPE_Darwiche09book_p244) {
225  // The factor graph in Darwiche09book, page 244
226  DiscreteKey A(4, 2), C(3, 2), S(2, 2), T1(0, 2), T2(1, 2);
227 
228  // Create Factor graph
229  DiscreteFactorGraph graph;
230  graph.add(S, "0.55 0.45");
231  graph.add(S & C, "0.05 0.95 0.01 0.99");
232  graph.add(C & T1, "0.80 0.20 0.20 0.80");
233  graph.add(S & C & T2, "0.80 0.20 0.20 0.80 0.95 0.05 0.05 0.95");
234  graph.add(T1 & T2 & A, "1 0 0 1 0 1 1 0");
235  graph.add(A, "1 0"); // evidence, A = yes (first choice in Darwiche)
236 
237  DiscreteValues mpe { {0, 1}, {1, 1}, {2, 1}, {3, 1}, {4, 0}};
238  EXPECT_DOUBLES_EQUAL(0.33858, graph(mpe), 1e-5); // regression
239  // You can check visually by printing product:
240  // graph.product().print("Darwiche-product");
241 
242  // Check MPE.
243  auto actualMPE = graph.optimize();
244  EXPECT(assert_equal(mpe, actualMPE));
245 
246  // Check Bayes Net
247  const Ordering ordering{0, 1, 2, 3, 4};
248  auto chordal = graph.eliminateSequential(ordering);
249  EXPECT_LONGS_EQUAL(5, chordal->size());
250 
251  // Let us create the Bayes tree here, just for fun, because we don't use it
252  DiscreteBayesTree::shared_ptr bayesTree =
253  graph.eliminateMultifrontal(ordering);
254  // bayesTree->print("Bayes Tree");
255  EXPECT_LONGS_EQUAL(2, bayesTree->size());
256 }
257 
258 /* ************************************************************************* */
259 TEST(DiscreteFactorGraph, Dot) {
260  // Create Factor graph
261  DiscreteFactorGraph graph;
262  DiscreteKey C(0, 2), A(1, 2), B(2, 2);
263  graph.add(C & A, "0.2 0.8 0.3 0.7");
264  graph.add(C & B, "0.1 0.9 0.4 0.6");
265 
266  string actual = graph.dot();
267  string expected =
268  "graph {\n"
269  " size=\"5,5\";\n"
270  "\n"
271  " var0[label=\"0\"];\n"
272  " var1[label=\"1\"];\n"
273  " var2[label=\"2\"];\n"
274  "\n"
275  " factor0[label=\"\", shape=point];\n"
276  " var0--factor0;\n"
277  " var1--factor0;\n"
278  " factor1[label=\"\", shape=point];\n"
279  " var0--factor1;\n"
280  " var2--factor1;\n"
281  "}\n";
282  EXPECT(actual == expected);
283 }
284 
285 /* ************************************************************************* */
286 TEST(DiscreteFactorGraph, DotWithNames) {
287  // Create Factor graph
288  DiscreteFactorGraph graph;
289  DiscreteKey C(0, 2), A(1, 2), B(2, 2);
290  graph.add(C & A, "0.2 0.8 0.3 0.7");
291  graph.add(C & B, "0.1 0.9 0.4 0.6");
292 
293  vector<string> names{"C", "A", "B"};
294  auto formatter = [names](Key key) { return names[key]; };
295  string actual = graph.dot(formatter);
296  string expected =
297  "graph {\n"
298  " size=\"5,5\";\n"
299  "\n"
300  " var0[label=\"C\"];\n"
301  " var1[label=\"A\"];\n"
302  " var2[label=\"B\"];\n"
303  "\n"
304  " factor0[label=\"\", shape=point];\n"
305  " var0--factor0;\n"
306  " var1--factor0;\n"
307  " factor1[label=\"\", shape=point];\n"
308  " var0--factor1;\n"
309  " var2--factor1;\n"
310  "}\n";
311  EXPECT(actual == expected);
312 }
313 
314 /* ************************************************************************* */
315 // Check markdown representation looks as expected.
316 TEST(DiscreteFactorGraph, markdown) {
317  // Create Factor graph
318  DiscreteFactorGraph graph;
319  DiscreteKey C(0, 2), A(1, 2), B(2, 2);
320  graph.add(C & A, "0.2 0.8 0.3 0.7");
321  graph.add(C & B, "0.1 0.9 0.4 0.6");
322 
323  string expected =
324  "`DiscreteFactorGraph` of size 2\n"
325  "\n"
326  "factor 0:\n"
327  "|C|A|value|\n"
328  "|:-:|:-:|:-:|\n"
329  "|0|0|0.2|\n"
330  "|0|1|0.8|\n"
331  "|1|0|0.3|\n"
332  "|1|1|0.7|\n"
333  "\n"
334  "factor 1:\n"
335  "|C|B|value|\n"
336  "|:-:|:-:|:-:|\n"
337  "|0|0|0.1|\n"
338  "|0|1|0.9|\n"
339  "|1|0|0.4|\n"
340  "|1|1|0.6|\n\n";
341  vector<string> names{"C", "A", "B"};
342  auto formatter = [names](Key key) { return names[key]; };
343  string actual = graph.markdown(formatter);
344  EXPECT(actual == expected);
345 
346  // Make sure values are correctly displayed.
347  DiscreteValues values;
348  values[0] = 1;
349  values[1] = 0;
350  EXPECT_DOUBLES_EQUAL(0.3, graph[0]->operator()(values), 1e-9);
351 }
352 
353 /* ************************************************************************* */
354 int main() {
355 TestResult tr;
356 return TestRegistry::runAllTests(tr);
357 }
358 /* ************************************************************************* */
359 
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::EliminateDiscrete
std::pair< DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr > EliminateDiscrete(const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
Main elimination function for DiscreteFactorGraph.
Definition: DiscreteFactorGraph.cpp:219
gtsam::markdown
string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of markdown.
Definition: DiscreteValues.cpp:130
gtsam::DecisionTreeFactor
Definition: DecisionTreeFactor.h:45
B
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
gtsam::DiscreteFactorGraph
Definition: DiscreteFactorGraph.h:99
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
gtsam::DiscreteBayesTree::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: DiscreteBayesTree.h:80
DiscreteFactorGraph.h
formatter
const KeyFormatter & formatter
Definition: treeTraversal-inst.h:204
gtsam::DiscreteLookupDAG
Definition: DiscreteLookupDAG.h:77
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
P3
static double P3[]
Definition: jv.c:563
gtsam::NonlinearFactorGraph::dot
void dot(std::ostream &os, const Values &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const GraphvizFormatting &writer=GraphvizFormatting()) const
Output to graphviz format, stream version, with Values/extra options.
Definition: NonlinearFactorGraph.cpp:102
TestableAssertions.h
Provides additional testing facilities for common data structures.
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
test
Definition: test.py:1
different_sigmas::bayesNet
const HybridBayesNet bayesNet
Definition: testHybridBayesNet.cpp:242
gtsam::DiscreteBayesNet
Definition: DiscreteBayesNet.h:38
main
int main()
Definition: testDiscreteFactorGraph.cpp:354
gtsam::DiscreteEliminationTree
Elimination tree for discrete factors.
Definition: DiscreteEliminationTree.h:31
P1
static double P1[]
Definition: jv.c:552
DiscreteFactor.h
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::EliminationTree::eliminate
std::pair< std::shared_ptr< BayesNetType >, std::shared_ptr< FactorGraphType > > eliminate(Eliminate function) const
Definition: EliminationTree-inst.h:225
Symbol.h
T2
static const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
DiscreteEliminationTree.h
BayesNet.h
Bayes network.
DiscreteBayesTree.h
Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree.
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
ordering
static enum @1096 ordering
TestResult
Definition: TestResult.h:26
key
const gtsam::Symbol key('X', 0)
gtsam::Ordering::OrderingType
OrderingType
Type of ordering to use.
Definition: inference/Ordering.h:40
process_shonan_timing_results.names
dictionary names
Definition: process_shonan_timing_results.py:175
TEST
TEST(DiscreteFactorGraph, test)
Definition: testDiscreteFactorGraph.cpp:102
gtsam::DiscreteConditional
Definition: DiscreteConditional.h:37
TEST_UNSAFE
TEST_UNSAFE(DiscreteFactorGraph, debugScheduler)
Definition: testDiscreteFactorGraph.cpp:33
gtsam::DiscreteBayesNet::add
void add(const DiscreteKey &key, const std::string &spec)
Definition: DiscreteBayesNet.h:85
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
gtsam
traits
Definition: SFMdata.h:40
gtsam::DiscreteValues
Definition: DiscreteValues.h:34
CHECK
#define CHECK(condition)
Definition: Test.h:108
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Definition: DiscreteKey.h:38
std
Definition: BFloat16.h:88
gtsam::DecisionTreeFactor::max
shared_ptr max(size_t nrFrontals) const
Create new factor by maximizing over all values with the same separator.
Definition: DecisionTreeFactor.h:176
gtsam::DiscreteBayesNet::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: DiscreteBayesNet.h:43
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
product
void product(const MatrixType &m)
Definition: product.h:20
gtsam::DiscreteLookupDAG::argmax
DiscreteValues argmax(DiscreteValues given=DiscreteValues()) const
argmax by back-substitution, optionally given certain variables.
Definition: DiscreteLookupDAG.cpp:121
T1
static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1)
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:171
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Factor::size
size_t size() const
Definition: Factor.h:160
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::Ordering
Definition: inference/Ordering.h:33
gtsam::Signature
Definition: Signature.h:54
S
DiscreteKey S(1, 2)
P2
static double P2[]
Definition: jv.c:557
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51

gtsam
Author(s):
autogenerated on Wed Jan 1 2025 04:06:10