12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

I now use push_back to add factors to the new factor graph, and called getOrdering to compute a colamd order. I fixed the test so it checks the right order. Unfortunately colamd is a worse ordering, and the two cliques from before are merged into one.

release/4.3a0
Frank Dellaert 2009-11-19 06:37:06 +00:00
parent 8d8503bd99
commit cd913566f2
1 changed files with 27 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
cpp/testIncremental.cpp
View File

@ -2,6 +2,8 @@
* @file testIncremental.cpp * @file testIncremental.cpp
* @brief Unit tests for graph-based iSAM * @brief Unit tests for graph-based iSAM
* @author Michael Kaess * @author Michael Kaess
* @author Viorela Ila
* @author Frank Dellaert
*/ */
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
@ -21,15 +23,6 @@ using namespace gtsam;
typedef BayesTree<SymbolicConditional> SymbolicBayesTree; typedef BayesTree<SymbolicConditional> SymbolicBayesTree;
typedef BayesTree<GaussianConditional> GaussianBayesTree; typedef BayesTree<GaussianConditional> GaussianBayesTree;
// Conditionals for ASIA example from the tutorial with A and D evidence
SymbolicConditional::shared_ptr B(new SymbolicConditional("B")), L(
new SymbolicConditional("L", "B")), E(
new SymbolicConditional("E", "B", "L")), S1(new SymbolicConditional("S",
"B", "L")), S2(new SymbolicConditional("S", "L", "B")), T(
new SymbolicConditional("T", "E", "L")), X(
new SymbolicConditional("X", "E"));
/* ************************************************************************* */ /* ************************************************************************* */
SymbolicBayesTree update(const SymbolicBayesTree& initial, SymbolicBayesTree update(const SymbolicBayesTree& initial,
@ -43,21 +36,20 @@ SymbolicBayesTree update(const SymbolicBayesTree& initial,
FactorGraph<SymbolicFactor> ELB_factors(*ELB); FactorGraph<SymbolicFactor> ELB_factors(*ELB);
// add it to the factor graph // add it to the factor graph
factorGraph = combine(factorGraph, ELB_factors); // todo: potentially expensive factorGraph.push_back(ELB_factors);
// get the SLB clique // get the SLB clique
SymbolicBayesTree::sharedClique SLB = initial["S"]; SymbolicBayesTree::sharedClique SLB = initial["S"];
FactorGraph<SymbolicFactor> SLB_factors(*SLB); FactorGraph<SymbolicFactor> SLB_factors(*SLB);
// add it to the factor graph // add it to the factor graph
factorGraph = combine(factorGraph, SLB_factors); factorGraph.push_back(SLB_factors);
// now add the new factor // now add the new factor
factorGraph.push_back(newFactor); factorGraph.push_back(newFactor);
// create an ordering ESLB // create an ordering BELS
Ordering ordering; Ordering ordering = factorGraph.getOrdering();
ordering += "E","S","L","B";
// eliminate into a Bayes net // eliminate into a Bayes net
SymbolicBayesNet bayesNet = eliminate<SymbolicFactor,SymbolicConditional>(factorGraph,ordering); SymbolicBayesNet bayesNet = eliminate<SymbolicFactor,SymbolicConditional>(factorGraph,ordering);
@ -81,21 +73,37 @@ SymbolicBayesTree update(const SymbolicBayesTree& initial,
/* ************************************************************************* */ /* ************************************************************************* */
TEST( BayesTree, iSAM ) TEST( BayesTree, iSAM )
{ {
// Conditionals for ASIA example from the tutorial with A and D evidence
SymbolicConditional::shared_ptr
B(new SymbolicConditional("B")),
L(new SymbolicConditional("L", "B")),
E(new SymbolicConditional("E", "B", "L")),
S(new SymbolicConditional("S", "L", "B")),
T(new SymbolicConditional("T", "E", "L")),
X(new SymbolicConditional("X", "E"));
// Create using insert // Create using insert
SymbolicBayesTree bayesTree; SymbolicBayesTree bayesTree;
bayesTree.insert(B); bayesTree.insert(B);
bayesTree.insert(L); bayesTree.insert(L);
bayesTree.insert(E); bayesTree.insert(E);
bayesTree.insert(S2); bayesTree.insert(S);
bayesTree.insert(T); bayesTree.insert(T);
bayesTree.insert(X); bayesTree.insert(X);
// New conditionals in modified top of the tree
SymbolicConditional::shared_ptr
S_(new SymbolicConditional("S")),
L_(new SymbolicConditional("L", "S")),
E_(new SymbolicConditional("E", "L", "S")),
B_(new SymbolicConditional("B", "E", "L", "S"));
// Create expected Bayes tree // Create expected Bayes tree
SymbolicBayesTree expected; SymbolicBayesTree expected;
expected.insert(B); expected.insert(S_);
expected.insert(L); expected.insert(L_);
expected.insert(S1); expected.insert(E_);
expected.insert(E); expected.insert(B_);
expected.insert(T); expected.insert(T);
expected.insert(X); expected.insert(X);