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

using removeTop now

release/4.3a0
Frank Dellaert 2009-11-22 18:27:14 +00:00
parent 952798e751
commit 32db0595f4
1 changed files with 8 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
cpp/testIncremental.cpp
View File

@ -6,6 +6,7 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign; using namespace boost::assign;
@ -26,48 +27,19 @@ typedef BayesTree<GaussianConditional> GaussianBayesTree;
/* ************************************************************************* */ /* ************************************************************************* */
SymbolicBayesTree update(const SymbolicBayesTree& initial, SymbolicBayesTree update(SymbolicBayesTree& initial,
const boost::shared_ptr<SymbolicFactor>& newFactor) { const boost::shared_ptr<SymbolicFactor>& newFactor) {
// create an empty factor graph // Remove the contaminated part of the Bayes tree
SymbolicFactorGraph factorGraph; FactorGraph<SymbolicFactor> factors;
SymbolicBayesTree::Cliques orphans;
// the list of orphaned subtrees boost::tie(factors,orphans) = initial.removeTop<SymbolicFactor>(newFactor);
list<SymbolicBayesTree::sharedClique> orphans;
// process each key of the new factor
BOOST_FOREACH(string key, newFactor->keys()) {
// todo: add path to root
// only add if key is not yet in the factor graph
if (!factorGraph.involves(key)) {
// get the clique
SymbolicBayesTree::sharedClique clique = initial[key];
// if in orphans, remove the clique
orphans.remove(clique);
// add children to orphans
BOOST_FOREACH(SymbolicBayesTree::sharedClique child, clique->children_)
orphans.push_back(child);
// convert to factors
FactorGraph<SymbolicFactor> clique_factors(*clique);
// add it to the factor graph
factorGraph.push_back(clique_factors);
}
}
// now add the new factor
factorGraph.push_back(newFactor);
// create an ordering BELS // create an ordering BELS
Ordering ordering = factorGraph.getOrdering(); Ordering ordering = factors.getOrdering();
// eliminate into a Bayes net // eliminate into a Bayes net
SymbolicBayesNet bayesNet = eliminate<SymbolicFactor,SymbolicConditional>(factorGraph,ordering); SymbolicBayesNet bayesNet = eliminate<SymbolicFactor,SymbolicConditional>(factors,ordering);
// turn back into a Bayes Tree // turn back into a Bayes Tree
BayesTree<SymbolicConditional> newTree(bayesNet); BayesTree<SymbolicConditional> newTree(bayesNet);