From 2df0e49e9b16aa36475ca2bde0646a751e775d08 Mon Sep 17 00:00:00 2001 From: Michael Kaess Date: Thu, 10 Dec 2009 17:00:11 +0000 Subject: [PATCH] optimize works for GaussianBayessTree --- cpp/GaussianBayesTree.cpp | 7 +++++-- cpp/testGaussianBayesTree.cpp | 9 ++++++--- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/cpp/GaussianBayesTree.cpp b/cpp/GaussianBayesTree.cpp index f465381fa..88af4543e 100644 --- a/cpp/GaussianBayesTree.cpp +++ b/cpp/GaussianBayesTree.cpp @@ -22,12 +22,14 @@ namespace gtsam { void optimize(const GaussianBayesTree::sharedClique& clique, VectorConfig& result) { // parents are assumed to already be solved and available in result 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; 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_) { +// list::const_iterator child; +// for (child = clique->children_.begin(); child != clique->children_.end(); child++) { optimize(child, result); } } @@ -37,6 +39,7 @@ VectorConfig optimize(const GaussianBayesTree& bayesTree) { VectorConfig result; // starting from the root, call optimize on each conditional optimize(bayesTree.root(), result); + return result; } } /// namespace gtsam diff --git a/cpp/testGaussianBayesTree.cpp b/cpp/testGaussianBayesTree.cpp index d36c19093..08ae9df8b 100644 --- a/cpp/testGaussianBayesTree.cpp +++ b/cpp/testGaussianBayesTree.cpp @@ -266,9 +266,12 @@ TEST( BayesTree, iSAM_smoother ) CHECK(assert_equal(expected, actual)); // obtain solution - //VectorConfig expected_optimized; // no clue... - //VectorConfig optimized = optimize(actual); - //CHECK(assert_equal(expected_optimized, optimized)); + VectorConfig e; // expected solution + Vector v = Vector_(2, 0., 0.); + for (int i=1; i<=7; i++) + e.insert(symbol('x', i), v); + VectorConfig optimized = optimize(actual); // actual solution + CHECK(assert_equal(e, optimized)); } /* ************************************************************************* */