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

Re-implemented eliminated factor caching needed for ISAM2 (need to move this to the ISAM2 class though, see ticket)

release/4.3a0
Richard Roberts 2011-01-25 22:30:59 +00:00
parent 7b0a63ecd3
commit 8f9542b67d
4 changed files with 15 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/inference/BayesTree-inl.h
View File

@ -133,6 +133,7 @@ namespace gtsam {
void BayesTree<CONDITIONAL>::Clique::permuteWithInverse(const Permutation& inversePermutation) {
BayesNet<CONDITIONAL>::permuteWithInverse(inversePermutation);
BOOST_FOREACH(Index& separatorKey, separator_) { separatorKey = inversePermutation[separatorKey]; }
if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
BOOST_FOREACH(const shared_ptr& child, children_) {
child->permuteWithInverse(inversePermutation);
}
@ -152,6 +153,7 @@ namespace gtsam {
#endif
if(changed) {
BOOST_FOREACH(Index& separatorKey, separator_) { separatorKey = inversePermutation[separatorKey]; }
if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
BOOST_FOREACH(const shared_ptr& child, children_) {
(void)child->permuteSeparatorWithInverse(inversePermutation);
}
@ -611,7 +613,7 @@ namespace gtsam {
// otherwise, find the parent clique by using the index data structure
// to find the lowest-ordered parent
Index parentRepresentative = findParentClique(parents);
if(debug) cout << "First-eliminated parent is " << parentRepresentative << endl;
if(debug) cout << "First-eliminated parent is " << parentRepresentative << ", have " << nodes_.size() << " nodes." << endl;
sharedClique parent_clique = (*this)[parentRepresentative];
if(debug) parent_clique->print("Parent clique is ");

4
gtsam/inference/BayesTree.h
View File

@ -58,6 +58,7 @@ namespace gtsam {
weak_ptr parent_;
std::list<shared_ptr> children_;
std::list<Index> separator_; /** separator keys */
typename CONDITIONAL::Factor::shared_ptr cachedFactor_;
friend class BayesTree<CONDITIONAL>;
@ -89,6 +90,9 @@ namespace gtsam {
/** The size of subtree rooted at this clique, i.e., nr of Cliques */
size_t treeSize() const;
/** Access the cached factor (this is a hack) */
typename CONDITIONAL::Factor::shared_ptr& cachedFactor() { return cachedFactor_; }
/** print this node and entire subtree below it */
void printTree(const std::string& indent="") const;

10
gtsam/inference/JunctionTree-inl.h
View File

@ -146,7 +146,7 @@ namespace gtsam {
/* ************************************************************************* */
template <class FG>
pair<typename JunctionTree<FG>::BayesTree::sharedClique, typename FG::sharedFactor>
JunctionTree<FG>::eliminateOneClique(const boost::shared_ptr<const Clique>& current) const {
JunctionTree<FG>::eliminateOneClique(const boost::shared_ptr<const Clique>& current, bool cache) const {
FG fg; // factor graph will be assembled from local factors and marginalized children
fg.reserve(current->size() + current->children().size());
@ -156,7 +156,7 @@ namespace gtsam {
list<typename BayesTree::sharedClique> children;
BOOST_FOREACH(const boost::shared_ptr<const Clique>& child, current->children()) {
pair<typename BayesTree::sharedClique, typename FG::sharedFactor> tree_factor(
eliminateOneClique(child));
eliminateOneClique(child, cache));
children.push_back(tree_factor.first);
fg.push_back(tree_factor.second);
}
@ -181,6 +181,8 @@ namespace gtsam {
BOOST_FOREACH(typename BayesTree::sharedClique& childRoot, children) {
childRoot->parent_ = new_clique;
}
if(cache)
new_clique->cachedFactor() = eliminated.second;
toc(3, "Update tree");
return make_pair(new_clique, eliminated.second);
@ -188,9 +190,9 @@ namespace gtsam {
/* ************************************************************************* */
template <class FG>
typename JunctionTree<FG>::BayesTree::sharedClique JunctionTree<FG>::eliminate() const {
typename JunctionTree<FG>::BayesTree::sharedClique JunctionTree<FG>::eliminate(bool cache) const {
if(this->root()) {
pair<typename BayesTree::sharedClique, typename FG::sharedFactor> ret = this->eliminateOneClique(this->root());
pair<typename BayesTree::sharedClique, typename FG::sharedFactor> ret = this->eliminateOneClique(this->root(), cache);
if (ret.second->size() != 0)
throw runtime_error("JuntionTree::eliminate: elimination failed because of factors left over!");
return ret.first;

4
gtsam/inference/JunctionTree.h
View File

@ -64,7 +64,7 @@ namespace gtsam {
// recursive elimination function
std::pair<typename BayesTree::sharedClique, typename FG::sharedFactor>
eliminateOneClique(const boost::shared_ptr<const Clique>& clique) const;
eliminateOneClique(const boost::shared_ptr<const Clique>& clique, bool cache=false) const;
// internal constructor
void construct(const FG& fg, const VariableIndex& variableIndex);
@ -80,7 +80,7 @@ namespace gtsam {
JunctionTree(const FG& fg, const VariableIndex& variableIndex);
// eliminate the factors in the subgraphs
typename BayesTree::sharedClique eliminate() const;
typename BayesTree::sharedClique eliminate(bool cache=false) const;
}; // JunctionTree