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

generalized most of the update algorithm

release/4.3a0
Michael Kaess 2009-11-19 23:25:03 +00:00
parent 429f27550c
commit 4ca1dbf325
1 changed files with 47 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
cpp/testIncremental.cpp
View File

@ -31,19 +31,28 @@ SymbolicBayesTree update(const SymbolicBayesTree& initial,
// create an empty factor graph // create an empty factor graph
SymbolicFactorGraph factorGraph; SymbolicFactorGraph factorGraph;
// get the ELB clique // process each key of the new factor
SymbolicBayesTree::sharedClique ELB = initial["B"]; BOOST_FOREACH(string key, newFactor->keys()) {
FactorGraph<SymbolicFactor> ELB_factors(*ELB); // todo: add path to root
// only add if key is not yet in the factor graph
std::list<std::string> keys = factorGraph.keys();
if (find(keys.begin(), keys.end(), key) == keys.end()) {
// get the clique
SymbolicBayesTree::sharedClique clique = initial[key];
FactorGraph<SymbolicFactor> clique_factors(*clique);
// add it to the factor graph // add it to the factor graph
factorGraph.push_back(ELB_factors); factorGraph.push_back(clique_factors);
}
}
// get the SLB clique // todo: automatically generate list of orphans, including subtrees!
SymbolicBayesTree::sharedClique SLB = initial["S"]; std::list<SymbolicBayesTree::sharedClique> orphans;
FactorGraph<SymbolicFactor> SLB_factors(*SLB); SymbolicBayesTree::sharedClique TEL = initial["T"];
SymbolicBayesTree::sharedClique XE = initial["X"];
// add it to the factor graph orphans += TEL, XE;
factorGraph.push_back(SLB_factors);
// now add the new factor // now add the new factor
factorGraph.push_back(newFactor); factorGraph.push_back(newFactor);
@ -57,15 +66,33 @@ SymbolicBayesTree update(const SymbolicBayesTree& initial,
// turn back into a Bayes Tree // turn back into a Bayes Tree
BayesTree<SymbolicConditional> newTree(bayesNet); BayesTree<SymbolicConditional> newTree(bayesNet);
// get the orphan cliques // add orphans to the bottom of the new tree
SymbolicBayesTree::sharedClique TEL = initial["T"]; BOOST_FOREACH(SymbolicBayesTree::sharedClique orphan, orphans) {
SymbolicBayesTree::sharedClique XE = initial["X"]; std::list<std::string>::const_iterator it = orphan->separator_.begin();
for (; it != orphan->separator_.end(); it++) {
// get clique from new tree to attach to // get clique from new tree to attach to
SymbolicBayesTree::sharedClique new_ELB = newTree["E"]; SymbolicBayesTree::sharedClique clique = newTree[*it];
// add orphans to the bottom of the new tree // check if all conditionals in there, only add once
new_ELB->children_ += TEL,XE; bool is_subset = true;
std::list<std::string>::const_iterator it2 = orphan->separator_.begin();
for (; it2 != orphan->separator_.end(); it2++) {
// if any one is not included, then we have to stop and search for another clique
std::list<std::string> keys = clique->keys();
if (find(keys.begin(), keys.end(), *it2) == keys.end()) {
is_subset = false;
break;
}
}
// this clique contains all the keys of the orphan, so we can add the orphan as a child todo: what about the tree below the orphan?
if (is_subset) {
clique->children_ += orphan;
break;
}
}
}
return newTree; return newTree;
} }
@ -91,6 +118,8 @@ TEST( BayesTree, iSAM )
bayesTree.insert(T); bayesTree.insert(T);
bayesTree.insert(X); bayesTree.insert(X);
// Now we modify the Bayes tree by inserting a new factor over B and S
// New conditionals in modified top of the tree // New conditionals in modified top of the tree
SymbolicConditional::shared_ptr SymbolicConditional::shared_ptr
S_(new SymbolicConditional("S")), S_(new SymbolicConditional("S")),