testSymbolicISAM.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 
20 
22 
23 using namespace std;
24 using namespace gtsam;
25 
26 /* ************************************************************************* */
27 // Some numbers that should be consistent among all smoother tests
28 
29 //static double sigmax1 = 0.786153, sigmax2 = 0.687131 ,sigmax3 = 0.671512,
30 //sigmax4 = 0.669534, sigmax5 = sigmax3,, sigmax7 = sigmax1, sigmax6 = sigmax2;
31 
32 /* ************************************************************************* */
33 
35 //SymbolicConditional::shared_ptr x3(new SymbolicConditional("x3")),
36 // x2(new SymbolicConditional("x2","x3")),
37 // x1(new SymbolicConditional("x1","x2","x3")),
38 // l1(new SymbolicConditional("l1","x1","x2")),
39 // l2(new SymbolicConditional("l2","x1","x3"));
40 //
42 //SymbolicISAM createSlamSymbolicISAM(){
43 // // Create using insert
44 // SymbolicISAM bayesTree_slam;
45 // bayesTree_slam.insert(x3);
46 // bayesTree_slam.insert(x2);
47 // bayesTree_slam.insert(x1);
48 // bayesTree_slam.insert(l2);
49 // bayesTree_slam.insert(l1);
50 // return bayesTree_slam;
51 //}
52 
53 /* ************************************************************************* */
54 
56 //SymbolicConditional::shared_ptr
57 // B(new SymbolicConditional("B")),
58 // L(new SymbolicConditional("L", "B")),
59 // E(new SymbolicConditional("E", "B", "L")),
60 // S(new SymbolicConditional("S", "L", "B")),
61 // T(new SymbolicConditional("T", "E", "L")),
62 // X(new SymbolicConditional("X", "E"));
63 //
65 //SymbolicISAM createAsiaSymbolicISAM() {
66 // SymbolicISAM bayesTree;
67 // bayesTree.insert(B);
68 // bayesTree.insert(L);
69 // bayesTree.insert(E);
70 // bayesTree.insert(S);
71 // bayesTree.insert(T);
72 // bayesTree.insert(X);
73 // return bayesTree;
74 //}
75 
76 /* ************************************************************************* */
78 {
79  // Now we modify the Bayes tree by inserting a new factor over B and S
80 
81  SymbolicFactorGraph fullGraph;
82  fullGraph.push_back(asiaGraph);
83  fullGraph.emplace_shared<SymbolicFactor>(_B_, _S_);
84 
85  // This ordering is chosen to match the one chosen by COLAMD during the ISAM update
86  Ordering ordering {_X_, _B_, _S_, _E_, _L_, _T_};
88 
89  // Add factor on B and S
90  SymbolicISAM actual = *asiaGraph.eliminateMultifrontal();
91 
92  // Check whether the same
94 }
95 
96 /* ************************************************************************* */
97 //TEST( ISAM, iSAM_slam )
98 //{
99 // // Create using insert
100 // SymbolicISAM bayesTree_slam = createSlamSymbolicISAM();
101 //
102 // //New conditionals for the expected Bayes tree
103 // SymbolicConditional::shared_ptr
104 // l1_(new SymbolicConditional("l1","x1","x2","x3"));
105 //
106 // // Create expected Bayes tree
107 // SymbolicISAM expected_slam;
108 // expected_slam.insert(x3);
109 // expected_slam.insert(x2);
110 // expected_slam.insert(x1);
111 // expected_slam.insert(l1_);
112 // expected_slam.insert(l2);
113 //
114 //
115 // // create new factors to be inserted
116 // SymbolicFactorGraph factorGraph_slam;
117 // factorGraph_slam.push_factor("x3","l1");
118 // factorGraph_slam.push_factor("x3");
119 //
120 // // do incremental inference
121 // bayesTree_slam.update(factorGraph_slam);
122 //
123 // // Check whether the same
124 // CHECK(assert_equal(expected_slam,bayesTree_slam));
125 //}
126 
127 /* ************************************************************************* */
128 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
129 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
main
int main()
Definition: testSymbolicISAM.cpp:128
gtsam::SymbolicBayesTree
Definition: SymbolicBayesTree.h:48
gtsam::EliminateableFactorGraph::eliminateMultifrontal
std::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:89
SymbolicISAM.h
gtsam::SymbolicFactorGraph
Definition: SymbolicFactorGraph.h:61
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
_L_
static const Key _L_
Definition: testSymbolicBayesNet.cpp:31
ordering
static enum @1096 ordering
TestResult
Definition: TestResult.h:26
gtsam::SymbolicISAM
Definition: SymbolicISAM.h:26
gtsam
traits
Definition: SFMdata.h:40
_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
std
Definition: BFloat16.h:88
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
TEST
TEST(SymbolicISAM, iSAM)
Definition: testSymbolicISAM.cpp:77
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
Author(s):
autogenerated on Fri Nov 1 2024 03:41:48