From 8bd19e5ef8202e7c543c68f6ee81c719685cdd6e Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 6 Aug 2013 16:10:08 +0000 Subject: [PATCH] Added Ordering::COLAMDConstrainedFirst function --- gtsam/inference/Ordering.cpp | 38 +++++++++++++++++++++++++- gtsam/inference/Ordering.h | 27 ++++++++++++++++-- gtsam/inference/tests/testOrdering.cpp | 5 ++++ 3 files changed, 67 insertions(+), 3 deletions(-) diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index d2e1870c0..062a173d3 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -137,6 +137,40 @@ namespace gtsam { return Ordering::COLAMDConstrained(variableIndex, cmember); } + /* ************************************************************************* */ + Ordering Ordering::COLAMDConstrainedFirst( + const VariableIndex& variableIndex, const std::vector& constrainFirst, bool forceOrder) + { + gttic(Ordering_COLAMDConstrainedFirst); + + const int none = -1; + size_t n = variableIndex.size(); + std::vector cmember(n, none); + + // Build a mapping to look up sorted Key indices by Key + FastMap keyIndices; + size_t j = 0; + BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); + + // If at least some variables are not constrained to be last, constrain the + // ones that should be constrained. + int group = 0; + BOOST_FOREACH(Key key, constrainFirst) { + cmember[keyIndices.at(key)] = group; + if(forceOrder) + ++ group; + } + + if(!forceOrder) + ++ group; + BOOST_FOREACH(int& c, cmember) + if(c == none) + c = group; + + return Ordering::COLAMDConstrained(variableIndex, cmember); + } + /* ************************************************************************* */ Ordering Ordering::COLAMDConstrained(const VariableIndex& variableIndex, const FastMap& groups) @@ -171,9 +205,11 @@ namespace gtsam { static const size_t varsPerLine = 10; bool endedOnNewline = false; for(size_t i = 0; i < size(); ++i) { + if(i % varsPerLine == 0) + cout << "Position " << i << ": "; if(i % varsPerLine != 0) cout << ", "; - cout << i << ":" << keyFormatter(at(i)); + cout << keyFormatter(at(i)); if(i % varsPerLine == varsPerLine - 1) { cout << "\n"; endedOnNewline = true; diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index c6a9420bf..ea2ad7eda 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -72,7 +72,7 @@ namespace gtsam { /// the variables in \c constrainLast to the end of the ordering, and orders all other variables /// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c /// constrainLast will be ordered in the same order specified in the vector \c - /// constrainLast. If \c constrainLast is false, the variables in \c constrainLast will be + /// constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. template static Ordering COLAMDConstrainedLast(const FactorGraph& graph, @@ -83,11 +83,34 @@ namespace gtsam { /// function constrains the variables in \c constrainLast to the end of the ordering, and orders /// all other variables before in a fill-reducing ordering. If \c forceOrder is true, the /// variables in \c constrainLast will be ordered in the same order specified in the vector - /// \c constrainLast. If \c constrainLast is false, the variables in \c constrainLast will be + /// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. static GTSAM_EXPORT Ordering COLAMDConstrainedLast(const VariableIndex& variableIndex, const std::vector& constrainLast, bool forceOrder = false); + /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details + /// for note on performance). This internally builds a VariableIndex so if you already have a + /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains + /// the variables in \c constrainLast to the end of the ordering, and orders all other variables + /// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c + /// constrainLast will be ordered in the same order specified in the vector \c + /// constrainLast. If \c forceOrder is false, the variables in \c constrainFirst will be + /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. + template + static Ordering COLAMDConstrainedFirst(const FactorGraph& graph, + const std::vector& constrainFirst, bool forceOrder = false) { + return COLAMDConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); } + + /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This + /// function constrains the variables in \c constrainFirst to the front of the ordering, and + /// orders all other variables after in a fill-reducing ordering. If \c forceOrder is true, the + /// variables in \c constrainFirst will be ordered in the same order specified in the + /// vector \c constrainFirst. If \c forceOrder is false, the variables in \c + /// constrainFirst will be ordered after all the others, but will be rearranged by CCOLAMD to + /// reduce fill-in as well. + static GTSAM_EXPORT Ordering COLAMDConstrainedFirst(const VariableIndex& variableIndex, + const std::vector& constrainFirst, bool forceOrder = false); + /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details /// for note on performance). This internally builds a VariableIndex so if you already have a /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). In this function, a group diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 2b381277c..2443dba2a 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -46,6 +46,11 @@ TEST(Ordering, constrained_ordering) { Ordering actConstrained = Ordering::COLAMDConstrainedLast(sfg, list_of(2)(4)); Ordering expConstrained = Ordering(list_of(0)(1)(5)(3)(4)(2)); EXPECT(assert_equal(expConstrained, actConstrained)); + + // constrained version - push one set to the start + Ordering actConstrained2 = Ordering::COLAMDConstrainedFirst(sfg, list_of(2)(4)); + Ordering expConstrained2 = Ordering(list_of(2)(4)(0)(1)(3)(5)); + EXPECT(assert_equal(expConstrained2, actConstrained2)); } /* ************************************************************************* */