Added eliminateFrontals function to FactorGraph, SymbolicFactorGraph, and GaussianFactorGraph - eliminates requested number of frontal variables and returns the resulting conditional and remaining factor graph

gtsam.h

@@ -787,6 +787,8 @@ class SymbolicFactorGraph {
   // Standard interface
   // FIXME: Must wrap FastSet<Index> for this to work
   //FastSet<Index> keys() const;
+
+	pair<gtsam::IndexConditional*, gtsam::SymbolicFactorGraph> eliminateFrontals(size_t nFrontals) const;
 };
 
 #include <gtsam/inference/SymbolicSequentialSolver.h>
@@ -996,6 +998,9 @@ class GaussianFactorGraph {
 	size_t size() const;
 	gtsam::GaussianFactor* at(size_t idx) const;
 
+	// Inference
+	pair<gtsam::GaussianConditional*, gtsam::GaussianFactorGraph> eliminateFrontals(size_t nFrontals) const;
+
 	// Building the graph
 	void push_back(gtsam::GaussianFactor* factor);
 	void add(Vector b);

gtsam/inference/FactorGraph-inl.h

@@ -23,6 +23,7 @@
 #pragma once
 
 #include <gtsam/inference/BayesTree.h>
+#include <gtsam/inference/VariableIndex.h>
 
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
@@ -85,6 +86,57 @@ namespace gtsam {
 		return size_;
 	}
 
+	/* ************************************************************************* */
+	template<class FACTOR>
+	std::pair<typename FactorGraph<FACTOR>::sharedConditional, FactorGraph<FACTOR> >
+		FactorGraph<FACTOR>::eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const
+	{
+		// Build variable index
+		VariableIndex variableIndex(*this);
+
+		// Find first variable
+		Index firstIndex = 0;
+		while(firstIndex < variableIndex.size() && variableIndex[firstIndex].empty())
+			++ firstIndex;
+
+		// Check that number of variables is in bounds
+		if(firstIndex + nFrontals >= variableIndex.size())
+			throw std::invalid_argument("Requested to eliminate more frontal variables than exist in the factor graph.");
+
+		// Get set of involved factors
+		FastSet<size_t> involvedFactorIs;
+		for(Index j = firstIndex; j < firstIndex + nFrontals; ++j) {
+			BOOST_FOREACH(size_t i, variableIndex[j]) {
+				involvedFactorIs.insert(i);
+			}
+		}
+
+		// Separate factors into involved and remaining
+		FactorGraph<FactorType> involvedFactors;
+		FactorGraph<FactorType> remainingFactors;
+		FastSet<size_t>::const_iterator involvedFactorIsIt = involvedFactorIs.begin();
+		for(size_t i = 0; i < this->size(); ++i) {
+			if(*involvedFactorIsIt == i) {
+				// If the current factor is involved, add it to involved and increment involved iterator
+				involvedFactors.push_back((*this)[i]);
+				++ involvedFactorIsIt;
+			} else {
+				// If not involved, add to remaining
+				remainingFactors.push_back((*this)[i]);
+			}
+		}
+
+		// Do dense elimination on the involved factors
+		typename FactorGraph<FactorType>::EliminationResult eliminationResult =
+			eliminate(involvedFactors, nFrontals);
+
+		// Add the remaining factor back into the factor graph
+		remainingFactors.push_back(eliminationResult.second);
+
+		// Return the eliminated factor and remaining factor graph
+		return std::make_pair(eliminationResult.first, remainingFactors);
+	}
+
 	/* ************************************************************************* */
 	template<class FACTOR>
 	void FactorGraph<FACTOR>::replace(size_t index, sharedFactor factor) {

gtsam/inference/FactorGraph.h

@@ -175,6 +175,13 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
 		/** Get the last factor */
 		sharedFactor back() const { return factors_.back(); }
 
+		/** Eliminate the first \c n frontal variables, returning the resulting
+		 * conditional and remaining factor graph - this is very inefficient for
+		 * eliminating all variables, to do that use EliminationTree or
+		 * JunctionTree.
+		 */
+		std::pair<sharedConditional, FactorGraph<FactorType> > eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const;
+
 		/// @}
 		/// @name Modifying Factor Graphs (imperative, discouraged)
 		/// @{

gtsam/inference/SymbolicFactorGraph.cpp

@@ -63,6 +63,13 @@ namespace gtsam {
     return keys;
   }
 
+	/* ************************************************************************* */
+	std::pair<SymbolicFactorGraph::sharedConditional, SymbolicFactorGraph>
+		SymbolicFactorGraph::eliminateFrontals(size_t nFrontals) const
+	{
+		return FactorGraph<IndexFactor>::eliminateFrontals(nFrontals, EliminateSymbolic);
+	}
+
 	/* ************************************************************************* */
 	IndexFactor::shared_ptr CombineSymbolic(
 			const FactorGraph<IndexFactor>& factors, const FastMap<Index,

gtsam/inference/SymbolicFactorGraph.h

@@ -57,6 +57,15 @@ namespace gtsam {
 		 */
 		template<class FACTOR>
 		SymbolicFactorGraph(const FactorGraph<FACTOR>& fg);
+		
+		/** Eliminate the first \c n frontal variables, returning the resulting
+		 * conditional and remaining factor graph - this is very inefficient for
+		 * eliminating all variables, to do that use EliminationTree or
+		 * JunctionTree.  Note that this version simply calls
+		 * FactorGraph<IndexFactor>::eliminateFrontals with EliminateSymbolic
+		 * as the eliminate function argument.
+		 */
+		std::pair<sharedConditional, SymbolicFactorGraph> eliminateFrontals(size_t nFrontals) const;
 
 		/// @}
 		/// @name Standard Interface
@@ -68,6 +77,8 @@ namespace gtsam {
 		 */
 		FastSet<Index> keys() const;
 
+
+
 		/// @}
 		/// @name Advanced Interface
 		/// @{
@@ -87,9 +98,8 @@ namespace gtsam {
 	};
 
 	/** Create a combined joint factor (new style for EliminationTree). */
-	IndexFactor::shared_ptr CombineSymbolic(
-			const FactorGraph<IndexFactor>& factors, const FastMap<Index,
-					std::vector<Index> >& variableSlots);
+	IndexFactor::shared_ptr CombineSymbolic(const FactorGraph<IndexFactor>& factors,
+		const FastMap<Index, std::vector<Index> >& variableSlots);
 
 	/**
 	 * CombineAndEliminate provides symbolic elimination.

gtsam/inference/tests/testFactorGraph.cpp

@@ -21,16 +21,44 @@
 #include <boost/shared_ptr.hpp>
 #include <boost/tuple/tuple.hpp>
 #include <boost/assign/std/set.hpp> // for operator +=
+#include <boost/assign/std/vector.hpp>
 using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <gtsam/inference/IndexConditional.h>
 #include <gtsam/inference/SymbolicFactorGraph.h>
 
 using namespace std;
 using namespace gtsam;
 
-typedef boost::shared_ptr<SymbolicFactorGraph> shared;
+/* ************************************************************************* */
+TEST(FactorGraph, eliminateFrontals) {
+
+	SymbolicFactorGraph sfgOrig;
+	sfgOrig.push_factor(0,1);
+	sfgOrig.push_factor(0,2);
+	sfgOrig.push_factor(1,3);
+	sfgOrig.push_factor(1,4);
+	sfgOrig.push_factor(2,3);
+	sfgOrig.push_factor(4,5);
+
+	IndexConditional::shared_ptr actualCond;
+	SymbolicFactorGraph actualSfg;
+	boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2, EliminateSymbolic);
+
+	vector<Index> condIndices;
+	condIndices += 0,1,2,3,4;
+	IndexConditional expectedCond(condIndices, 2);
+
+	SymbolicFactorGraph expectedSfg;
+	expectedSfg.push_factor(2,3);
+	expectedSfg.push_factor(4,5);
+	expectedSfg.push_factor(2,3,4);
+
+	EXPECT(assert_equal(expectedSfg, actualSfg));
+	EXPECT(assert_equal(expectedCond, *actualCond));
+}
 
 ///* ************************************************************************* */
 // SL-FIX TEST( FactorGraph, splitMinimumSpanningTree )

gtsam/linear/GaussianFactorGraph.cpp

@@ -47,6 +47,13 @@ namespace gtsam {
 		return keys;
 	}
 
+	/* ************************************************************************* */
+	std::pair<GaussianFactorGraph::sharedConditional, GaussianFactorGraph>
+		GaussianFactorGraph::eliminateFrontals(size_t nFrontals) const
+	{
+		return FactorGraph<GaussianFactor>::eliminateFrontals(nFrontals, EliminateQR);
+	}
+
 	/* ************************************************************************* */
 	void GaussianFactorGraph::permuteWithInverse(
 	    const Permutation& inversePermutation) {

gtsam/linear/GaussianFactorGraph.h

@@ -132,6 +132,16 @@ namespace gtsam {
     typedef FastSet<Index> Keys;
     Keys keys() const;
 
+				
+		/** Eliminate the first \c n frontal variables, returning the resulting
+		 * conditional and remaining factor graph - this is very inefficient for
+		 * eliminating all variables, to do that use EliminationTree or
+		 * JunctionTree.  Note that this version simply calls
+		 * FactorGraph<GaussianFactor>::eliminateFrontals with EliminateQR as the
+		 * eliminate function argument.
+		 */
+		std::pair<sharedConditional, GaussianFactorGraph> eliminateFrontals(size_t nFrontals) const;
+
     /** Permute the variables in the factors */
     void permuteWithInverse(const Permutation& inversePermutation);