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

More efficient removeTop

release/4.3a0
Frank Dellaert 2009-11-23 00:02:06 +00:00
parent 35af122e83
commit 4951a25453
3 changed files with 19 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
cpp/BayesTree-inl.h
View File

@ -393,11 +393,8 @@ namespace gtsam {
// TODO: add to factors and orphans
template<class Conditional>
template<class Factor>
pair<FactorGraph<Factor>, typename BayesTree<Conditional>::Cliques>
BayesTree<Conditional>::removeTop(const boost::shared_ptr<Factor>& newFactor) {
FactorGraph<Factor> factors;
Cliques orphans;
void BayesTree<Conditional>::removeTop(const boost::shared_ptr<Factor>& newFactor,
FactorGraph<Factor> &factors, typename BayesTree<Conditional>::Cliques& orphans) {
// process each key of the new factor
BOOST_FOREACH(string key, newFactor->keys())
@ -415,8 +412,6 @@ namespace gtsam {
orphans.splice (orphans.begin(), orphans1);
} catch (std::invalid_argument e) {
}
return make_pair(factors,orphans);
}
/* ************************************************************************* */
@ -428,15 +423,8 @@ namespace gtsam {
// Remove the contaminated part of the Bayes tree
FactorGraph<Factor> factors;
Cliques orphans;
BOOST_FOREACH(boost::shared_ptr<Factor> factor, newFactors) {
FactorGraph<Factor> factors1;
Cliques orphans1;
boost::tie(factors1, orphans1) = this->removeTop<Factor>(factor);
factors.push_back(factors1);
orphans.splice (orphans.begin(), orphans1);
}
BOOST_FOREACH(boost::shared_ptr<Factor> factor, newFactors)
this->removeTop<Factor>(factor, factors, orphans);
return make_pair(factors,orphans);
}

15
cpp/BayesTree.h
View File

@ -170,18 +170,21 @@ namespace gtsam {
std::pair<FactorGraph<Factor>, Cliques> removePath(sharedClique clique);
/**
* Given a set of factors, turn "contaminated" part of the tree back into a factor graph
* and return it along with a list of orphaned subtree roots.
* This is used for incrementally updating a BayesTree given new measurements (factors).
* Given a factor, turn "contaminated" part of the tree back into a factor graph.
* Factors and orphans are added to the in/out arguments.
*/
template<class Factor>
std::pair<FactorGraph<Factor>, Cliques> removeTop(const boost::shared_ptr<Factor>& newFactor);
void removeTop(const boost::shared_ptr<Factor>& newFactor,
FactorGraph<Factor> &factors, Cliques& orphans);
/**
* Call removeTop for several factors in a factorGraph
* Given a set of factors, turn "contaminated" part of the tree back into a
* factor graph and return it along with a list of orphaned subtree roots.
* Used for incrementally updating a BayesTree given new measurements (factors).
*/
template<class Factor>
std::pair<FactorGraph<Factor>, Cliques> removeTop(const FactorGraph<Factor>& newFactors);
std::pair<FactorGraph<Factor>, Cliques>
removeTop(const FactorGraph<Factor>& newFactors);
/**
* iSAM.

10
cpp/testBayesTree.cpp
View File

@ -415,7 +415,7 @@ TEST( BayesTree, removeTop )
// Remove the contaminated part of the Bayes tree
FactorGraph<SymbolicFactor> factors;
SymbolicBayesTree::Cliques orphans;
boost::tie(factors,orphans) = bayesTree.removeTop<SymbolicFactor>(newFactor);
bayesTree.removeTop<SymbolicFactor>(newFactor, factors, orphans);
// Check expected outcome
SymbolicFactorGraph expected;
@ -430,11 +430,13 @@ TEST( BayesTree, removeTop )
// Try removeTop again with a factor that should not change a thing
boost::shared_ptr<SymbolicFactor> newFactor2(new SymbolicFactor("B"));
boost::tie(factors,orphans) = bayesTree.removeTop<SymbolicFactor>(newFactor2);
FactorGraph<SymbolicFactor> factors2;
SymbolicBayesTree::Cliques orphans2;
bayesTree.removeTop<SymbolicFactor>(newFactor2, factors2, orphans2);
SymbolicFactorGraph expected2;
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors));
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors2));
SymbolicBayesTree::Cliques expectedOrphans2;
CHECK(assert_equal(expectedOrphans2, orphans));
CHECK(assert_equal(expectedOrphans2, orphans2));
}