added a variant of getOrdering

cpp/FactorGraph-inl.h

@@ -216,7 +216,7 @@ void colamd(int n_col, int n_row, int nrNonZeros, const map<Key, vector<int> >&
 
 /* ************************************************************************* */
 template<class Factor>
-void FactorGraph<Factor>::getOrdering(Ordering& ordering) const{
+void FactorGraph<Factor>::getOrdering(Ordering& ordering, boost::optional<const set<Symbol>&> interested) const{
 
 	// A factor graph is really laid out in row-major format, each factor a row
 	// Below, we compute a symbolic matrix stored in sparse columns.
@@ -225,11 +225,16 @@ void FactorGraph<Factor>::getOrdering(Ordering& ordering) const{
 	int n_row = factors_.size();    /* colamd arg 1: number of rows in A */
 
 	// loop over all factors = rows
+	bool hasInterested = interested.is_initialized();
 	for (int i = 0; i < n_row; i++) {
 		if (factors_[i]==NULL) continue;
 		list<Symbol> keys = factors_[i]->keys();
-		BOOST_FOREACH(const Symbol& key, keys) columns[key].push_back(i);
-		nrNonZeros+= keys.size();
+		BOOST_FOREACH(const Symbol& key, keys) {
+			if (!hasInterested || interested->find(key) != interested->end()) {
+				columns[key].push_back(i);
+				nrNonZeros++;
+			}
+		}
 	}
 	int n_col = (int)(columns.size()); /* colamd arg 2: number of columns in A */
 	if(n_col != 0)
@@ -253,6 +258,14 @@ Ordering FactorGraph<Factor>::getOrdering() const {
 	return ordering;
 }
 
+/* ************************************************************************* */
+template<class Factor>
+Ordering FactorGraph<Factor>::getOrdering(const set<Symbol>& interested) const {
+	Ordering ordering;
+	getOrdering(ordering, interested);
+	return ordering;
+}
+
 /* ************************************************************************* */
 /** O(1)                                                                     */
 /* ************************************************************************* */

cpp/FactorGraph.h

@@ -106,8 +106,9 @@ namespace gtsam {
 		/**
 		 * Compute colamd ordering, including I/O and shared pointer version
 		 */
-		void getOrdering(Ordering& ordering) const;
+		void getOrdering(Ordering& ordering, boost::optional<const std::set<Symbol>&> interested = boost::none) const;
 		Ordering getOrdering() const;
+		Ordering getOrdering(const std::set<Symbol>& interested) const;
 		boost::shared_ptr<Ordering>  getOrdering_() const;
 
     /**

cpp/testGaussianFactorGraph.cpp

@@ -11,6 +11,7 @@ using namespace std;
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
 #include <boost/assign/std/list.hpp> // for operator +=
+#include <boost/assign/std/set.hpp> // for operator +=
 #include <boost/assign/std/vector.hpp> // for operator +=
 using namespace boost::assign;
 
@@ -432,6 +433,16 @@ TEST( GaussianFactorGraph, getOrdering)
   CHECK(assert_equal(expected,actual));
 }
 
+TEST( GaussianFactorGraph, getOrdering2)
+{
+  Ordering expected;
+  expected += "l1","x1";
+  GaussianFactorGraph fg = createGaussianFactorGraph();
+  set<Symbol> interested; interested += "l1","x1";
+  Ordering actual = fg.getOrdering(interested);
+  CHECK(assert_equal(expected,actual));
+}
+
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, optimize )
 {