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

Some simplification

release/4.3a0
Frank Dellaert 2009-11-22 22:39:32 +00:00
parent ab117947d7
commit 4e74edba72
1 changed files with 7 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
cpp/testIncremental.cpp
View File

@ -27,21 +27,19 @@ typedef BayesTree<GaussianConditional> GaussianBayesTree;
/* ************************************************************************* */ /* ************************************************************************* */
void update(SymbolicBayesTree& bayesTree, const FactorGraph<SymbolicFactor> factorGraph) { void update(SymbolicBayesTree& bayesTree, const FactorGraph<SymbolicFactor>& factorGraph) {
// Remove the contaminated part of the Bayes tree
FactorGraph<SymbolicFactor> factors; FactorGraph<SymbolicFactor> factors;
SymbolicBayesTree::Cliques orphans; SymbolicBayesTree::Cliques orphans;
BOOST_FOREACH(boost::shared_ptr<SymbolicFactor> factor, factorGraph) {
BOOST_FOREACH(FactorGraph<SymbolicFactor>::sharedFactor factor, factorGraph) {
// Remove the contaminated part of the Bayes tree
FactorGraph<SymbolicFactor> newFactors; FactorGraph<SymbolicFactor> newFactors;
SymbolicBayesTree::Cliques newOrphans; SymbolicBayesTree::Cliques newOrphans;
boost::tie(newFactors, newOrphans) = bayesTree.removeTop<SymbolicFactor>(factor); boost::tie(newFactors, newOrphans) = bayesTree.removeTop<SymbolicFactor>(factor);
orphans.insert(orphans.begin(), newOrphans.begin(), newOrphans.end()); factors.push_back(newFactors);
const FactorGraph<SymbolicFactor> test = newFactors; orphans.splice (orphans.begin(), newOrphans);
BOOST_FOREACH(FactorGraph<SymbolicFactor>::sharedFactor newFactor, (const FactorGraph<SymbolicFactor>)newFactors)
factors.push_back(newFactor);
} }
// create an ordering for the new and contaminated factors // create an ordering for the new and contaminated factors
@ -50,14 +48,14 @@ void update(SymbolicBayesTree& bayesTree, const FactorGraph<SymbolicFactor> fact
// eliminate into a Bayes net // eliminate into a Bayes net
SymbolicBayesNet bayesNet = eliminate<SymbolicFactor,SymbolicConditional>(factors,ordering); SymbolicBayesNet bayesNet = eliminate<SymbolicFactor,SymbolicConditional>(factors,ordering);
// turn back into a Bayes Tree // insert conditionals back in, straight into the topless bayesTree
SymbolicBayesNet::const_reverse_iterator rit; SymbolicBayesNet::const_reverse_iterator rit;
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
bayesTree.insert(*rit); bayesTree.insert(*rit);
// add orphans to the bottom of the new tree // add orphans to the bottom of the new tree
BOOST_FOREACH(SymbolicBayesTree::sharedClique orphan, orphans) { BOOST_FOREACH(SymbolicBayesTree::sharedClique orphan, orphans) {
string key = *(orphan->separator_.begin()); // todo: assumes there is a separator... string key = orphan->separator_.front(); // todo: assumes there is a separator...
SymbolicBayesTree::sharedClique parent = bayesTree[key]; SymbolicBayesTree::sharedClique parent = bayesTree[key];
parent->children_ += orphan; parent->children_ += orphan;
} }