diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 9fe5a6105..08f755b5a 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -137,4 +137,28 @@ namespace gtsam { return Ordering::COLAMDConstrained(variableIndex, cmember); } + /* ************************************************************************* */ + Ordering Ordering::COLAMDConstrained(const VariableIndex& variableIndex, + const FastMap& groups) + { + gttic(Ordering_COLAMDConstrained); + size_t n = variableIndex.size(); + std::vector cmember(n, 0); + + // 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++)); + + // Assign groups + typedef FastMap::value_type key_group; + BOOST_FOREACH(const key_group& p, groups) { + // FIXME: check that no groups are skipped + cmember[keyIndices.at(p.first)] = p.second; + } + + return Ordering::COLAMDConstrained(variableIndex, cmember); + } + } diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index c737c2743..66fce476a 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -43,23 +43,65 @@ namespace gtsam { /// Invert (not reverse) the ordering - returns a map from key to order position FastMap invert() const; - /// Compute an ordering using COLAMD directly from a factor graph - this internally builds a - /// VariableIndex so if you already have a VariableIndex, it is faster to use COLAMD(const - /// VariableIndex&) + /// Compute a fill-reducing ordering using 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&) template static Ordering COLAMD(const FactorGraph& graph) { - return COLAMD(VariableIndex(graph)); - } + return COLAMD(VariableIndex(graph)); } + /// Compute a fill-reducing ordering using COLAMD from a VariableIndex. static GTSAM_EXPORT Ordering COLAMD(const VariableIndex& variableIndex); - 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 constrainLast 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, + const std::vector& constrainLast, bool forceOrder = false) { + return COLAMDConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); } + + /// Compute a fill-reducing ordering using constrained COLAMD from a 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 constrainLast 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&). In this function, a group + /// for each variable should be specified in \c groups, and each group of variables will appear + /// in the ordering in group index order. \c groups should be a map from Key to group index. + /// The group indices used should be consecutive starting at 0, but may appear in \c groups in + /// arbitrary order. Any variables not present in \c groups will be assigned to group 0. This + /// function simply fills the \c cmember argument to CCOLAMD with the supplied indices, see the + /// CCOLAMD documentation for more information. + template + static Ordering COLAMDConstrained(const FactorGraph& graph, + const FastMap& groups) { + return COLAMDConstrained(VariableIndex(graph), groups); } + + /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. In this + /// function, a group for each variable should be specified in \c groups, and each group of + /// variables will appear in the ordering in group index order. \c groups should be a map from + /// Key to group index. The group indices used should be consecutive starting at 0, but may + /// appear in \c groups in arbitrary order. Any variables not present in \c groups will be + /// assigned to group 0. This function simply fills the \c cmember argument to CCOLAMD with the + /// supplied indices, see the CCOLAMD documentation for more information. + static GTSAM_EXPORT Ordering COLAMDConstrained(const VariableIndex& variableIndex, + const FastMap& groups); private: static GTSAM_EXPORT Ordering COLAMDConstrained( - const VariableIndex& variableIndex, - std::vector& cmember); + const VariableIndex& variableIndex, std::vector& cmember); }; }