Added Ordering::COLAMDConstrained and more detailed comments on other COLAMD functions

gtsam/inference/Ordering.cpp

@@ -137,4 +137,28 @@ namespace gtsam {
     return Ordering::COLAMDConstrained(variableIndex, cmember);
   }
 
+  /* ************************************************************************* */
+  Ordering Ordering::COLAMDConstrained(const VariableIndex& variableIndex,
+    const FastMap<Key, int>& groups)
+  {
+    gttic(Ordering_COLAMDConstrained);
+    size_t n = variableIndex.size();
+    std::vector<int> cmember(n, 0);
+
+    // Build a mapping to look up sorted Key indices by Key
+    FastMap<Key, size_t> 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<Key, int>::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);
+  }
+
 }

gtsam/inference/Ordering.h

@@ -43,23 +43,65 @@ namespace gtsam {
     /// Invert (not reverse) the ordering - returns a map from key to order position
     FastMap<Key, size_t> invert() const;
 
-    /// Compute an ordering using COLAMD directly from a factor graph - this internally builds a
+    /// Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on
-    /// VariableIndex so if you already have a VariableIndex, it is faster to use COLAMD(const
+    /// performance). This internally builds a VariableIndex so if you already have a VariableIndex,
-    /// VariableIndex&)
+    /// it is faster to use COLAMD(const VariableIndex&)
     template<class FACTOR>
     static Ordering COLAMD(const FactorGraph<FACTOR>& 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(
+    /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
-      const VariableIndex& variableIndex, const std::vector<Key>& constrainLast, bool forceOrder = false);
+    /// 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<Key> \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<class FACTOR>
+    static Ordering COLAMDConstrainedLast(const FactorGraph<FACTOR>& graph,
+      const std::vector<Key>& 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<Key>
+    /// \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<Key>& 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<class FACTOR>
+    static Ordering COLAMDConstrained(const FactorGraph<FACTOR>& graph,
+      const FastMap<Key, int>& 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<Key, int>& groups);
 
   private:
     static GTSAM_EXPORT Ordering COLAMDConstrained(
-      const VariableIndex& variableIndex,
+      const VariableIndex& variableIndex, std::vector<int>& cmember);
-      std::vector<int>& cmember);
   };
 }