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

Use [involves], and now updating orphan list

release/4.3a0
Frank Dellaert 2009-11-20 05:23:47 +00:00
parent 55e85fd8a5
commit e440767db9
1 changed files with 13 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
cpp/testIncremental.cpp
View File

@ -31,16 +31,27 @@ SymbolicBayesTree update(const SymbolicBayesTree& initial,
// create an empty factor graph // create an empty factor graph
SymbolicFactorGraph factorGraph; SymbolicFactorGraph factorGraph;
// the list of orphaned subtrees
std::list<SymbolicBayesTree::sharedClique> orphans;
// process each key of the new factor // process each key of the new factor
BOOST_FOREACH(string key, newFactor->keys()) { BOOST_FOREACH(string key, newFactor->keys()) {
// todo: add path to root // todo: add path to root
// only add if key is not yet in the factor graph // only add if key is not yet in the factor graph
std::list<std::string> keys = factorGraph.keys(); if (!factorGraph.involves(key)) {
if (find(keys.begin(), keys.end(), key) == keys.end()) {
// get the clique // get the clique
SymbolicBayesTree::sharedClique clique = initial[key]; 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); FactorGraph<SymbolicFactor> clique_factors(*clique);
// add it to the factor graph // add it to the factor graph
@ -48,12 +59,6 @@ SymbolicBayesTree update(const SymbolicBayesTree& initial,
} }
} }
// todo: automatically generate list of orphans, including subtrees!
std::list<SymbolicBayesTree::sharedClique> orphans;
SymbolicBayesTree::sharedClique TEL = initial["T"];
SymbolicBayesTree::sharedClique XE = initial["X"];
orphans += TEL, XE;
// now add the new factor // now add the new factor
factorGraph.push_back(newFactor); factorGraph.push_back(newFactor);