Added mechanism to create a constrained ordering directly from a NonlinearFactorGraph

.cproject

@@ -990,6 +990,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testNonlinearFactorGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testNonlinearFactorGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="clean" path="build/wrap/gtsam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>

gtsam/inference/inference-inl.h

@@ -48,6 +48,22 @@ Permutation::shared_ptr PermutationCOLAMD(
   return PermutationCOLAMD_(variableIndex, cmember);
 }
 
+/* ************************************************************************* */
+template<typename CONSTRAINED_MAP>
+Permutation::shared_ptr PermutationCOLAMDGrouped(
+		const VariableIndex& variableIndex, const CONSTRAINED_MAP& constraints) {
+  std::vector<int> cmember(variableIndex.size(), 0);
+
+	typedef typename CONSTRAINED_MAP::value_type constraint_pair;
+  BOOST_FOREACH(const constraint_pair& p, constraints) {
+  	assert(p.first < variableIndex.size());
+  	// FIXME: check that no groups are skipped
+  	cmember[p.first] = p.second;
+  }
+
+  return PermutationCOLAMD_(variableIndex, cmember);
+}
+
 /* ************************************************************************* */
 inline Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex) {
   std::vector<int> cmember(variableIndex.size(), 0);

gtsam/inference/inference.h

@@ -48,8 +48,21 @@ namespace gtsam {
 		Permutation::shared_ptr PermutationCOLAMD(
 				const VariableIndex& variableIndex, const CONSTRAINED& constrainLast);
 
+		/**
+		 * Compute a permutation of variable ordering using constrained colamd to
+		 * move variables to the end in groups (0 = unconstrained, higher numbers at
+		 * the end).
+		 * @param variableIndex is the variable index lookup from a graph
+		 * @param constraintMap is a map from variable index -> group number for constrained variables
+		 * @tparam CONSTRAINED_MAP is an associative structure (like std::map), from size_t->int
+		 */
+		template<typename CONSTRAINED_MAP>
+		Permutation::shared_ptr PermutationCOLAMDGrouped(
+				const VariableIndex& variableIndex, const CONSTRAINED_MAP& constraints);
+
 		/**
 		 * Compute a CCOLAMD permutation using the constraint groups in cmember.
+		 * The format for cmember is a part of ccolamd.
 		 *
 		 * @param variableIndex is the variable structure from a graph
 		 * @param cmember is the constraint group list for each variable, where

gtsam/inference/tests/testInference.cpp

@@ -81,6 +81,38 @@ TEST(inference, constrained_ordering) {
   EXPECT(assert_equal(expConstrained, *actConstrained));
 }
 
+/* ************************************************************************* */
+TEST(inference, grouped_constrained_ordering) {
+  SymbolicFactorGraph sfg;
+
+  // create graph with constrained groups:
+  // 1: 2, 4
+  // 2: 5
+  sfg.push_factor(0,1);
+  sfg.push_factor(1,2);
+  sfg.push_factor(2,3);
+  sfg.push_factor(3,4);
+  sfg.push_factor(4,5);
+
+  VariableIndex variableIndex(sfg);
+
+  // constrained version - push one set to the end
+  FastMap<size_t, int> constraints;
+  constraints[2] = 1;
+  constraints[4] = 1;
+  constraints[5] = 2;
+
+  Permutation::shared_ptr actConstrained(inference::PermutationCOLAMDGrouped(variableIndex, constraints));
+  Permutation expConstrained(6);
+  expConstrained[0] = 0;
+  expConstrained[1] = 1;
+  expConstrained[2] = 3;
+  expConstrained[3] = 2;
+  expConstrained[4] = 4;
+  expConstrained[5] = 5;
+  EXPECT(assert_equal(expConstrained, *actConstrained));
+}
+
 /* ************************************************************************* */
 int main() {  TestResult tr;  return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -83,9 +83,34 @@ Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
 
 	// Permute the Ordering with the COLAMD ordering
 	ordering->permuteWithInverse(*colamdPerm->inverse());
+	return ordering;
+}
 
-	// Return the Ordering and VariableIndex to be re-used during linearization
-	// and elimination
+/* ************************************************************************* */
+Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMDConstrained(const Values& config,
+		const std::map<Key, int>& constraints) const {
+	// Create symbolic graph and initial (iterator) ordering
+	SymbolicFactorGraph::shared_ptr symbolic;
+	Ordering::shared_ptr ordering;
+	boost::tie(symbolic, ordering) = this->symbolic(config);
+
+	// Compute the VariableIndex (column-wise index)
+	VariableIndex variableIndex(*symbolic, ordering->size());
+	if (config.size() != variableIndex.size()) throw std::runtime_error(
+			"orderingCOLAMD: some variables in the graph are not constrained!");
+
+	// Create a constraint list with correct indices
+	typedef std::map<Key, int>::value_type constraint_type;
+	std::map<Index, int> index_constraints;
+	BOOST_FOREACH(const constraint_type& p, constraints)
+		index_constraints[ordering->at(p.first)] = p.second;
+
+	// Compute a constrained fill-reducing ordering with COLAMD
+	Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMDGrouped(
+			variableIndex, index_constraints));
+
+	// Permute the Ordering with the COLAMD ordering
+	ordering->permuteWithInverse(*colamdPerm->inverse());
 	return ordering;
 }
 

gtsam/nonlinear/NonlinearFactorGraph.h

@@ -75,12 +75,21 @@ namespace gtsam {
 		symbolic(const Values& config) const;
 
     /**
-     * Compute a fill-reducing ordering using COLAMD.  This returns the
-     * ordering and a VariableIndex, which can later be re-used to save
-     * computation.
+     * Compute a fill-reducing ordering using COLAMD.
      */
 		Ordering::shared_ptr orderingCOLAMD(const Values& config) const;
 
+		/**
+		 * Compute a fill-reducing ordering with constraints using CCOLAMD
+		 *
+		 * @param constraints is a map of Key->group, where 0 is unconstrained, and higher
+		 * group numbers are further back in the ordering. Only keys with nonzero group
+		 * indices need to appear in the constraints, unconstrained is assumed for all
+		 * other variables
+		 */
+	  Ordering::shared_ptr orderingCOLAMDConstrained(const Values& config,
+	  		const std::map<Key, int>& constraints) const;
+
 		/**
 		 * linearize a nonlinear factor graph
 		 */

tests/testNonlinearFactorGraph.cpp

@@ -83,7 +83,14 @@ TEST( Graph, GET_ORDERING)
   Ordering::shared_ptr ordering;
   boost::tie(symbolic, ordering) = nlfg.symbolic(createNoisyValues());
   Ordering actual = *nlfg.orderingCOLAMD(createNoisyValues());
-  CHECK(assert_equal(expected,actual));
+  EXPECT(assert_equal(expected,actual));
+
+  // Constrained ordering - put x2 at the end
+  std::map<Key, int> constraints;
+  constraints[X(2)] = 1;
+  Ordering actualConstrained = *nlfg.orderingCOLAMDConstrained(createNoisyValues(), constraints);
+  Ordering expectedConstrained; expectedConstrained += L(1), X(1), X(2);
+  EXPECT(assert_equal(expectedConstrained, actualConstrained));
 }
 
 /* ************************************************************************* */
@@ -146,8 +153,5 @@ TEST( Graph, rekey )
 }
 
 /* ************************************************************************* */
-int main() {
-	TestResult tr;
-	return TestRegistry::runAllTests(tr);
-}
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */