testSymbolicBayesNet.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 #include <gtsam/base/Testable.h>
20 #include <gtsam/base/Vector.h>
21 #include <gtsam/base/VectorSpace.h>
22 #include <gtsam/inference/Symbol.h>
24 
26 
27 
28 using namespace std;
29 using namespace gtsam;
30 
31 static const Key _L_ = 0;
32 static const Key _A_ = 1;
33 static const Key _B_ = 2;
34 static const Key _C_ = 3;
35 
36 static SymbolicConditional::shared_ptr
39 
40 /* ************************************************************************* */
42 {
44  f1.push_back(B);
45  f1.push_back(L);
47  f2.push_back(L);
48  f2.push_back(B);
49  CHECK(f1.equals(f1));
50  CHECK(!f1.equals(f2));
51 }
52 
53 /* ************************************************************************* */
55 {
56  SymbolicConditional::shared_ptr
60 
61  // p(A|BC)
62  SymbolicBayesNet p_ABC;
63  p_ABC.push_back(A);
64 
65  // P(BC)=P(B|C)P(C)
66  SymbolicBayesNet p_BC;
67  p_BC.push_back(B);
68  p_BC.push_back(C);
69 
70  // P(ABC) = P(A|BC) P(BC)
71  p_ABC.push_back(p_BC);
72 
74  expected.push_back(A);
75  expected.push_back(B);
76  expected.push_back(C);
77 
78  CHECK(assert_equal(expected,p_ABC));
79 }
80 
81 /* ************************************************************************* */
83  using symbol_shorthand::A;
84  using symbol_shorthand::X;
86  bn.emplace_shared<SymbolicConditional>(X(3), X(2), A(2));
87  bn.emplace_shared<SymbolicConditional>(X(2), X(1), A(1));
89 
90  DotWriter writer;
91  writer.positionHints.emplace('a', 2);
92  writer.positionHints.emplace('x', 1);
93  writer.boxes.emplace(A(1));
94  writer.boxes.emplace(A(2));
95 
96  auto position = writer.variablePos(A(1));
97  CHECK(position);
98  EXPECT(assert_equal(Vector2(1, 2), *position, 1e-5));
99 
100  string actual = bn.dot(DefaultKeyFormatter, writer);
101  bn.saveGraph("bn.dot", DefaultKeyFormatter, writer);
102  EXPECT(actual ==
103  "digraph {\n"
104  " size=\"5,5\";\n"
105  "\n"
106  " var6989586621679009793[label=\"a1\", pos=\"1,2!\", shape=box];\n"
107  " var6989586621679009794[label=\"a2\", pos=\"2,2!\", shape=box];\n"
108  " var8646911284551352321[label=\"x1\", pos=\"1,1!\"];\n"
109  " var8646911284551352322[label=\"x2\", pos=\"2,1!\"];\n"
110  " var8646911284551352323[label=\"x3\", pos=\"3,1!\"];\n"
111  "\n"
112  " var8646911284551352321->var8646911284551352322\n"
113  " var6989586621679009793->var8646911284551352322\n"
114  " var8646911284551352322->var8646911284551352323\n"
115  " var6989586621679009794->var8646911284551352323\n"
116  "}");
117 }
118 
119 /* ************************************************************************* */
120 int main() {
121  TestResult tr;
122  return TestRegistry::runAllTests(tr);
123 }
124 /* ************************************************************************* */
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
#define CHECK(condition)
Definition: Test.h:108
static const Key _C_
GTSAM_EXPORT bool equals(const This &bn, double tol=1e-9) const
Concept check for values that can be used in unit tests.
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
static int runAllTests(TestResult &result)
Matrix expected
Definition: testMatrix.cpp:971
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
Output to graphviz format, stream version.
Definition: BayesNet-inst.h:45
Definition: BFloat16.h:88
static const Key _L_
double f2(const Vector2 &x)
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
#define EXPECT(condition)
Definition: Test.h:150
Array< double, 1, 3 > e(1./3., 0.5, 2.)
TEST(SymbolicBayesNet, equals)
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
DotWriter is a helper class for writing graphviz .dot files.
Definition: DotWriter.h:36
static const Key _B_
traits
Definition: chartTesting.h:28
typedef and functions to augment Eigen&#39;s VectorXd
Eigen::Vector2d Vector2
Definition: Vector.h:42
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
Point2 f1(const Point3 &p, OptionalJacobian< 2, 3 > H)
static SymbolicConditional::shared_ptr L(new SymbolicConditional(_L_, _B_))
static const Key _A_
#define X
Definition: icosphere.cpp:20
Point3 position(const NavState &X, OptionalJacobian< 3, 9 > H)
int main()
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
std::map< char, double > positionHints
Definition: DotWriter.h:55


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