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 // TODO: add to factors and orphans
template<class Conditional> template<class Conditional>
template<class Factor> template<class Factor>
pair<FactorGraph<Factor>, typename BayesTree<Conditional>::Cliques> void BayesTree<Conditional>::removeTop(const boost::shared_ptr<Factor>& newFactor,
BayesTree<Conditional>::removeTop(const boost::shared_ptr<Factor>& newFactor) { FactorGraph<Factor> &factors, typename BayesTree<Conditional>::Cliques& orphans) {
FactorGraph<Factor> factors;
Cliques 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())
@ -415,8 +412,6 @@ namespace gtsam {
orphans.splice (orphans.begin(), orphans1); orphans.splice (orphans.begin(), orphans1);
} catch (std::invalid_argument e) { } catch (std::invalid_argument e) {
} }
return make_pair(factors,orphans);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -428,15 +423,8 @@ namespace gtsam {
// Remove the contaminated part of the Bayes tree // Remove the contaminated part of the Bayes tree
FactorGraph<Factor> factors; FactorGraph<Factor> factors;
Cliques orphans; Cliques orphans;
BOOST_FOREACH(boost::shared_ptr<Factor> factor, newFactors) { BOOST_FOREACH(boost::shared_ptr<Factor> factor, newFactors)
this->removeTop<Factor>(factor, factors, orphans);
FactorGraph<Factor> factors1;
Cliques orphans1;
boost::tie(factors1, orphans1) = this->removeTop<Factor>(factor);
factors.push_back(factors1);
orphans.splice (orphans.begin(), orphans1);
}
return make_pair(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); std::pair<FactorGraph<Factor>, Cliques> removePath(sharedClique clique);
/** /**
* Given a set of factors, turn "contaminated" part of the tree back into a factor graph * Given a factor, turn "contaminated" part of the tree back into a factor graph.
* and return it along with a list of orphaned subtree roots. * Factors and orphans are added to the in/out arguments.
* This is used for incrementally updating a BayesTree given new measurements (factors).
*/ */
template<class Factor> 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> template<class Factor>
std::pair<FactorGraph<Factor>, Cliques> removeTop(const FactorGraph<Factor>& newFactors); std::pair<FactorGraph<Factor>, Cliques>
removeTop(const FactorGraph<Factor>& newFactors);
/** /**
* iSAM. * iSAM.

10
cpp/testBayesTree.cpp
View File

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