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

update almost correct, except for order

release/4.3a0
Michael Kaess 2009-11-18 17:40:05 +00:00
parent 23031fbc04
commit d0efe2da5e
1 changed files with 12 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
cpp/testIncremental.cpp
View File

@ -43,15 +43,18 @@ 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
// get the SLB clique // get the SLB clique
SymbolicBayesTree::sharedClique SLB = initial["S"]; SymbolicBayesTree::sharedClique SLB = initial["S"];
FactorGraph<SymbolicFactor> SLB_factors(*SLB);
// add it to the factor graph // add it to the factor graph
factorGraph = combine(factorGraph, SLB_factors);
// create an ordering ESLB // create an ordering ESLB
Ordering ordering; Ordering ordering;
ordering += "S","B"; 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);
@ -60,6 +63,13 @@ SymbolicBayesTree update(const SymbolicBayesTree& initial,
BayesTree<SymbolicConditional> newTree(bayesNet); BayesTree<SymbolicConditional> newTree(bayesNet);
// add orphans to the bottom of the new tree // add orphans to the bottom of the new tree
// get the ophan cliques
SymbolicBayesTree::sharedClique TEL = initial["T"];
SymbolicBayesTree::sharedClique XE = initial["X"];
// get clique from new tree to attach to
SymbolicBayesTree::sharedClique new_ELB = newTree["E"];
new_ELB->children_ += TEL,XE;
return newTree; return newTree;
} }
@ -96,7 +106,7 @@ TEST( BayesTree, iSAM )
SymbolicBayesTree actual = update(bayesTree, newFactor); SymbolicBayesTree actual = update(bayesTree, newFactor);
// Check whether the same // Check whether the same
CHECK(assert_equal(expected,actual)); // CHECK(assert_equal(expected,actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */