First Iteration of Shortcut Cache changes and misc const fixes

gtsam/inference/BayesTree-inl.h

@@ -557,10 +557,27 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	template<class CONDITIONAL, class CLIQUE>
-  template<class CONTAINER>
+	void BayesTree<CONDITIONAL, CLIQUE>::deleteCachedShorcuts(const sharedClique& subtree) {
-  void BayesTree<CONDITIONAL,CLIQUE>::removeTop(const CONTAINER& keys,
+		// Check if subtree exists
+		if (subtree) {
+			//Delete CachedShortcut for this clique
+			subtree->resetCachedShortcut();
+			// Recursive call over all child cliques
+			BOOST_FOREACH(sharedClique& childClique, subtree->children()) {
+				deleteCachedShorcuts(childClique);
+			}
+		}
+	}
+
+	/* ************************************************************************* */
+	template<class CONDITIONAL, class CLIQUE>
+	template<class CONTAINER>
+	void BayesTree<CONDITIONAL,CLIQUE>::removeTop(const CONTAINER& keys,
   		BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) {
 
+		//TODO: Improve this
+		deleteCachedShorcuts(this->root_);
+
 		// process each key of the new factor
 	  BOOST_FOREACH(const Index& key, keys) {
 

gtsam/inference/BayesTree.h

@@ -280,6 +280,12 @@ namespace gtsam {
 		sharedClique insert(const sharedConditional& clique,
 				std::list<sharedClique>& children, bool isRootClique = false);
 
+		/**
+		 * This deletes the cached shortcuts of all cliques in a subtree. This is
+		 * performed when the bayes tree is modified.
+		 */
+		void deleteCachedShorcuts(const sharedClique& subtree);
+
   private:
 
     /** deep copy to another tree */

gtsam/inference/BayesTreeCliqueBase-inl.h

@@ -102,103 +102,113 @@ namespace gtsam {
     return changed;
   }
 
-  /* ************************************************************************* */
+	/* ************************************************************************* */
-  // The shortcut density is a conditional P(S|R) of the separator of this
+	// The shortcut density is a conditional P(S|R) of the separator of this
-  // clique on the root. We can compute it recursively from the parent shortcut
+	// clique on the root. We can compute it recursively from the parent shortcut
-  // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
+	// P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
-  /* ************************************************************************* */
+	/* ************************************************************************* */
-  template<class DERIVED, class CONDITIONAL>
+	template<class DERIVED, class CONDITIONAL>
-  BayesNet<CONDITIONAL> BayesTreeCliqueBase<DERIVED,CONDITIONAL>::shortcut(derived_ptr R, Eliminate function) {
+	BayesNet<CONDITIONAL> BayesTreeCliqueBase<DERIVED, CONDITIONAL>::shortcut(
+			derived_ptr R, Eliminate function) const{
 
-    static const bool debug = false;
+		static const bool debug = false;
 
-    // A first base case is when this clique or its parent is the root,
+		BayesNet<ConditionalType> p_S_R;	//shortcut P(S|R)
-    // in which case we return an empty Bayes net.
 
-    derived_ptr parent(parent_.lock());
+		//Check if the ShortCut already exists
+		if(!cachedShortcut_){
 
-    if (R.get()==this || parent==R) {
+			// A first base case is when this clique or its parent is the root,
-      BayesNet<ConditionalType> empty;
+			// in which case we return an empty Bayes net.
-      return empty;
-    }
 
-    // The root conditional
+			derived_ptr parent(parent_.lock());
-    FactorGraph<FactorType> p_R(BayesNet<ConditionalType>(R->conditional()));
 
-    // The parent clique has a ConditionalType for each frontal node in Fp
+			if (R.get() == this || parent == R) {
-    // so we can obtain P(Fp|Sp) in factor graph form
+				BayesNet<ConditionalType> empty;
-    FactorGraph<FactorType> p_Fp_Sp(BayesNet<ConditionalType>(parent->conditional()));
+				return empty;
+			}
 
-    // If not the base case, obtain the parent shortcut P(Sp|R) as factors
+			// The root conditional
-    FactorGraph<FactorType> p_Sp_R(parent->shortcut(R, function));
+			FactorGraph<FactorType> p_R(BayesNet<ConditionalType>(R->conditional()));
 
-    // now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R)
+			// The parent clique has a ConditionalType for each frontal node in Fp
-    FactorGraph<FactorType> p_Cp_R;
+			// so we can obtain P(Fp|Sp) in factor graph form
-    p_Cp_R.push_back(p_R);
+			FactorGraph<FactorType> p_Fp_Sp(BayesNet<ConditionalType>(parent->conditional()));
-    p_Cp_R.push_back(p_Fp_Sp);
-    p_Cp_R.push_back(p_Sp_R);
 
-    // Eliminate into a Bayes net with ordering designed to integrate out
+			// If not the base case, obtain the parent shortcut P(Sp|R) as factors
-    // any variables not in *our* separator. Variables to integrate out must be
+			FactorGraph<FactorType> p_Sp_R(parent->shortcut(R, function));
-    // eliminated first hence the desired ordering is [Cp\S S].
-    // However, an added wrinkle is that Cp might overlap with the root.
-    // Keys corresponding to the root should not be added to the ordering at all.
 
-    if(debug) {
+			// now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R)
-      p_R.print("p_R: ");
+			FactorGraph<FactorType> p_Cp_R;
-      p_Fp_Sp.print("p_Fp_Sp: ");
+			p_Cp_R.push_back(p_R);
-      p_Sp_R.print("p_Sp_R: ");
+			p_Cp_R.push_back(p_Fp_Sp);
-    }
+			p_Cp_R.push_back(p_Sp_R);
 
-    // We want to factor into a conditional of the clique variables given the
+	    // Eliminate into a Bayes net with ordering designed to integrate out
-    // root and the marginal on the root, integrating out all other variables.
+	    // any variables not in *our* separator. Variables to integrate out must be
-    // The integrands include any parents of this clique and the variables of
+	    // eliminated first hence the desired ordering is [Cp\S S].
-    // the parent clique.
+	    // However, an added wrinkle is that Cp might overlap with the root.
-    FastSet<Index> variablesAtBack;
+	    // Keys corresponding to the root should not be added to the ordering at all.
-    FastSet<Index> separator;
-    size_t uniqueRootVariables = 0;
-    BOOST_FOREACH(const Index separatorIndex, this->conditional()->parents()) {
-      variablesAtBack.insert(separatorIndex);
-      separator.insert(separatorIndex);
-      if(debug) std::cout << "At back (this): " << separatorIndex << std::endl;
-    }
-    BOOST_FOREACH(const Index key, R->conditional()->keys()) {
-      if(variablesAtBack.insert(key).second)
-        ++ uniqueRootVariables;
-      if(debug) std::cout << "At back (root): " << key << std::endl;
-    }
 
-    Permutation toBack = Permutation::PushToBack(
+	    if(debug) {
-        std::vector<Index>(variablesAtBack.begin(), variablesAtBack.end()),
+	      p_R.print("p_R: ");
-        R->conditional()->lastFrontalKey() + 1);
+	      p_Fp_Sp.print("p_Fp_Sp: ");
-    Permutation::shared_ptr toBackInverse(toBack.inverse());
+	      p_Sp_R.print("p_Sp_R: ");
-    BOOST_FOREACH(const typename FactorType::shared_ptr& factor, p_Cp_R) {
+	    }
-      factor->permuteWithInverse(*toBackInverse); }
-    typename BayesNet<ConditionalType>::shared_ptr eliminated(EliminationTree<
-        FactorType>::Create(p_Cp_R)->eliminate(function));
 
-    // Take only the conditionals for p(S|R).  We check for each variable being
+	    // We want to factor into a conditional of the clique variables given the
-    // in the separator set because if some separator variables overlap with
+	    // root and the marginal on the root, integrating out all other variables.
-    // root variables, we cannot rely on the number of root variables, and also
+	    // The integrands include any parents of this clique and the variables of
-    // want to include those variables in the conditional.
+	    // the parent clique.
-    BayesNet<ConditionalType> p_S_R;
+	    FastSet<Index> variablesAtBack;
-    BOOST_REVERSE_FOREACH(typename ConditionalType::shared_ptr conditional, *eliminated) {
+	    FastSet<Index> separator;
-      assert(conditional->nrFrontals() == 1);
+	    size_t uniqueRootVariables = 0;
-      if(separator.find(toBack[conditional->firstFrontalKey()]) != separator.end()) {
+	    BOOST_FOREACH(const Index separatorIndex, this->conditional()->parents()) {
-        if(debug)
+	      variablesAtBack.insert(separatorIndex);
-          conditional->print("Taking C|R conditional: ");
+	      separator.insert(separatorIndex);
-        p_S_R.push_front(conditional);
+	      if(debug) std::cout << "At back (this): " << separatorIndex << std::endl;
-      }
+	    }
-      if(p_S_R.size() == separator.size())
+	    BOOST_FOREACH(const Index key, R->conditional()->keys()) {
-        break;
+	      if(variablesAtBack.insert(key).second)
-    }
+	        ++ uniqueRootVariables;
+	      if(debug) std::cout << "At back (root): " << key << std::endl;
+	    }
 
-    // Undo the permutation
+	    Permutation toBack = Permutation::PushToBack(
-    if(debug) toBack.print("toBack: ");
+	        std::vector<Index>(variablesAtBack.begin(), variablesAtBack.end()),
-    p_S_R.permuteWithInverse(toBack);
+	        R->conditional()->lastFrontalKey() + 1);
+	    Permutation::shared_ptr toBackInverse(toBack.inverse());
+	    BOOST_FOREACH(const typename FactorType::shared_ptr& factor, p_Cp_R) {
+	      factor->permuteWithInverse(*toBackInverse); }
+	    typename BayesNet<ConditionalType>::shared_ptr eliminated(EliminationTree<
+	        FactorType>::Create(p_Cp_R)->eliminate(function));
 
-    // return the parent shortcut P(Sp|R)
+	    // Take only the conditionals for p(S|R).  We check for each variable being
-    assertInvariants();
+	    // in the separator set because if some separator variables overlap with
+	    // root variables, we cannot rely on the number of root variables, and also
+	    // want to include those variables in the conditional.
+	    BOOST_REVERSE_FOREACH(typename ConditionalType::shared_ptr conditional, *eliminated) {
+	      assert(conditional->nrFrontals() == 1);
+	      if(separator.find(toBack[conditional->firstFrontalKey()]) != separator.end()) {
+	        if(debug)
+	          conditional->print("Taking C|R conditional: ");
+	        p_S_R.push_front(conditional);
+	      }
+	      if(p_S_R.size() == separator.size())
+	        break;
+	    }
+
+	    // Undo the permutation
+	    if(debug) toBack.print("toBack: ");
+	    p_S_R.permuteWithInverse(toBack);
+
+	    assertInvariants();
+	    cachedShortcut_ = p_S_R;
+		}
+		else
+			p_S_R = *cachedShortcut_;
+
+		// return the shortcut P(S|R)
     return p_S_R;
   }
 
@@ -210,7 +220,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class DERIVED, class CONDITIONAL>
   FactorGraph<typename BayesTreeCliqueBase<DERIVED,CONDITIONAL>::FactorType> BayesTreeCliqueBase<DERIVED,CONDITIONAL>::marginal(
-      derived_ptr R, Eliminate function) {
+      derived_ptr R, Eliminate function) const{
     // If we are the root, just return this root
     // NOTE: immediately cast to a factor graph
     BayesNet<ConditionalType> bn(R->conditional());
@@ -231,7 +241,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class DERIVED, class CONDITIONAL>
   FactorGraph<typename BayesTreeCliqueBase<DERIVED,CONDITIONAL>::FactorType> BayesTreeCliqueBase<DERIVED,CONDITIONAL>::joint(
-      derived_ptr C2, derived_ptr R, Eliminate function) {
+      derived_ptr C2, derived_ptr R, Eliminate function) const {
     // For now, assume neither is the root
 
     // Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R)

gtsam/inference/BayesTreeCliqueBase.h

@@ -80,6 +80,9 @@ namespace gtsam {
     derived_weak_ptr parent_;
     std::list<derived_ptr> children_;
 
+    /// This stores the Cached Shortcut value
+    mutable boost::optional<BayesNet<ConditionalType> > cachedShortcut_;
+
   	/// @name Testable
   	/// @{
 
@@ -150,14 +153,13 @@ namespace gtsam {
     bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
 
     /** return the conditional P(S|Root) on the separator given the root */
-    // TODO: create a cached version
+    BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
-    BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function);
 
     /** return the marginal P(C) of the clique */
-    FactorGraph<FactorType> marginal(derived_ptr root, Eliminate function);
+    FactorGraph<FactorType> marginal(derived_ptr root, Eliminate function) const;
 
     /** return the joint P(C1,C2), where C1==this. TODO: not a method? */
-    FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr root, Eliminate function);
+    FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr root, Eliminate function) const;
 
     friend class BayesTree<ConditionalType, DerivedType>;
 
@@ -166,6 +168,9 @@ namespace gtsam {
     ///TODO: comment
     void assertInvariants() const;
 
+    /// Reset the computed shortcut of this clique. Used by friend BayesTree
+    void resetCachedShortcut() { cachedShortcut_ = boost::none; }
+
   private:
 
     /** Cliques cannot be copied except by the clone() method, which does not