Implemented partial elimination and sparse variable index remapping (Reduction) to enable Frank's new marginal code

gtsam/discrete/DecisionTreeFactor.h

@@ -140,6 +140,14 @@ namespace gtsam {
 			Potentials::permuteWithInverse(inversePermutation);
 		}
     
+	  /**
+	   * Apply a reduction, which is a remapping of variable indices.
+	   */
+    virtual void reduceWithInverse(const internal::Reduction& inverseReduction) {
+    	DiscreteFactor::reduceWithInverse(inverseReduction);
+      Potentials::reduceWithInverse(inverseReduction);
+    }
+
 		/// @}
 	};
 // DecisionTreeFactor

gtsam/discrete/DiscreteFactor.h

@@ -108,6 +108,13 @@ namespace gtsam {
 			IndexFactor::permuteWithInverse(inversePermutation);
 		}
 
+	  /**
+	   * Apply a reduction, which is a remapping of variable indices.
+	   */
+    virtual void reduceWithInverse(const internal::Reduction& inverseReduction) {
+    	IndexFactor::reduceWithInverse(inverseReduction);
+    }
+
 		/// @}
 	};
 // DiscreteFactor

gtsam/discrete/DiscreteFactorGraph.cpp

@@ -75,6 +75,24 @@ namespace gtsam {
 		}
 	}
 
+  /* ************************************************************************* */
+  void DiscreteFactorGraph::permuteWithInverse(
+    const Permutation& inversePermutation) {
+      BOOST_FOREACH(const sharedFactor& factor, factors_) {
+        if(factor)
+          factor->permuteWithInverse(inversePermutation);
+      }
+  }
+
+  /* ************************************************************************* */
+  void DiscreteFactorGraph::reduceWithInverse(
+    const internal::Reduction& inverseReduction) {
+      BOOST_FOREACH(const sharedFactor& factor, factors_) {
+        if(factor)
+          factor->reduceWithInverse(inverseReduction);
+      }
+  }
+
 	/* ************************************************************************* */
 	std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>  //
 	EliminateDiscrete(const FactorGraph<DiscreteFactor>& factors, size_t num) {

gtsam/discrete/DiscreteFactorGraph.h

@@ -79,6 +79,12 @@ public:
 	/// print
 	void print(const std::string& s = "DiscreteFactorGraph",
 			const IndexFormatter& formatter =DefaultIndexFormatter) const;
+
+  /** Permute the variables in the factors */
+  void permuteWithInverse(const Permutation& inversePermutation);
+
+  /** Apply a reduction, which is a remapping of variable indices. */
+  void reduceWithInverse(const internal::Reduction& inverseReduction);
 };
 // DiscreteFactorGraph
 

gtsam/discrete/Potentials.cpp

@@ -61,18 +61,19 @@ namespace gtsam {
 	}
 
   /* ************************************************************************* */
-	void Potentials::permuteWithInverse(const Permutation& permutation) {
+  template<class P>
+  void Potentials::remapIndices(const P& remapping) {
     // Permute the _cardinalities (TODO: Inefficient Consider Improving)
     DiscreteKeys keys;
     map<Index, Index> ordering;
 
-		// Get the orginal keys from cardinalities_
+    // Get the original keys from cardinalities_
     BOOST_FOREACH(const DiscreteKey& key, cardinalities_)
       keys & key;
 
     // Perform Permutation
     BOOST_FOREACH(DiscreteKey& key, keys) {
-			ordering[key.first] = permutation[key.first];
+      ordering[key.first] = remapping[key.first];
       key.first = ordering[key.first];
     }
 
@@ -83,5 +84,15 @@ namespace gtsam {
   }
 
 	/* ************************************************************************* */
+	void Potentials::permuteWithInverse(const Permutation& inversePermutation) {
+    remapIndices(inversePermutation);
+	}
+
+  /* ************************************************************************* */
+  void Potentials::reduceWithInverse(const internal::Reduction& inverseReduction) {
+    remapIndices(inverseReduction);
+  }
+
+	/* ************************************************************************* */
 
 } // namespace gtsam

gtsam/discrete/Potentials.h

@@ -49,6 +49,10 @@ namespace gtsam {
 		// Safe division for probabilities
 		static double safe_div(const double& a, const double& b);
 
+    // Apply either a permutation or a reduction
+    template<class P>
+    void remapIndices(const P& remapping);
+
 	public:
 
 		/** Default constructor for I/O */
@@ -79,6 +83,11 @@ namespace gtsam {
 		 */
 		virtual void permuteWithInverse(const Permutation& inversePermutation);
 
+    /**
+	   * Apply a reduction, which is a remapping of variable indices.
+	   */
+    virtual void reduceWithInverse(const internal::Reduction& inverseReduction);
+
 	}; // Potentials
 
 } // namespace gtsam

gtsam/inference/BayesTree-inl.h

@@ -498,7 +498,11 @@ namespace gtsam {
     sharedClique clique = (*this)[j];
 
     // calculate or retrieve its marginal P(C) = P(F,S)
+#ifdef MARGINAL2
     FactorGraph<FactorType> cliqueMarginal = clique->marginal2(root_,function);
+#else
+    FactorGraph<FactorType> cliqueMarginal = clique->marginal(root_,function);
+#endif
 
     // now, marginalize out everything that is not variable j
     GenericSequentialSolver<FactorType> solver(cliqueMarginal);

gtsam/inference/BayesTreeCliqueBase-inl.h

@@ -17,6 +17,7 @@
 #pragma once
 
 #include <gtsam/inference/GenericSequentialSolver.h>
+#include <boost/foreach.hpp>
 
 namespace gtsam {
 
@@ -53,8 +54,10 @@ namespace gtsam {
     std::vector<Index> &indicesB = B->conditional()->keys();
     std::vector<Index> S_setminus_B = separator_setminus_B(B); // TODO, get as argument?
     std::vector<Index> keep;
+    // keep = S\B intersect allKeys
     std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), //
         allKeys.begin(), allKeys.end(), back_inserter(keep));
+    // keep += B intersect allKeys
     std::set_intersection(indicesB.begin(), indicesB.end(), //
         allKeys.begin(), allKeys.end(), back_inserter(keep));
     // BOOST_FOREACH(Index j, keep) std::cout << j << " "; std::cout << std::endl;
@@ -190,18 +193,42 @@ namespace gtsam {
         // systems and exceptions are thrown. However, we should be able to omit
         // this if we can get ATTEMPT_AT_NOT_ELIMINATING_ALL in
         // GenericSequentialSolver.* working...
+#ifndef ATTEMPT_AT_NOT_ELIMINATING_ALL
         p_Cp_B.push_back(B->conditional()->toFactor()); // P(B)
+#endif
+
+        // Determine the variables we want to keepSet, S union B
+        std::vector<Index> keep = shortcut_indices(B, p_Cp_B);
+        //std::set<Index> keepSet;
+        //BOOST_FOREACH(Index j, this->conditional()->parents()) {
+        //  keepSet.insert(j); }
+        //BOOST_FOREACH(Index j, (*B->conditional())) {
+        //  keepSet.insert(j); }
+        //std::vector<Index> keep(keepSet.begin(), keepSet.end());
+
+        // Reduce the variable indices to start at zero
+        const Permutation reduction = internal::createReducingPermutation(p_Cp_B.keys());
+        internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
+        BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_Cp_B) {
+          if(factor) factor->reduceWithInverse(inverseReduction); }
+        inverseReduction.applyInverse(keep);
 
         // Create solver that will marginalize for us
         GenericSequentialSolver<FactorType> solver(p_Cp_B);
 
-        // Determine the variables we want to keep
-        std::vector<Index> keep = shortcut_indices(B, p_Cp_B);
-
         // Finally, we only want to have S\B variables in the Bayes net, so
         size_t nrFrontals = S_setminus_B.size();
         cachedShortcut_ = //
             *solver.conditionalBayesNet(keep, nrFrontals, function);
+
+        // Undo the reduction
+        BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, p_Cp_B) {
+          if (factor) {
+            factor->permuteWithInverse(reduction);
+          }
+        }
+        cachedShortcut_->permuteWithInverse(reduction);
+
         assertInvariants();
       } else {
         BayesNet<CONDITIONAL> empty;
@@ -264,7 +291,7 @@ namespace gtsam {
         // Create solver that will marginalize for us
         GenericSequentialSolver<FactorType> solver(p_Cp);
 
-        // The variables we want to keep are exactly the ones in S
+        // The variables we want to keepSet are exactly the ones in S
         sharedConditional p_F_S = this->conditional();
         std::vector<Index> indicesS(p_F_S->beginParents(), p_F_S->endParents());
 

gtsam/inference/EliminationTree-inl.h

@@ -41,6 +41,7 @@ typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::eliminat
 
     if(debug) std::cout << "ETree: eliminating " << this->key_ << std::endl;
 
+    if(this->key_ < conditionals.size()) { // If it is requested to eliminate the current variable
       // Create the list of factors to be eliminated, initially empty, and reserve space
       FactorGraph<FACTOR> factors;
       factors.reserve(this->factors_.size() + this->subTrees_.size());
@@ -63,6 +64,13 @@ typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::eliminat
       if(debug) eliminated.second->print("Factor: ");
 
       return eliminated.second;
+    } else {
+      // Eliminate each child but discard the result.
+      BOOST_FOREACH(const shared_ptr& child, subTrees_) {
+        (void)child->eliminate_(function, conditionals);
+      }
+      return sharedFactor(); // Return a NULL factor
+    }
 }
 
 /* ************************************************************************* */
@@ -138,6 +146,7 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
     if(derivedFactor) {
       sharedFactor factor(derivedFactor);
       Index j = *std::min_element(factor->begin(), factor->end());
+      if(j < structure.size())
         trees[j]->add(factor);
     }
   }
@@ -193,12 +202,13 @@ bool EliminationTree<FACTORGRAPH>::equals(const EliminationTree<FACTORGRAPH>& ex
 /* ************************************************************************* */
 template<class FACTOR>
 typename EliminationTree<FACTOR>::BayesNet::shared_ptr
-EliminationTree<FACTOR>::eliminate(Eliminate function) const {
+  EliminationTree<FACTOR>::eliminatePartial(typename EliminationTree<FACTOR>::Eliminate function, size_t nrToEliminate) const {
 
   // call recursive routine
   tic(1, "ET recursive eliminate");
-  size_t nrConditionals = this->key_ + 1;    // root key has highest index
+  if(nrToEliminate > this->key_ + 1)
-  Conditionals conditionals(nrConditionals); // reserve a vector of conditional shared pointers
+    throw std::invalid_argument("Requested that EliminationTree::eliminatePartial eliminate more variables than exist");
+  Conditionals conditionals(nrToEliminate); // reserve a vector of conditional shared pointers
   (void)eliminate_(function, conditionals);  // modify in place
   toc(1, "ET recursive eliminate");
 
@@ -214,4 +224,12 @@ EliminationTree<FACTOR>::eliminate(Eliminate function) const {
   return bayesNet;
 }
 
+/* ************************************************************************* */
+template<class FACTOR>
+typename EliminationTree<FACTOR>::BayesNet::shared_ptr
+EliminationTree<FACTOR>::eliminate(Eliminate function) const {
+  size_t nrConditionals = this->key_ + 1;    // root key has highest index
+  return eliminatePartial(function, nrConditionals);
+}
+
 }

gtsam/inference/EliminationTree.h

@@ -110,6 +110,13 @@ public:
    */
   typename BayesNet::shared_ptr eliminate(Eliminate function) const;
 
+  /** Eliminate the factors to a Bayes Net and return the remaining factor
+   * @param function The function to use to eliminate, see the namespace functions
+   * in GaussianFactorGraph.h
+   * @return The BayesNet resulting from elimination and the remaining factor
+   */
+  typename BayesNet::shared_ptr eliminatePartial(Eliminate function, size_t nrToEliminate) const;
+
 	/// @}
 	/// @name Testable
 	/// @{

gtsam/inference/GenericSequentialSolver-inl.h

@@ -89,11 +89,7 @@ namespace gtsam {
   template<class FACTOR>
   typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
   GenericSequentialSolver<FACTOR>::eliminate(const Permutation& permutation,
-      Eliminate function
+      Eliminate function, boost::optional<size_t> nrToEliminate) const {
-#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL
-      , boost::optional<size_t> nrToEliminate
-#endif
-      ) const {
 
     // Create inverse permutation
     Permutation::shared_ptr permutationInverse(permutation.inverse());
@@ -106,15 +102,12 @@ namespace gtsam {
         factor->permuteWithInverse(*permutationInverse);
 
     // Eliminate using elimination tree provided
-    typename EliminationTree<FACTOR>::shared_ptr etree;
+    typename EliminationTree<FACTOR>::shared_ptr etree = EliminationTree<FACTOR>::Create(*factors_);
-#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL
+    sharedBayesNet bayesNet;
-    if (nrToEliminate) {
+    if(nrToEliminate)
-      VariableIndex structure(*factors_, *nrToEliminate);
+      bayesNet = etree->eliminatePartial(function, *nrToEliminate);
-      etree = EliminationTree<FACTOR>::Create(*factors_, structure);
+    else
-    } else
+      bayesNet = etree->eliminate(function);
-#endif
-    etree = EliminationTree<FACTOR>::Create(*factors_);
-    sharedBayesNet bayesNet = etree->eliminate(function);
 
     // Undo the permutation on the original factors and on the structure.
     BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *factors_)
@@ -147,7 +140,7 @@ namespace gtsam {
     // my trick below (passing nrToEliminate to eliminate) sometimes leads
     // to a disconnected graph.
     // Eliminate only variables J \cup F from P(J,F,S) to get P(F|S)
-    size_t nrVariables = factors_->keys().size();// TODO expensive!
+    size_t nrVariables = structure_->size();
     size_t nrMarginalized = nrVariables - js.size();
     size_t nrToEliminate = nrMarginalized + nrFrontals;
     sharedBayesNet bayesNet = eliminate(*permutation, function, nrToEliminate);

gtsam/inference/GenericSequentialSolver.h

@@ -88,12 +88,8 @@ namespace gtsam {
     /**
      * Eliminate in a different order, given a permutation
      */
-    sharedBayesNet
+     sharedBayesNet eliminate(const Permutation& permutation, Eliminate function,
-    eliminate(const Permutation& permutation, Eliminate function
+     boost::optional<size_t> nrToEliminate = boost::none ///< If given a number of variables to eliminate, will only eliminate that many
-#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL
-        , boost::optional<size_t> nrToEliminate = boost::none
-        // If given a number of variables to eliminate, will only eliminate that many
-#endif
      ) const;
 
   public:

gtsam/inference/IndexFactor.cpp

@@ -61,5 +61,13 @@ namespace gtsam {
 			key = inversePermutation[key];
 		assertInvariants();
 	}
+
+	/* ************************************************************************* */
+  void IndexFactor::reduceWithInverse(const internal::Reduction& inverseReduction) {
+    BOOST_FOREACH(Index& key, keys())
+      key = inverseReduction[key];
+    assertInvariants();
+  }
+
   /* ************************************************************************* */
 } // gtsam

gtsam/inference/IndexFactor.h

@@ -156,6 +156,11 @@ namespace gtsam {
 	   */
 	  void permuteWithInverse(const Permutation& inversePermutation);
 
+	  /**
+	   * Apply a reduction, which is a remapping of variable indices.
+	   */
+    void reduceWithInverse(const internal::Reduction& inverseReduction);
+
 		virtual ~IndexFactor() {
 		}
 

gtsam/inference/Permutation.cpp

@@ -152,4 +152,64 @@ void Permutation::print(const std::string& str) const {
   std::cout << std::endl;
 }
 
+namespace internal {
+/* ************************************************************************* */
+  Reduction Reduction::CreateAsInverse(const Permutation& p) {
+    Reduction result;
+    for(Index j = 0; j < p.size(); ++j)
+      result.insert(std::make_pair(p[j], j));
+    return result;
+  }
+
+  /* ************************************************************************* */
+  void Reduction::applyInverse(std::vector<Index>& js) const {
+    BOOST_FOREACH(Index& j, js) {
+      j = this->find(j)->second;
+    }
+  }
+
+  /* ************************************************************************* */
+  Permutation Reduction::inverse() const {
+    Index maxIndex = 0;
+    BOOST_FOREACH(const value_type& target_source, *this) {
+      if(target_source.second > maxIndex)
+        maxIndex = target_source.second;
+    }
+    Permutation result(maxIndex + 1);
+    BOOST_FOREACH(const value_type& target_source, *this) {
+      result[target_source.second] = target_source.first;
+    }
+    return result;
+  }
+
+  /* ************************************************************************* */
+  Index& Reduction::operator[](const Index& j) {
+    iterator it = this->find(j);
+    if(it == this->end())
+      throw std::out_of_range("Index to Reduction::operator[] not present");
+    else
+      return it->second;
+  }
+
+  /* ************************************************************************* */
+  const Index& Reduction::operator[](const Index& j) const {
+    const_iterator it = this->find(j);
+    if(it == this->end())
+      throw std::out_of_range("Index to Reduction::operator[] not present");
+    else
+      return it->second;
+  }
+
+  /* ************************************************************************* */
+  Permutation createReducingPermutation(const std::set<Index>& indices) {
+    Permutation p(indices.size());
+    Index newJ = 0;
+    BOOST_FOREACH(Index j, indices) {
+      p[newJ] = j;
+      ++ newJ;
+    }
+    return p;
+  }
+}
+
 }

gtsam/inference/Permutation.h

@@ -19,10 +19,12 @@
 
 #include <vector>
 #include <string>
+#include <set>
 #include <iostream>
 #include <boost/shared_ptr.hpp>
 
 #include <gtsam/base/types.h>
+#include <gtsam/base/FastMap.h>
 
 namespace gtsam {
 
@@ -182,4 +184,20 @@ protected:
 	/// @}
 };
 
+
+namespace internal {
+  // An internal class used for storing and applying a permutation from a map
+  class Reduction : public gtsam::FastMap<Index,Index> {
+  public:
+    static Reduction CreateAsInverse(const Permutation& p);
+    void applyInverse(std::vector<Index>& js) const;
+    Permutation inverse() const;
+    Index& operator[](const Index& j);
+    const Index& operator[](const Index& j) const;
+  };
+
+  // Reduce the variable indices so that those in the set are mapped to start at zero
+  Permutation createReducingPermutation(const std::set<Index>& indices);
+}
+
 }

gtsam/inference/SymbolicFactorGraph.cpp

@@ -70,6 +70,24 @@ namespace gtsam {
 		return FactorGraph<IndexFactor>::eliminateFrontals(nFrontals, EliminateSymbolic);
 	}
 
+  /* ************************************************************************* */
+  void SymbolicFactorGraph::permuteWithInverse(
+    const Permutation& inversePermutation) {
+      BOOST_FOREACH(const sharedFactor& factor, factors_) {
+        if(factor)
+          factor->permuteWithInverse(inversePermutation);
+      }
+  }
+
+  /* ************************************************************************* */
+  void SymbolicFactorGraph::reduceWithInverse(
+    const internal::Reduction& inverseReduction) {
+      BOOST_FOREACH(const sharedFactor& factor, factors_) {
+        if(factor)
+          factor->reduceWithInverse(inverseReduction);
+      }
+  }
+
 	/* ************************************************************************* */
 	IndexFactor::shared_ptr CombineSymbolic(
 			const FactorGraph<IndexFactor>& factors, const FastMap<Index,

gtsam/inference/SymbolicFactorGraph.h

@@ -77,8 +77,6 @@ namespace gtsam {
 		 */
 		FastSet<Index> keys() const;
 
-
-
 		/// @}
 		/// @name Advanced Interface
 		/// @{
@@ -95,6 +93,12 @@ namespace gtsam {
 		/** Push back 4-way factor */
 		void push_factor(Index key1, Index key2, Index key3, Index key4);
 
+    /** Permute the variables in the factors */
+    void permuteWithInverse(const Permutation& inversePermutation);
+
+    /** Apply a reduction, which is a remapping of variable indices. */
+    void reduceWithInverse(const internal::Reduction& inverseReduction);
+
 	};
 
 	/** Create a combined joint factor (new style for EliminationTree). */

gtsam/inference/tests/testSymbolicBayesTree.cpp

@@ -30,9 +30,6 @@ using namespace gtsam;
 
 static bool debug = false;
 
-typedef BayesNet<IndexConditional> SymbolicBayesNet;
-typedef BayesTree<IndexConditional> SymbolicBayesTree;
-
 /* ************************************************************************* */
 
 TEST_UNSAFE( SymbolicBayesTree, thinTree ) {

gtsam/inference/tests/testSymbolicSequentialSolver.cpp

@@ -122,27 +122,6 @@ TEST( SymbolicSequentialSolver, inference ) {
   }
 }
 
-/* ************************************************************************* */
-// This test shows a problem with my (Frank) attempt at a faster conditionalBayesNet
-TEST( SymbolicSequentialSolver, problematicConditional ) {
-  // Create factor graph
-  SymbolicFactorGraph fg;
-  fg.push_factor(9, 12, 14);
-
-  // eliminate
-  SymbolicSequentialSolver solver(fg);
-  // conditionalBayesNet
-  vector<Index> js;
-  js.push_back(12);
-  js.push_back(14);
-  size_t nrFrontals = 1;
-  SymbolicBayesNet::shared_ptr actualBN = //
-      solver.conditionalBayesNet(js, nrFrontals);
-  SymbolicBayesNet expectedBN;
-  expectedBN.push_front(boost::make_shared<IndexConditional>(12,14));
-  EXPECT( assert_equal(expectedBN,*actualBN));
-}
-
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/linear/GaussianFactor.cpp

@@ -25,7 +25,6 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	GaussianFactor::GaussianFactor(const GaussianConditional& c) :
-		IndexFactor(c) {
+    IndexFactor(c) {}
-	}
 
 }

gtsam/linear/GaussianFactor.h

@@ -115,6 +115,13 @@ namespace gtsam {
     	IndexFactor::permuteWithInverse(inversePermutation);
     }
 
+	  /**
+	   * Apply a reduction, which is a remapping of variable indices.
+	   */
+    virtual void reduceWithInverse(const internal::Reduction& inverseReduction) {
+    	IndexFactor::reduceWithInverse(inverseReduction);
+    }
+
     /**
      * Construct the corresponding anti-factor to negate information
      * stored stored in this factor.

gtsam/linear/GaussianFactorGraph.cpp

@@ -63,6 +63,15 @@ namespace gtsam {
     }
 	}
 
+  /* ************************************************************************* */
+  void GaussianFactorGraph::reduceWithInverse(
+    const internal::Reduction& inverseReduction) {
+      BOOST_FOREACH(const sharedFactor& factor, factors_) {
+        if(factor)
+          factor->reduceWithInverse(inverseReduction);
+      }
+  }
+
 	/* ************************************************************************* */
 	void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg) {
 		for (const_iterator factor = lfg.factors_.begin(); factor

gtsam/linear/GaussianFactorGraph.h

@@ -127,6 +127,9 @@ namespace gtsam {
     /** Permute the variables in the factors */
     void permuteWithInverse(const Permutation& inversePermutation);
 
+    /** Apply a reduction, which is a remapping of variable indices. */
+    void reduceWithInverse(const internal::Reduction& inverseReduction);
+
     /** unnormalized error */
     double error(const VectorValues& x) const {
       double total_error = 0.;