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

optimize works for GaussianBayessTree

release/4.3a0
Michael Kaess 2009-12-10 17:00:11 +00:00
parent edf8bfa0fa
commit 2df0e49e9b
2 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cpp/GaussianBayesTree.cpp
View File

@ -22,12 +22,14 @@ namespace gtsam {
void optimize(const GaussianBayesTree::sharedClique& clique, VectorConfig& result) { void optimize(const GaussianBayesTree::sharedClique& clique, VectorConfig& result) {
// parents are assumed to already be solved and available in result // parents are assumed to already be solved and available in result
GaussianBayesTree::Clique::const_reverse_iterator it; GaussianBayesTree::Clique::const_reverse_iterator it;
for(it = clique->rend(); it!=clique->rbegin(); it++) { for (it = clique->rbegin(); it!=clique->rend(); it++) {
GaussianConditional::shared_ptr cg = *it; GaussianConditional::shared_ptr cg = *it;
Vector x = cg->solve(result); // Solve for that variable Vector x = cg->solve(result); // Solve for that variable
result.insert(cg->key(), x); // store result in partial solution result.insert(cg->key(), x); // store result in partial solution
} }
BOOST_FOREACH(GaussianBayesTree::sharedClique child, clique->children_) { BOOST_FOREACH(GaussianBayesTree::sharedClique child, clique->children_) {
// list<GaussianBayesTree::Clique::shared_ptr>::const_iterator child;
// for (child = clique->children_.begin(); child != clique->children_.end(); child++) {
optimize(child, result); optimize(child, result);
} }
} }
@ -37,6 +39,7 @@ VectorConfig optimize(const GaussianBayesTree& bayesTree) {
VectorConfig result; VectorConfig result;
// starting from the root, call optimize on each conditional // starting from the root, call optimize on each conditional
optimize(bayesTree.root(), result); optimize(bayesTree.root(), result);
return result;
} }
} /// namespace gtsam } /// namespace gtsam

9
cpp/testGaussianBayesTree.cpp
View File

@ -266,9 +266,12 @@ TEST( BayesTree, iSAM_smoother )
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
// obtain solution // obtain solution
//VectorConfig expected_optimized; // no clue... VectorConfig e; // expected solution
//VectorConfig optimized = optimize(actual); Vector v = Vector_(2, 0., 0.);
//CHECK(assert_equal(expected_optimized, optimized)); for (int i=1; i<=7; i++)
e.insert(symbol('x', i), v);
VectorConfig optimized = optimize(actual); // actual solution
CHECK(assert_equal(e, optimized));
} }
/* ************************************************************************* */ /* ************************************************************************* */