From 8789201822e8e3431112493fe0033f1114bbc764 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 20 Jun 2012 01:35:42 +0000 Subject: [PATCH] Added mechanism to create a constrained ordering directly from a NonlinearFactorGraph --- .cproject | 8 ++++++ gtsam/inference/inference-inl.h | 16 ++++++++++++ gtsam/inference/inference.h | 13 ++++++++++ gtsam/inference/tests/testInference.cpp | 32 ++++++++++++++++++++++++ gtsam/nonlinear/NonlinearFactorGraph.cpp | 29 +++++++++++++++++++-- gtsam/nonlinear/NonlinearFactorGraph.h | 15 ++++++++--- tests/testNonlinearFactorGraph.cpp | 14 +++++++---- 7 files changed, 117 insertions(+), 10 deletions(-) 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); } /* ************************************************************************* */