From 8f9542b67db6319aca702be8540cd4ff88c1e8d1 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 25 Jan 2011 22:30:59 +0000 Subject: [PATCH] Re-implemented eliminated factor caching needed for ISAM2 (need to move this to the ISAM2 class though, see ticket) --- gtsam/inference/BayesTree-inl.h | 4 +++- gtsam/inference/BayesTree.h | 4 ++++ gtsam/inference/JunctionTree-inl.h | 10 ++++++---- gtsam/inference/JunctionTree.h | 4 ++-- 4 files changed, 15 insertions(+), 7 deletions(-) diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index fa2fdecbc..910869def 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -133,6 +133,7 @@ namespace gtsam { void BayesTree::Clique::permuteWithInverse(const Permutation& inversePermutation) { BayesNet::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 "); diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index a5ad7732e..372afb566 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -58,6 +58,7 @@ namespace gtsam { weak_ptr parent_; std::list children_; std::list separator_; /** separator keys */ + typename CONDITIONAL::Factor::shared_ptr cachedFactor_; friend class BayesTree; @@ -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; diff --git a/gtsam/inference/JunctionTree-inl.h b/gtsam/inference/JunctionTree-inl.h index 381d79ee3..45ea558ca 100644 --- a/gtsam/inference/JunctionTree-inl.h +++ b/gtsam/inference/JunctionTree-inl.h @@ -146,7 +146,7 @@ namespace gtsam { /* ************************************************************************* */ template pair::BayesTree::sharedClique, typename FG::sharedFactor> - JunctionTree::eliminateOneClique(const boost::shared_ptr& current) const { + JunctionTree::eliminateOneClique(const boost::shared_ptr& 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 children; BOOST_FOREACH(const boost::shared_ptr& child, current->children()) { pair 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 - typename JunctionTree::BayesTree::sharedClique JunctionTree::eliminate() const { + typename JunctionTree::BayesTree::sharedClique JunctionTree::eliminate(bool cache) const { if(this->root()) { - pair ret = this->eliminateOneClique(this->root()); + pair 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; diff --git a/gtsam/inference/JunctionTree.h b/gtsam/inference/JunctionTree.h index f9311bab5..879e1ff75 100644 --- a/gtsam/inference/JunctionTree.h +++ b/gtsam/inference/JunctionTree.h @@ -64,7 +64,7 @@ namespace gtsam { // recursive elimination function std::pair - eliminateOneClique(const boost::shared_ptr& clique) const; + eliminateOneClique(const boost::shared_ptr& 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