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

iSAM smoother test

release/4.3a0
Michael Kaess 2009-11-23 01:16:58 +00:00
parent 4951a25453
commit 10a8ac34ec
1 changed files with 27 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
cpp/testBayesTree.cpp
View File

@ -464,7 +464,7 @@ TEST( BayesTree, removeTop2 )
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
SymbolicBayesTree::Cliques expectedOrphans;
expectedOrphans += bayesTree["T"], bayesTree["X"];
// CHECK(assert_equal(expectedOrphans, orphans)); fails !
CHECK(assert_equal(expectedOrphans, orphans));
}
/* ************************************************************************* */
@ -502,6 +502,32 @@ TEST( BayesTree, iSAM )
CHECK(assert_equal(expected,bayesTree));
}
/* ************************************************************************* */
TEST( BayesTree, iSAM_smoother )
{
// Create smoother with 7 nodes
GaussianFactorGraph smoother = createSmoother(7);
// iSAM
GaussianBayesTree bayesTree;
BOOST_FOREACH(boost::shared_ptr<GaussianFactor> factor, (const GaussianFactorGraph)smoother) {
GaussianFactorGraph factorGraph;
factorGraph.push_back(factor);
bayesTree.update(factorGraph);
}
// expected
Ordering ordering;
for (int t = 1; t <= 7; t++)
ordering.push_back(symbol('x', t));
// eliminate using the "natural" ordering
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
// Create the Bayes tree
GaussianBayesTree expectedBayesTree(chordalBayesNet);
CHECK(assert_equal(expectedBayesTree, bayesTree));
}
/* ************************************************************************* */
int main() {
TestResult tr;