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

Fixed Michael's c++ trubbles.

release/4.3a0
Frank Dellaert 2009-12-09 22:15:38 +00:00
parent 72ba1bee24
commit db0d7286c2
3 changed files with 13 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cpp/GaussianBayesTree.cpp
View File

@ -19,21 +19,21 @@ template class BayesTree<GaussianConditional>;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void optimize(GaussianBayesTree::sharedClique clique, VectorConfig& result) { void optimize(const GaussianBayesTree::sharedClique& clique, VectorConfig& result) {
#if 0
// parents are assumed to already be solved and available in result // parents are assumed to already be solved and available in result
BOOST_REVERSE_FOREACH(GaussianConditional::shared_ptr cg, clique) { GaussianBayesTree::Clique::const_reverse_iterator it;
for(it = clique->rend(); it!=clique->rbegin(); 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_) {
optimize(child, result); optimize(child, result);
} }
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig optimize(GaussianBayesTree& bayesTree) { 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);

4
cpp/GaussianBayesTree.h
View File

@ -26,9 +26,9 @@ namespace gtsam {
typedef BayesTree<GaussianConditional> GaussianBayesTree; typedef BayesTree<GaussianConditional> GaussianBayesTree;
// recursively optimize this conditional and all subtrees // recursively optimize this conditional and all subtrees
void optimize(GaussianBayesTree::sharedClique clique, VectorConfig& result); void optimize(const GaussianBayesTree::sharedClique& clique, VectorConfig& result);
// optimize the BayesTree, starting from the root // optimize the BayesTree, starting from the root
VectorConfig optimize(GaussianBayesTree& bayesTree); VectorConfig optimize(const GaussianBayesTree& bayesTree);
}/// namespace gtsam }/// namespace gtsam

11
cpp/testGaussianBayesTree.cpp
View File

@ -262,12 +262,13 @@ TEST( BayesTree, iSAM_smoother )
for (int t = 1; t <= 7; t++) ordering += symbol('x', t); for (int t = 1; t <= 7; t++) ordering += symbol('x', t);
GaussianBayesTree expected(smoother.eliminate(ordering)); GaussianBayesTree expected(smoother.eliminate(ordering));
// obtain solution // Check whether BayesTree is correct
VectorConfig expected_optimized; // no clue...
VectorConfig optimized = optimize(actual);
CHECK(assert_equal(expected_optimized, optimized));
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
// obtain solution
//VectorConfig expected_optimized; // no clue...
//VectorConfig optimized = optimize(actual);
//CHECK(assert_equal(expected_optimized, optimized));
} }
/* ************************************************************************* */ /* ************************************************************************* */