First Iteration of Shortcut Cache changes and misc const fixes

gtsam/inference/BayesTree-inl.h

@@ -555,12 +555,29 @@ namespace gtsam {
 		}
 	}
 
+	/* ************************************************************************* */
+	template<class CONDITIONAL, class CLIQUE>
+	void BayesTree<CONDITIONAL, CLIQUE>::deleteCachedShorcuts(const sharedClique& subtree) {
+		// 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

@@ -108,10 +108,16 @@ namespace gtsam {
 	// P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
 	/* ************************************************************************* */
 	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;
 
+		BayesNet<ConditionalType> p_S_R;	//shortcut P(S|R)
+
+		//Check if the ShortCut already exists
+		if(!cachedShortcut_){
+
 			// A first base case is when this clique or its parent is the root,
 			// in which case we return an empty Bayes net.
 
@@ -181,7 +187,6 @@ namespace gtsam {
 	    // 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.
-    BayesNet<ConditionalType> p_S_R;
 	    BOOST_REVERSE_FOREACH(typename ConditionalType::shared_ptr conditional, *eliminated) {
 	      assert(conditional->nrFrontals() == 1);
 	      if(separator.find(toBack[conditional->firstFrontalKey()]) != separator.end()) {
@@ -197,8 +202,13 @@ namespace gtsam {
 	    if(debug) toBack.print("toBack: ");
 	    p_S_R.permuteWithInverse(toBack);
 
-    // return the parent shortcut P(Sp|R)
 	    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);
+    BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
 
     /** 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