diff --git a/.cproject b/.cproject
index ca640b7b1..cb05ca49e 100644
--- a/.cproject
+++ b/.cproject
@@ -990,6 +990,14 @@
true
true
+
+ make
+ -j5
+ testNonlinearFactorGraph.run
+ true
+ true
+ true
+
make
-j5
diff --git a/gtsam/inference/inference-inl.h b/gtsam/inference/inference-inl.h
index b24c2d6d2..c17f947d1 100644
--- a/gtsam/inference/inference-inl.h
+++ b/gtsam/inference/inference-inl.h
@@ -48,6 +48,22 @@ Permutation::shared_ptr PermutationCOLAMD(
return PermutationCOLAMD_(variableIndex, cmember);
}
+/* ************************************************************************* */
+template
+Permutation::shared_ptr PermutationCOLAMDGrouped(
+ const VariableIndex& variableIndex, const CONSTRAINED_MAP& constraints) {
+ std::vector 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 cmember(variableIndex.size(), 0);
diff --git a/gtsam/inference/inference.h b/gtsam/inference/inference.h
index 6ee278c68..7ba268789 100644
--- a/gtsam/inference/inference.h
+++ b/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
+ 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
diff --git a/gtsam/inference/tests/testInference.cpp b/gtsam/inference/tests/testInference.cpp
index 2523eb32b..b25703cb6 100644
--- a/gtsam/inference/tests/testInference.cpp
+++ b/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 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); }
/* ************************************************************************* */
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp
index d29e7282d..8c4076c56 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.cpp
+++ b/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& 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::value_type constraint_type;
+ std::map 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;
}
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index 29aa9ede1..1742c7b78 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/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& constraints) const;
+
/**
* linearize a nonlinear factor graph
*/
diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp
index 7d9160c1d..f44113f93 100644
--- a/tests/testNonlinearFactorGraph.cpp
+++ b/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 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); }
/* ************************************************************************* */