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

gtsam/inference/BayesTree-inl.h

@@ -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 ");
 

gtsam/inference/BayesTree.h

@@ -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;
 

gtsam/inference/JunctionTree-inl.h

@@ -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;

gtsam/inference/JunctionTree.h

@@ -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