diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 5ae25d543..0882b87d1 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -29,242 +29,236 @@ using namespace std; namespace gtsam { - /* ************************************************************************* */ - FastMap Ordering::invert() const - { - FastMap inverted; - for(size_t pos = 0; pos < this->size(); ++pos) - inverted.insert(make_pair((*this)[pos], pos)); - return inverted; +/* ************************************************************************* */ +FastMap Ordering::invert() const { + FastMap inverted; + for (size_t pos = 0; pos < this->size(); ++pos) + inverted.insert(make_pair((*this)[pos], pos)); + return inverted; +} + +/* ************************************************************************* */ +Ordering Ordering::colamd(const VariableIndex& variableIndex) { + // Call constrained version with all groups set to zero + vector dummy_groups(variableIndex.size(), 0); + return Ordering::colamdConstrained(variableIndex, dummy_groups); +} + +/* ************************************************************************* */ +Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex, + std::vector& cmember) { + gttic(Ordering_COLAMDConstrained); + + gttic(Prepare); + size_t nEntries = variableIndex.nEntries(), nFactors = + variableIndex.nFactors(), nVars = variableIndex.size(); + // Convert to compressed column major format colamd wants it in (== MATLAB format!) + size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors, + (int) nVars); /* colamd arg 3: size of the array A */ + vector A = vector(Alen); /* colamd arg 4: row indices of A, of size Alen */ + vector p = vector(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */ + + // Fill in input data for COLAMD + p[0] = 0; + int count = 0; + vector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order + size_t index = 0; + BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) { + // Arrange factor indices into COLAMD format + const VariableIndex::Factors& column = key_factors.second; + size_t lastFactorId = numeric_limits::max(); + BOOST_FOREACH(size_t factorIndex, column) { + if (lastFactorId != numeric_limits::max()) + assert(factorIndex > lastFactorId); + A[count++] = (int) factorIndex; // copy sparse column + } + p[index + 1] = count; // column j (base 1) goes from A[j-1] to A[j]-1 + // Store key in array and increment index + keys[index] = key_factors.first; + ++index; } - /* ************************************************************************* */ - Ordering Ordering::colamd(const VariableIndex& variableIndex) - { - // Call constrained version with all groups set to zero - vector dummy_groups(variableIndex.size(), 0); - return Ordering::colamdConstrained(variableIndex, dummy_groups); + assert((size_t)count == variableIndex.nEntries()); + + //double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */ + double knobs[CCOLAMD_KNOBS]; + ccolamd_set_defaults(knobs); + knobs[CCOLAMD_DENSE_ROW] = -1; + knobs[CCOLAMD_DENSE_COL] = -1; + + int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */ + + gttoc(Prepare); + + // call colamd, result will be in p + /* returns (1) if successful, (0) otherwise*/ + if (nVars > 0) { + gttic(ccolamd); + int rv = ccolamd((int) nFactors, (int) nVars, (int) Alen, &A[0], &p[0], + knobs, stats, &cmember[0]); + if (rv != 1) + throw runtime_error( + (boost::format("ccolamd failed with return value %1%") % rv).str()); } - /* ************************************************************************* */ - Ordering Ordering::colamdConstrained( - const VariableIndex& variableIndex, std::vector& cmember) - { - gttic(Ordering_COLAMDConstrained); + // ccolamd_report(stats); - gttic(Prepare); - size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size(); - // Convert to compressed column major format colamd wants it in (== MATLAB format!) - size_t Alen = ccolamd_recommended((int)nEntries, (int)nFactors, (int)nVars); /* colamd arg 3: size of the array A */ - vector A = vector(Alen); /* colamd arg 4: row indices of A, of size Alen */ - vector p = vector(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */ + gttic(Fill_Ordering); + // Convert elimination ordering in p to an ordering + Ordering result; + result.resize(nVars); + for (size_t j = 0; j < nVars; ++j) + result[j] = keys[p[j]]; + gttoc(Fill_Ordering); - // Fill in input data for COLAMD - p[0] = 0; - int count = 0; - vector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order - size_t index = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) { - // Arrange factor indices into COLAMD format - const VariableIndex::Factors& column = key_factors.second; - size_t lastFactorId = numeric_limits::max(); - BOOST_FOREACH(size_t factorIndex, column) { - if(lastFactorId != numeric_limits::max()) - assert(factorIndex > lastFactorId); - A[count++] = (int)factorIndex; // copy sparse column - } - p[index+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1 - // Store key in array and increment index - keys[index] = key_factors.first; - ++ index; - } + return result; +} - assert((size_t)count == variableIndex.nEntries()); +/* ************************************************************************* */ +Ordering Ordering::colamdConstrainedLast(const VariableIndex& variableIndex, + const std::vector& constrainLast, bool forceOrder) { + gttic(Ordering_COLAMDConstrainedLast); - //double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */ - double knobs[CCOLAMD_KNOBS]; - ccolamd_set_defaults(knobs); - knobs[CCOLAMD_DENSE_ROW]=-1; - knobs[CCOLAMD_DENSE_COL]=-1; + size_t n = variableIndex.size(); + std::vector cmember(n, 0); - int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */ + // 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++)); - gttoc(Prepare); + // If at least some variables are not constrained to be last, constrain the + // ones that should be constrained. + int group = (constrainLast.size() != n ? 1 : 0); + BOOST_FOREACH(Key key, constrainLast) { + cmember[keyIndices.at(key)] = group; + if (forceOrder) + ++group; + } - // call colamd, result will be in p - /* returns (1) if successful, (0) otherwise*/ - if(nVars > 0) { - gttic(ccolamd); - int rv = ccolamd((int)nFactors, (int)nVars, (int)Alen, &A[0], &p[0], knobs, stats, &cmember[0]); - if(rv != 1) - throw runtime_error((boost::format("ccolamd failed with return value %1%")%rv).str()); - } + return Ordering::colamdConstrained(variableIndex, cmember); +} - // ccolamd_report(stats); +/* ************************************************************************* */ +Ordering Ordering::colamdConstrainedFirst(const VariableIndex& variableIndex, + const std::vector& constrainFirst, bool forceOrder) { + gttic(Ordering_COLAMDConstrainedFirst); - gttic(Fill_Ordering); - // Convert elimination ordering in p to an ordering - Ordering result; - result.resize(nVars); - for(size_t j = 0; j < nVars; ++j) - result[j] = keys[p[j]]; - gttoc(Fill_Ordering); + 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 && !constrainFirst.empty()) + ++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) { + 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); +} + +/* ************************************************************************* */ +Ordering Ordering::metis(const MetisIndex& met) { + gttic(Ordering_METIS); + + vector xadj = met.xadj(); + vector adj = met.adj(); + vector perm, iperm; + + idx_t size = met.nValues(); + for (idx_t i = 0; i < size; i++) { + perm.push_back(0); + iperm.push_back(0); + } + + int outputError; + + outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], + &iperm[0]); + Ordering result; + + if (outputError != METIS_OK) { + std::cout << "METIS failed during Nested Dissection ordering!\n"; return result; } - /* ************************************************************************* */ - Ordering Ordering::colamdConstrainedLast( - const VariableIndex& variableIndex, const std::vector& constrainLast, bool forceOrder) - { - gttic(Ordering_COLAMDConstrainedLast); - - 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++)); - - // If at least some variables are not constrained to be last, constrain the - // ones that should be constrained. - int group = (constrainLast.size() != n ? 1 : 0); - BOOST_FOREACH(Key key, constrainLast) { - cmember[keyIndices.at(key)] = group; - if(forceOrder) - ++ group; - } - - return Ordering::colamdConstrained(variableIndex, cmember); + result.resize(size); + for (size_t j = 0; j < (size_t) size; ++j) { + // We have to add the minKey value back to obtain the original key in the Values + result[j] = met.intToKey(perm[j]); } + return result; +} - /* ************************************************************************* */ - 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 && !constrainFirst.empty()) - ++ 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) - { - 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); - } - - - /* ************************************************************************* */ - Ordering Ordering::metis(const MetisIndex& met) - { - gttic(Ordering_METIS); - - vector xadj = met.xadj(); - vector adj = met.adj(); - vector perm, iperm; - - idx_t size = met.nValues(); - for (idx_t i = 0; i < size; i++) - { - perm.push_back(0); - iperm.push_back(0); - } - - int outputError; - - outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], &iperm[0]); - Ordering result; - - if (outputError != METIS_OK) - { - std::cout << "METIS failed during Nested Dissection ordering!\n"; - return result; - } - - result.resize(size); - for (size_t j = 0; j < (size_t)size; ++j){ - // We have to add the minKey value back to obtain the original key in the Values - result[j] = met.intToKey(perm[j]); - } - return result; - } - - /* ************************************************************************* */ - void Ordering::print(const std::string& str, const KeyFormatter& keyFormatter) const - { - cout << str; - // Print ordering in index order - // Print the ordering with varsPerLine ordering entries printed on each line, - // for compactness. - 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 << keyFormatter(at(i)); - if(i % varsPerLine == varsPerLine - 1) { - cout << "\n"; - endedOnNewline = true; - } else { - endedOnNewline = false; - } - } - if(!endedOnNewline) +/* ************************************************************************* */ +void Ordering::print(const std::string& str, + const KeyFormatter& keyFormatter) const { + cout << str; + // Print ordering in index order + // Print the ordering with varsPerLine ordering entries printed on each line, + // for compactness. + 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 << keyFormatter(at(i)); + if (i % varsPerLine == varsPerLine - 1) { cout << "\n"; - cout.flush(); + endedOnNewline = true; + } else { + endedOnNewline = false; + } } + if (!endedOnNewline) + cout << "\n"; + cout.flush(); +} - /* ************************************************************************* */ - bool Ordering::equals(const Ordering& other, double tol) const - { - return (*this) == other; - } +/* ************************************************************************* */ +bool Ordering::equals(const Ordering& other, double tol) const { + return (*this) == other; +} } diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 92f011c7d..e4cc2c55d 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -30,192 +30,210 @@ namespace gtsam { - class Ordering : public std::vector { - protected: - typedef std::vector Base; +class Ordering: public std::vector { +protected: + typedef std::vector Base; - public: +public: - /// Type of ordering to use - enum OrderingType { - COLAMD, METIS, CUSTOM - }; - - typedef Ordering This; ///< Typedef to this class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - - /// Create an empty ordering - GTSAM_EXPORT Ordering() {} - - /// Create from a container - template - explicit Ordering(const KEYS& keys) : Base(keys.begin(), keys.end()) {} - - /// Create an ordering using iterators over keys - template - Ordering(ITERATOR firstKey, ITERATOR lastKey) : Base(firstKey, lastKey) {} - - /// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling - /// push_back. - boost::assign::list_inserter > - operator+=(Key key) { - return boost::assign::make_list_inserter(boost::assign_detail::call_push_back(*this))(key); - } - - /// Invert (not reverse) the ordering - returns a map from key to order position - FastMap invert() const; - - /// @name Fill-reducing Orderings @{ - - /// 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)); } - - /// Compute a fill-reducing ordering using COLAMD from a VariableIndex. - static GTSAM_EXPORT Ordering colamd(const VariableIndex& variableIndex); - - /// 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 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 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 - /// 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); - - /// Return a natural Ordering. Typically used by iterative solvers - template - static Ordering Natural(const FactorGraph &fg) { - FastSet src = fg.keys(); - std::vector keys(src.begin(), src.end()); - std::stable_sort(keys.begin(), keys.end()); - return Ordering(keys); - } - - /// METIS Formatting function - template - static GTSAM_EXPORT void CSRFormat(std::vector& xadj, std::vector& adj, const FactorGraph& graph); - - /// Compute an ordering determined by METIS from a VariableIndex - static GTSAM_EXPORT Ordering metis(const MetisIndex& met); - - template - static Ordering metis(const FactorGraph& graph) - { - return metis(MetisIndex(graph)); - } - - /// @} - - /// @name Named Constructors @{ - - template - static Ordering Create(OrderingType orderingType, - const FactorGraph& graph) { - - switch (orderingType) { - case COLAMD: - return colamd(graph); - case METIS: - return metis(graph); - case CUSTOM: - throw std::runtime_error( - "Ordering::Create error: called with CUSTOM ordering type."); - default: - throw std::runtime_error( - "Ordering::Create error: called with unknown ordering type."); - } - } - - /// @} - - /// @name Testable @{ - - GTSAM_EXPORT void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - GTSAM_EXPORT bool equals(const Ordering& other, double tol = 1e-9) const; - - /// @} - - private: - /// Internal COLAMD function - static GTSAM_EXPORT Ordering colamdConstrained( - const VariableIndex& variableIndex, std::vector& cmember); - - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - } + /// Type of ordering to use + enum OrderingType { + COLAMD, METIS, CUSTOM }; - /// traits - template<> struct traits : public Testable {}; + typedef Ordering This; ///< Typedef to this class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + + /// Create an empty ordering + GTSAM_EXPORT + Ordering() { + } + + /// Create from a container + template + explicit Ordering(const KEYS& keys) : + Base(keys.begin(), keys.end()) { + } + + /// Create an ordering using iterators over keys + template + Ordering(ITERATOR firstKey, ITERATOR lastKey) : + Base(firstKey, lastKey) { + } + + /// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling + /// push_back. + boost::assign::list_inserter > operator+=( + Key key) { + return boost::assign::make_list_inserter( + boost::assign_detail::call_push_back(*this))(key); + } + + /// Invert (not reverse) the ordering - returns a map from key to order position + FastMap invert() const; + + /// @name Fill-reducing Orderings @{ + + /// 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)); + } + + /// Compute a fill-reducing ordering using COLAMD from a VariableIndex. + static GTSAM_EXPORT Ordering colamd(const VariableIndex& variableIndex); + + /// 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 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 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 + /// 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); + + /// Return a natural Ordering. Typically used by iterative solvers + template + static Ordering Natural(const FactorGraph &fg) { + FastSet src = fg.keys(); + std::vector keys(src.begin(), src.end()); + std::stable_sort(keys.begin(), keys.end()); + return Ordering(keys); + } + + /// METIS Formatting function + template + static GTSAM_EXPORT void CSRFormat(std::vector& xadj, + std::vector& adj, const FactorGraph& graph); + + /// Compute an ordering determined by METIS from a VariableIndex + static GTSAM_EXPORT Ordering metis(const MetisIndex& met); + + template + static Ordering metis(const FactorGraph& graph) { + return metis(MetisIndex(graph)); + } + + /// @} + + /// @name Named Constructors @{ + + template + static Ordering Create(OrderingType orderingType, + const FactorGraph& graph) { + + switch (orderingType) { + case COLAMD: + return colamd(graph); + case METIS: + return metis(graph); + case CUSTOM: + throw std::runtime_error( + "Ordering::Create error: called with CUSTOM ordering type."); + default: + throw std::runtime_error( + "Ordering::Create error: called with unknown ordering type."); + } + } + + /// @} + + /// @name Testable @{ + + GTSAM_EXPORT + void print(const std::string& str = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; + + GTSAM_EXPORT + bool equals(const Ordering& other, double tol = 1e-9) const; + + /// @} + +private: + /// Internal COLAMD function + static GTSAM_EXPORT Ordering colamdConstrained( + const VariableIndex& variableIndex, std::vector& cmember); + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; + +/// traits +template<> struct traits : public Testable { +}; } diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index ee8751f47..26efb65b2 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -56,7 +56,8 @@ TEST(Ordering, constrained_ordering) { EXPECT(assert_equal(expConstrained, actConstrained)); // constrained version - push one set to the start - Ordering actConstrained2 = Ordering::colamdConstrainedFirst(sfg, list_of(2)(4)); + 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)); } @@ -82,43 +83,41 @@ TEST(Ordering, grouped_constrained_ordering) { /* ************************************************************************* */ TEST(Ordering, csr_format) { - // Example in METIS manual - SymbolicFactorGraph sfg; - sfg.push_factor(0, 1); - sfg.push_factor(1, 2); - sfg.push_factor(2, 3); - sfg.push_factor(3, 4); - sfg.push_factor(5, 6); - sfg.push_factor(6, 7); - sfg.push_factor(7, 8); - sfg.push_factor(8, 9); - sfg.push_factor(10, 11); - sfg.push_factor(11, 12); - sfg.push_factor(12, 13); - sfg.push_factor(13, 14); + // Example in METIS manual + SymbolicFactorGraph sfg; + sfg.push_factor(0, 1); + sfg.push_factor(1, 2); + sfg.push_factor(2, 3); + sfg.push_factor(3, 4); + sfg.push_factor(5, 6); + sfg.push_factor(6, 7); + sfg.push_factor(7, 8); + sfg.push_factor(8, 9); + sfg.push_factor(10, 11); + sfg.push_factor(11, 12); + sfg.push_factor(12, 13); + sfg.push_factor(13, 14); - sfg.push_factor(0, 5); - sfg.push_factor(5, 10); - sfg.push_factor(1, 6); - sfg.push_factor(6, 11); - sfg.push_factor(2, 7); - sfg.push_factor(7, 12); - sfg.push_factor(3, 8); - sfg.push_factor(8, 13); - sfg.push_factor(4, 9); - sfg.push_factor(9, 14); + sfg.push_factor(0, 5); + sfg.push_factor(5, 10); + sfg.push_factor(1, 6); + sfg.push_factor(6, 11); + sfg.push_factor(2, 7); + sfg.push_factor(7, 12); + sfg.push_factor(3, 8); + sfg.push_factor(8, 13); + sfg.push_factor(4, 9); + sfg.push_factor(9, 14); - MetisIndex mi(sfg); + MetisIndex mi(sfg); - vector xadjExpected, adjExpected; - xadjExpected += 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44; - adjExpected += 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11, - 2, 6, 8, 12, 3, 7, 9, 13, 4, 8, 14, 5, 11, 6, 10, 12, 7, 11, - 13, 8, 12, 14, 9, 13 ; + vector xadjExpected, adjExpected; + xadjExpected += 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44; + adjExpected += 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11, 2, 6, 8, 12, 3, 7, 9, 13, 4, 8, 14, 5, 11, 6, 10, 12, 7, 11, 13, 8, 12, 14, 9, 13; - EXPECT(xadjExpected == mi.xadj()); - EXPECT(adjExpected.size() == mi.adj().size()); - EXPECT(adjExpected == mi.adj()); + EXPECT(xadjExpected == mi.xadj()); + EXPECT(adjExpected.size() == mi.adj().size()); + EXPECT(adjExpected == mi.adj()); } /* ************************************************************************* */ @@ -136,7 +135,7 @@ TEST(Ordering, csr_format_2) { vector xadjExpected, adjExpected; xadjExpected += 0, 1, 4, 6, 8, 10; - adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3; + adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3; EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); @@ -237,18 +236,18 @@ TEST(Ordering, MetisLoop) { SymbolicFactorGraph sfg = example::symbolicChain(); // add loop closure - sfg.push_factor(0,5); + sfg.push_factor(0, 5); // METIS { - Ordering actual = Ordering::Create(Ordering::METIS,sfg); - // 0,3 - // 1 - // 2 - // 4 - // 5 - Ordering expected = Ordering(list_of(5)(4)(2)(1)(0)(3)); - EXPECT(assert_equal(expected, actual)); + Ordering actual = Ordering::Create(Ordering::METIS, sfg); + // 0,3 + // 1,3 + // 2 + // 4,0 + // 5 + Ordering expected = Ordering(list_of(5)(4)(2)(1)(0)(3)); + EXPECT(assert_equal(expected, actual)); } } @@ -260,28 +259,31 @@ TEST(Ordering, Create) { // COLAMD { - Ordering actual = Ordering::Create(Ordering::COLAMD,sfg); - Ordering expected = Ordering(list_of(0)(1)(2)(3)(4)(5)); - EXPECT(assert_equal(expected, actual)); + Ordering actual = Ordering::Create(Ordering::COLAMD, sfg); + Ordering expected = Ordering(list_of(0)(1)(2)(3)(4)(5)); + EXPECT(assert_equal(expected, actual)); } // METIS { - Ordering actual = Ordering::Create(Ordering::METIS,sfg); - // 2 - // 0 - // 1 - // 4 - // 3 - // 5 - Ordering expected = Ordering(list_of(5)(3)(4)(1)(0)(2)); - EXPECT(assert_equal(expected, actual)); + Ordering actual = Ordering::Create(Ordering::METIS, sfg); + // 2 + // 0 + // 1 + // 4 + // 3 + // 5 + Ordering expected = Ordering(list_of(5)(3)(4)(1)(0)(2)); + EXPECT(assert_equal(expected, actual)); } // CUSTOM - CHECK_EXCEPTION(Ordering::Create(Ordering::CUSTOM,sfg),runtime_error); + CHECK_EXCEPTION(Ordering::Create(Ordering::CUSTOM, sfg), runtime_error); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */