Added Ordering::COLAMDConstrainedFirst function

gtsam/inference/Ordering.cpp

@@ -137,6 +137,40 @@ namespace gtsam {
     return Ordering::COLAMDConstrained(variableIndex, cmember);
   }
 
+  /* ************************************************************************* */
+  Ordering Ordering::COLAMDConstrainedFirst(
+    const VariableIndex& variableIndex, const std::vector<Key>& constrainFirst, bool forceOrder)
+  {
+    gttic(Ordering_COLAMDConstrainedFirst);
+
+    const int none = -1;
+    size_t n = variableIndex.size();
+    std::vector<int> cmember(n, none);
+
+    // 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++));
+
+    // 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<Key, int>& 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;

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<Key> \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<class FACTOR>
     static Ordering COLAMDConstrainedLast(const FactorGraph<FACTOR>& 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<Key>
-    /// \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<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&).  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 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<class FACTOR>
+    static Ordering COLAMDConstrainedFirst(const FactorGraph<FACTOR>& graph,
+      const std::vector<Key>& 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<Key> \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<Key>& 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

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));
 }
 
 /* ************************************************************************* */