Merged in feature/betterOrdering (pull request #100)

Fix Ordering

examples/SFMExample_bal_COLAMD_METIS.cpp

@@ -0,0 +1,144 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    SFMExample.cpp
+ * @brief   This file is to compare the ordering performance for COLAMD vs METIS.
+ * Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file.
+ * @author  Frank Dellaert, Zhaoyang Lv
+ */
+
+// For an explanation of headers, see SFMExample.cpp
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/inference/Ordering.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/GeneralSFMFactor.h>
+#include <gtsam/slam/dataset.h> // for loading BAL datasets !
+
+#include <gtsam/base/timing.h>
+
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+using symbol_shorthand::C;
+using symbol_shorthand::P;
+
+// We will be using a projection factor that ties a SFM_Camera to a 3D point.
+// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
+// and has a total of 9 free parameters
+typedef GeneralSFMFactor<SfM_Camera,Point3> MyFactor;
+
+/* ************************************************************************* */
+int main (int argc, char* argv[]) {
+
+  // Find default file, but if an argument is given, try loading a file
+  string filename = findExampleDataFile("dubrovnik-3-7-pre");
+  if (argc>1) filename = string(argv[1]);
+
+  // Load the SfM data from file
+  SfM_data mydata;
+  readBAL(filename, mydata);
+  cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
+
+  // Create a factor graph
+  NonlinearFactorGraph graph;
+
+  // We share *one* noiseModel between all projection factors
+  noiseModel::Isotropic::shared_ptr noise =
+      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+
+  // Add measurements to the factor graph
+  size_t j = 0;
+  BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
+    BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
+      size_t i = m.first;
+      Point2 uv = m.second;
+      graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
+    }
+    j += 1;
+  }
+
+  // Add a prior on pose x1. This indirectly specifies where the origin is.
+  // and a prior on the position of the first landmark to fix the scale
+  graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0],  noiseModel::Isotropic::Sigma(9, 0.1)));
+  graph.push_back(PriorFactor<Point3>    (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
+
+  // Create initial estimate
+  Values initial;
+  size_t i = 0; j = 0;
+  BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
+  BOOST_FOREACH(const SfM_Track& track, mydata.tracks)    initial.insert(P(j++), track.p);
+
+  /** ---------------  COMPARISON  -----------------------**/
+  /** ----------------------------------------------------**/
+
+  LevenbergMarquardtParams params_using_COLAMD, params_using_METIS;
+  try {
+    params_using_METIS.setVerbosity("ERROR");
+    gttic_(METIS_ORDERING);
+    params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph);
+    gttoc_(METIS_ORDERING);
+
+    params_using_COLAMD.setVerbosity("ERROR");
+    gttic_(COLAMD_ORDERING);
+    params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph);
+    gttoc_(COLAMD_ORDERING);
+  } catch (exception& e) {
+    cout << e.what();
+  }
+
+  // expect they have different ordering results
+  if(params_using_COLAMD.ordering == params_using_METIS.ordering) {
+    cout << "COLAMD and METIS produce the same ordering. "
+         << "Problem here!!!" << endl;
+  }
+
+  /* Optimize the graph with METIS and COLAMD and time the results */
+
+  Values result_METIS, result_COLAMD;
+  try {
+    gttic_(OPTIMIZE_WITH_METIS);
+    LevenbergMarquardtOptimizer lm_METIS(graph, initial, params_using_METIS);
+    result_METIS = lm_METIS.optimize();
+    gttoc_(OPTIMIZE_WITH_METIS);
+
+    gttic_(OPTIMIZE_WITH_COLAMD);
+    LevenbergMarquardtOptimizer lm_COLAMD(graph, initial, params_using_COLAMD);
+    result_COLAMD = lm_COLAMD.optimize();
+    gttoc_(OPTIMIZE_WITH_COLAMD);
+  } catch (exception& e) {
+    cout << e.what();
+  }
+
+
+  { // printing the result
+
+    cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl;
+    cout << "METIS final error: " << graph.error(result_METIS) << endl;
+
+    cout << endl << endl;
+
+    cout << "Time comparison by solving " << filename << " results:" << endl;
+    cout << boost::format("%1% point tracks and %2% cameras\n") \
+            % mydata.number_tracks() % mydata.number_cameras() \
+         << endl;
+
+    tictoc_print_();
+  }
+
+
+  return 0;
+}
+/* ************************************************************************* */
+

examples/SolverComparer.cpp

@@ -576,7 +576,7 @@ void runStats()
 {
   cout << "Gathering statistics..." << endl;
   GaussianFactorGraph linear = *datasetMeasurements.linearize(initial);
-  GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::colamd(linear)));
+  GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::Colamd(linear)));
   treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt);
 
   ofstream file;

gtsam/inference/EliminateableFactorGraph-inst.h

@@ -55,9 +55,9 @@ namespace gtsam {
       // have a VariableIndex already here because we computed one if needed in the previous 'else'
       // block.
       if (orderingType == Ordering::METIS)
-	      return eliminateSequential(Ordering::metis(asDerived()), function, variableIndex, orderingType);
+        return eliminateSequential(Ordering::Metis(asDerived()), function, variableIndex, orderingType);
-	    else
+      else
-		    return eliminateSequential(Ordering::colamd(*variableIndex), function, variableIndex, orderingType);
+        return eliminateSequential(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType);
     }
   }
 
@@ -93,9 +93,9 @@ namespace gtsam {
       // have a VariableIndex already here because we computed one if needed in the previous 'else'
       // block.
 	    if (orderingType == Ordering::METIS)
-		    return eliminateMultifrontal(Ordering::metis(asDerived()), function, variableIndex, orderingType);
+		    return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType);
 	    else
-		    return eliminateMultifrontal(Ordering::colamd(*variableIndex), function, variableIndex, orderingType);
+		    return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType);
     }
   }
 
@@ -125,7 +125,7 @@ namespace gtsam {
     if(variableIndex) {
       gttic(eliminatePartialSequential);
       // Compute full ordering
-      Ordering fullOrdering = Ordering::colamdConstrainedFirst(*variableIndex, variables);
+      Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables);
 
       // Split off the part of the ordering for the variables being eliminated
       Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
@@ -163,7 +163,7 @@ namespace gtsam {
     if(variableIndex) {
       gttic(eliminatePartialMultifrontal);
       // Compute full ordering
-      Ordering fullOrdering = Ordering::colamdConstrainedFirst(*variableIndex, variables);
+      Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables);
 
       // Split off the part of the ordering for the variables being eliminated
       Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
@@ -216,7 +216,7 @@ namespace gtsam {
           boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables);
 
         Ordering totalOrdering =
-          Ordering::colamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
+          Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
 
         // Split up ordering
         const size_t nVars = variablesOrOrdering->size();
@@ -275,7 +275,7 @@ namespace gtsam {
           boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables);
 
         Ordering totalOrdering =
-          Ordering::colamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
+          Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
 
         // Split up ordering
         const size_t nVars = variablesOrOrdering->size();
@@ -301,7 +301,7 @@ namespace gtsam {
     if(variableIndex)
     {
       // Compute a total ordering for all variables
-      Ordering totalOrdering = Ordering::colamdConstrainedLast(*variableIndex, variables);
+      Ordering totalOrdering = Ordering::ColamdConstrainedLast(*variableIndex, variables);
 
       // Split out the part for the marginalized variables
       Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size());

gtsam/inference/EliminateableFactorGraph.h

@@ -128,7 +128,8 @@ namespace gtsam {
 	    OptionalOrderingType orderingType = boost::none) const;
 
     /** Do multifrontal elimination of all variables to produce a Bayes tree.  If an ordering is not
-     *  provided, the ordering provided by COLAMD will be used.
+     *  provided, the ordering will be computed using either COLAMD or METIS, dependeing on
+     *  the parameter orderingType (Ordering::COLAMD or Ordering::METIS)
      *  
      *  <b> Example - Full Cholesky elimination in COLAMD order: </b>
      *  \code

gtsam/inference/ISAM-inst.h

@@ -46,7 +46,7 @@ namespace gtsam {
     const VariableIndex varIndex(factors);
     const KeySet newFactorKeys = newFactors.keys();
     const Ordering constrainedOrdering =
-      Ordering::colamdConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
+      Ordering::ColamdConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
     Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex);
     this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end());
     this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end());

gtsam/inference/Ordering.cpp

@@ -29,242 +29,236 @@ using namespace std;
 
 namespace gtsam {
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  FastMap<Key, size_t> Ordering::invert() const
+FastMap<Key, size_t> Ordering::invert() const {
-  {
+  FastMap<Key, size_t> inverted;
-    FastMap<Key, size_t> inverted;
+  for (size_t pos = 0; pos < this->size(); ++pos)
-    for(size_t pos = 0; pos < this->size(); ++pos)
+    inverted.insert(make_pair((*this)[pos], pos));
-      inverted.insert(make_pair((*this)[pos], pos));
+  return inverted;
-    return inverted;
+}
+
+/* ************************************************************************* */
+Ordering Ordering::Colamd(const VariableIndex& variableIndex) {
+  // Call constrained version with all groups set to zero
+  vector<int> dummy_groups(variableIndex.size(), 0);
+  return Ordering::ColamdConstrained(variableIndex, dummy_groups);
+}
+
+/* ************************************************************************* */
+Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
+    std::vector<int>& 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<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */
+  vector<int> p = vector<int>(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<Key> 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<size_t>::max();
+    BOOST_FOREACH(size_t factorIndex, column) {
+      if (lastFactorId != numeric_limits<size_t>::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;
   }
 
-  /* ************************************************************************* */
+  assert((size_t)count == variableIndex.nEntries());
-  Ordering Ordering::colamd(const VariableIndex& variableIndex)
+
-  {
+  //double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
-    // Call constrained version with all groups set to zero
+  double knobs[CCOLAMD_KNOBS];
-    vector<int> dummy_groups(variableIndex.size(), 0);
+  ccolamd_set_defaults(knobs);
-    return Ordering::colamdConstrained(variableIndex, dummy_groups);
+  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());
   }
 
-  /* ************************************************************************* */
+  //  ccolamd_report(stats);
-  Ordering Ordering::colamdConstrained(
-    const VariableIndex& variableIndex, std::vector<int>& cmember)
-  {
-    gttic(Ordering_COLAMDConstrained);
 
-    gttic(Prepare);
+  gttic(Fill_Ordering);
-    size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
+  // Convert elimination ordering in p to an ordering
-    // Convert to compressed column major format colamd wants it in (== MATLAB format!)
+  Ordering result;
-    size_t Alen = ccolamd_recommended((int)nEntries, (int)nFactors, (int)nVars); /* colamd arg 3: size of the array A */
+  result.resize(nVars);
-    vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */
+  for (size_t j = 0; j < nVars; ++j)
-    vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
+    result[j] = keys[p[j]];
+  gttoc(Fill_Ordering);
 
-    // Fill in input data for COLAMD
+  return result;
-    p[0] = 0;
+}
-    int count = 0;
-    vector<Key> 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<size_t>::max();
-      BOOST_FOREACH(size_t factorIndex, column) {
-        if(lastFactorId != numeric_limits<size_t>::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;
-    }
 
-    assert((size_t)count == variableIndex.nEntries());
+/* ************************************************************************* */
+Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex,
+    const std::vector<Key>& constrainLast, bool forceOrder) {
+  gttic(Ordering_COLAMDConstrainedLast);
 
-    //double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
+  size_t n = variableIndex.size();
-    double knobs[CCOLAMD_KNOBS];
+  std::vector<int> cmember(n, 0);
-    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 */
+  // 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++));
 
-    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
+  return Ordering::ColamdConstrained(variableIndex, cmember);
-    /* 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());
-    }
 
-    //  ccolamd_report(stats);
+/* ************************************************************************* */
+Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex,
+    const std::vector<Key>& constrainFirst, bool forceOrder) {
+  gttic(Ordering_COLAMDConstrainedFirst);
 
-    gttic(Fill_Ordering);
+  const int none = -1;
-    // Convert elimination ordering in p to an ordering
+  size_t n = variableIndex.size();
-    Ordering result;
+  std::vector<int> cmember(n, none);
-    result.resize(nVars);
-    for(size_t j = 0; j < nVars; ++j)
-      result[j] = keys[p[j]];
-    gttoc(Fill_Ordering);
 
+  // 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 && !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<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);
+}
+
+/* ************************************************************************* */
+Ordering Ordering::Metis(const MetisIndex& met) {
+  gttic(Ordering_METIS);
+
+  vector<idx_t> xadj = met.xadj();
+  vector<idx_t> adj = met.adj();
+  vector<idx_t> 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);
-  Ordering Ordering::colamdConstrainedLast(
+  for (size_t j = 0; j < (size_t) size; ++j) {
-    const VariableIndex& variableIndex, const std::vector<Key>& constrainLast, bool forceOrder)
+    // We have to add the minKey value back to obtain the original key in the Values
-  {
+    result[j] = met.intToKey(perm[j]);
-    gttic(Ordering_COLAMDConstrainedLast);
-
-    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++));
-
-    // 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);
   }
+  return result;
+}
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  Ordering Ordering::colamdConstrainedFirst(
+void Ordering::print(const std::string& str,
-    const VariableIndex& variableIndex, const std::vector<Key>& constrainFirst, bool forceOrder)
+    const KeyFormatter& keyFormatter) const {
-  {
+  cout << str;
-    gttic(Ordering_COLAMDConstrainedFirst);
+  // Print ordering in index order
-
+  // Print the ordering with varsPerLine ordering entries printed on each line,
-    const int none = -1;
+  // for compactness.
-    size_t n = variableIndex.size();
+  static const size_t varsPerLine = 10;
-    std::vector<int> cmember(n, none);
+  bool endedOnNewline = false;
-
+  for (size_t i = 0; i < size(); ++i) {
-    // Build a mapping to look up sorted Key indices by Key
+    if (i % varsPerLine == 0)
-    FastMap<Key, size_t> keyIndices;
+      cout << "Position " << i << ": ";
-    size_t j = 0;
+    if (i % varsPerLine != 0)
-    BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex)
+      cout << ", ";
-      keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++));
+    cout << keyFormatter(at(i));
-
+    if (i % varsPerLine == varsPerLine - 1) {
-    // 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<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);
-  }
-
-
-  /* ************************************************************************* */
-  Ordering Ordering::metis(const MetisIndex& met)
-  {
-    gttic(Ordering_METIS);
-
-    vector<idx_t> xadj   = met.xadj();
-    vector<idx_t>  adj   = met.adj();
-    vector<idx_t> 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)
       cout << "\n";
-    cout.flush();
+      endedOnNewline = true;
+    } else {
+      endedOnNewline = false;
+    }
   }
+  if (!endedOnNewline)
+    cout << "\n";
+  cout.flush();
+}
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  bool Ordering::equals(const Ordering& other, double tol) const
+bool Ordering::equals(const Ordering& other, double tol) const {
-  {
+  return (*this) == other;
-    return (*this) == other;
+}
-  }
 
 }

gtsam/inference/Ordering.h

@@ -30,170 +30,212 @@
 
 namespace gtsam {
 
-  class Ordering : public std::vector<Key> {
+class Ordering: public std::vector<Key> {
-  protected:
+protected:
-    typedef std::vector<Key> Base;
+  typedef std::vector<Key> Base;
 
-  public:
+public:
 
-    /// Type of ordering to use
+  /// Type of ordering to use
-    enum OrderingType {
+  enum OrderingType {
-      COLAMD, METIS, CUSTOM
+    COLAMD, METIS, NATURAL, CUSTOM
-    };
-
-    typedef Ordering This; ///< Typedef to this class
-    typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
-
-    /// Create an empty ordering
-    GTSAM_EXPORT Ordering() {}
-
-    /// Create from a container
-    template<typename KEYS>
-    explicit Ordering(const KEYS& keys) : Base(keys.begin(), keys.end()) {}
-
-    /// Create an ordering using iterators over keys
-    template<typename ITERATOR>
-    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<boost::assign_detail::call_push_back<This> >
-      operator+=(Key key) {
-        return boost::assign::make_list_inserter(boost::assign_detail::call_push_back<This>(*this))(key);
-    }
-
-    /// Invert (not reverse) the ordering - returns a map from key to order position
-    FastMap<Key, size_t> 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<class FACTOR>
-    static Ordering colamd(const FactorGraph<FACTOR>& 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<Key> \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<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 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
-    /// 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 before 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 before 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
-    /// 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);
-
-    /// Return a natural Ordering. Typically used by iterative solvers
-    template <class FACTOR>
-    static Ordering Natural(const FactorGraph<FACTOR> &fg) {
-      KeySet src = fg.keys();
-      std::vector<Key> keys(src.begin(), src.end());
-      std::stable_sort(keys.begin(), keys.end());
-      return Ordering(keys);
-    }
-
-    /// METIS Formatting function
-    template<class FACTOR>
-    static GTSAM_EXPORT void CSRFormat(std::vector<int>& xadj, std::vector<int>& adj, const FactorGraph<FACTOR>& graph);
-
-    /// Compute an ordering determined by METIS from a VariableIndex
-    static GTSAM_EXPORT Ordering metis(const MetisIndex& met);
-
-    template<class FACTOR>
-    static Ordering metis(const FactorGraph<FACTOR>& graph)
-    {
-        return metis(MetisIndex(graph));
-    }
-
-    /// @}
-
-    /// @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<int>& cmember);
-
-
-    /** Serialization function */
-    friend class boost::serialization::access;
-    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-    }
   };
 
-  /// traits
+  typedef Ordering This; ///< Typedef to this class
-  template<> struct traits<Ordering> : public Testable<Ordering> {};
+  typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
+
+  /// Create an empty ordering
+  GTSAM_EXPORT
+  Ordering() {
+  }
+
+  /// Create from a container
+  template<typename KEYS>
+  explicit Ordering(const KEYS& keys) :
+      Base(keys.begin(), keys.end()) {
+  }
+
+  /// Create an ordering using iterators over keys
+  template<typename ITERATOR>
+  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<boost::assign_detail::call_push_back<This> > operator+=(
+      Key key) {
+    return boost::assign::make_list_inserter(
+        boost::assign_detail::call_push_back<This>(*this))(key);
+  }
+
+  /// Invert (not reverse) the ordering - returns a map from key to order position
+  FastMap<Key, size_t> 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<class FACTOR>
+  static Ordering Colamd(const FactorGraph<FACTOR>& 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<Key> \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<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 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
+  /// 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 before 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 before 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
+  /// 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);
+
+  /// Return a natural Ordering. Typically used by iterative solvers
+  template<class FACTOR>
+  static Ordering Natural(const FactorGraph<FACTOR> &fg) {
+    KeySet src = fg.keys();
+    std::vector<Key> keys(src.begin(), src.end());
+    std::stable_sort(keys.begin(), keys.end());
+    return Ordering(keys);
+  }
+
+  /// METIS Formatting function
+  template<class FACTOR>
+  static GTSAM_EXPORT void CSRFormat(std::vector<int>& xadj,
+      std::vector<int>& adj, const FactorGraph<FACTOR>& graph);
+
+  /// Compute an ordering determined by METIS from a VariableIndex
+  static GTSAM_EXPORT Ordering Metis(const MetisIndex& met);
+
+  template<class FACTOR>
+  static Ordering Metis(const FactorGraph<FACTOR>& graph) {
+    return Metis(MetisIndex(graph));
+  }
+
+  /// @}
+
+  /// @name Named Constructors @{
+
+  template<class FACTOR>
+  static Ordering Create(OrderingType orderingType,
+      const FactorGraph<FACTOR>& graph) {
+
+    switch (orderingType) {
+    case COLAMD:
+      return Colamd(graph);
+    case METIS:
+      return Metis(graph);
+    case NATURAL:
+      return Natural(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<int>& cmember);
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+  }
+};
+
+/// traits
+template<> struct traits<Ordering> : public Testable<Ordering> {
+};
 
 }
 

gtsam/inference/tests/testOrdering.cpp

@@ -28,45 +28,47 @@ using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
 
+namespace example {
+SymbolicFactorGraph symbolicChain() {
+  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(4, 5);
+  return sfg;
+}
+}
 /* ************************************************************************* */
 TEST(Ordering, constrained_ordering) {
-  SymbolicFactorGraph sfg;
 
   // create graph with wanted variable set = 2, 4
-  sfg.push_factor(0,1);
+  SymbolicFactorGraph sfg = example::symbolicChain();
-  sfg.push_factor(1,2);
-  sfg.push_factor(2,3);
-  sfg.push_factor(3,4);
-  sfg.push_factor(4,5);
 
   // unconstrained version
-  Ordering actUnconstrained = Ordering::colamd(sfg);
+  Ordering actUnconstrained = Ordering::Colamd(sfg);
   Ordering expUnconstrained = Ordering(list_of(0)(1)(2)(3)(4)(5));
   EXPECT(assert_equal(expUnconstrained, actUnconstrained));
 
   // constrained version - push one set to the end
-  Ordering actConstrained = Ordering::colamdConstrainedLast(sfg, list_of(2)(4));
+  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 actConstrained2 = Ordering::ColamdConstrainedFirst(sfg,
+      list_of(2)(4));
   Ordering expConstrained2 = Ordering(list_of(2)(4)(0)(1)(3)(5));
   EXPECT(assert_equal(expConstrained2, actConstrained2));
 }
 
 /* ************************************************************************* */
 TEST(Ordering, grouped_constrained_ordering) {
-  SymbolicFactorGraph sfg;
 
   // create graph with constrained groups:
   // 1: 2, 4
   // 2: 5
-  sfg.push_factor(0,1);
+  SymbolicFactorGraph sfg = example::symbolicChain();
-  sfg.push_factor(1,2);
-  sfg.push_factor(2,3);
-  sfg.push_factor(3,4);
-  sfg.push_factor(4,5);
 
   // constrained version - push one set to the end
   FastMap<size_t, int> constraints;
@@ -74,50 +76,48 @@ TEST(Ordering, grouped_constrained_ordering) {
   constraints[4] = 1;
   constraints[5] = 2;
 
-  Ordering actConstrained = Ordering::colamdConstrained(sfg, constraints);
+  Ordering actConstrained = Ordering::ColamdConstrained(sfg, constraints);
   Ordering expConstrained = list_of(0)(1)(3)(2)(4)(5);
   EXPECT(assert_equal(expConstrained, actConstrained));
 }
 
 /* ************************************************************************* */
 TEST(Ordering, csr_format) {
-    // Example in METIS manual
+  // Example in METIS manual
-    SymbolicFactorGraph sfg;
+  SymbolicFactorGraph sfg;
-    sfg.push_factor(0, 1);
+  sfg.push_factor(0, 1);
-    sfg.push_factor(1, 2);
+  sfg.push_factor(1, 2);
-    sfg.push_factor(2, 3);
+  sfg.push_factor(2, 3);
-    sfg.push_factor(3, 4);
+  sfg.push_factor(3, 4);
-    sfg.push_factor(5, 6);
+  sfg.push_factor(5, 6);
-    sfg.push_factor(6, 7);
+  sfg.push_factor(6, 7);
-    sfg.push_factor(7, 8);
+  sfg.push_factor(7, 8);
-    sfg.push_factor(8, 9);
+  sfg.push_factor(8, 9);
-    sfg.push_factor(10, 11);
+  sfg.push_factor(10, 11);
-    sfg.push_factor(11, 12);
+  sfg.push_factor(11, 12);
-    sfg.push_factor(12, 13);
+  sfg.push_factor(12, 13);
-    sfg.push_factor(13, 14);
+  sfg.push_factor(13, 14);
 
-    sfg.push_factor(0, 5);
+  sfg.push_factor(0, 5);
-    sfg.push_factor(5, 10);
+  sfg.push_factor(5, 10);
-    sfg.push_factor(1, 6);
+  sfg.push_factor(1, 6);
-    sfg.push_factor(6, 11);
+  sfg.push_factor(6, 11);
-    sfg.push_factor(2, 7);
+  sfg.push_factor(2, 7);
-    sfg.push_factor(7, 12);
+  sfg.push_factor(7, 12);
-    sfg.push_factor(3, 8);
+  sfg.push_factor(3, 8);
-    sfg.push_factor(8, 13);
+  sfg.push_factor(8, 13);
-    sfg.push_factor(4, 9);
+  sfg.push_factor(4, 9);
-    sfg.push_factor(9, 14);
+  sfg.push_factor(9, 14);
 
-    MetisIndex mi(sfg);
+  MetisIndex mi(sfg);
 
-    vector<int> xadjExpected, adjExpected;
+  vector<int> xadjExpected, adjExpected;
-    xadjExpected += 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44;
+  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,
+  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;
-                             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(xadjExpected == mi.xadj());
-    EXPECT(adjExpected.size() == mi.adj().size());
+  EXPECT(adjExpected.size() == mi.adj().size());
-    EXPECT(adjExpected  == mi.adj());
+  EXPECT(adjExpected == mi.adj());
 }
 
 /* ************************************************************************* */
@@ -135,12 +135,11 @@ TEST(Ordering, csr_format_2) {
 
   vector<int> 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());
   EXPECT(adjExpected == mi.adj());
-
 }
 
 /* ************************************************************************* */
@@ -170,7 +169,6 @@ TEST(Ordering, csr_format_3) {
   EXPECT(xadjExpected == mi.xadj());
   EXPECT(adjExpected.size() == mi.adj().size());
   EXPECT(adjExpected == adjAcutal);
-  
 }
 
 /* ************************************************************************* */
@@ -197,7 +195,7 @@ TEST(Ordering, csr_format_4) {
   EXPECT(adjExpected.size() == mi.adj().size());
   EXPECT(adjExpected == adjAcutal);
 
-  Ordering metOrder = Ordering::metis(sfg);
+  Ordering metOrder = Ordering::Metis(sfg);
 
   // Test different symbol types
   sfg.push_factor(Symbol('l', 1));
@@ -206,8 +204,7 @@ TEST(Ordering, csr_format_4) {
   sfg.push_factor(Symbol('x', 3), Symbol('l', 1));
   sfg.push_factor(Symbol('x', 4), Symbol('l', 1));
 
-  Ordering metOrder2 = Ordering::metis(sfg);
+  Ordering metOrder2 = Ordering::Metis(sfg);
-
 }
 
 /* ************************************************************************* */
@@ -229,8 +226,77 @@ TEST(Ordering, metis) {
   EXPECT(adjExpected.size() == mi.adj().size());
   EXPECT(adjExpected == mi.adj());
 
-  Ordering metis = Ordering::metis(sfg);
+  Ordering metis = Ordering::Metis(sfg);
+}
+
+/* ************************************************************************* */
+TEST(Ordering, MetisLoop) {
+
+  // create linear graph
+  SymbolicFactorGraph sfg = example::symbolicChain();
+
+  // add loop closure
+  sfg.push_factor(0, 5);
+
+  // METIS
+#if !defined(__APPLE__)
+  {
+    Ordering actual = Ordering::Create(Ordering::METIS, sfg);
+    //  - P( 0 4 1)
+    //  | - P( 2 | 4 1)
+    //  | | - P( 3 | 4 2)
+    //  | - P( 5 | 0 1)
+    Ordering expected = Ordering(list_of(3)(2)(5)(0)(4)(1));
+    EXPECT(assert_equal(expected, actual));
+  }
+#else
+  {
+    Ordering actual = Ordering::Create(Ordering::METIS, sfg);
+    //  - P( 1 0 3)
+    //  | - P( 4 | 0 3)
+    //  | | - P( 5 | 0 4)
+    //  | - P( 2 | 1 3)
+    Ordering expected = Ordering(list_of(5)(4)(2)(1)(0)(3));
+    EXPECT(assert_equal(expected, actual));
+  }
+#endif
+}
+
+/* ************************************************************************* */
+TEST(Ordering, Create) {
+
+  // create chain graph
+  SymbolicFactorGraph sfg = example::symbolicChain();
+
+  // COLAMD
+  {
+    //- P( 4 5)
+    //| - P( 3 | 4)
+    //| | - P( 2 | 3)
+    //| | | - P( 1 | 2)
+    //| | | | - P( 0 | 1)
+    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);
+    //- P( 1 0 2)
+    //| - P( 3 4 | 2)
+    //| | - P( 5 | 4)
+    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);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
-/* ************************************************************************* */

gtsam/nonlinear/DoglegOptimizer.cpp

@@ -95,8 +95,8 @@ void DoglegOptimizer::iterate(void) {
 
 /* ************************************************************************* */
 DoglegParams DoglegOptimizer::ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const {
-  if(!params.ordering)
+  if (!params.ordering)
-    params.ordering = Ordering::colamd(graph);
+    params.ordering = Ordering::Create(params.orderingType, graph);
   return params;
 }
 

gtsam/nonlinear/GaussNewtonOptimizer.cpp

@@ -46,10 +46,9 @@ void GaussNewtonOptimizer::iterate() {
 
 /* ************************************************************************* */
 GaussNewtonParams GaussNewtonOptimizer::ensureHasOrdering(
-  GaussNewtonParams params, const NonlinearFactorGraph& graph) const
+    GaussNewtonParams params, const NonlinearFactorGraph& graph) const {
-{
+  if (!params.ordering)
-  if(!params.ordering)
+    params.ordering = Ordering::Create(params.orderingType, graph);
-    params.ordering = Ordering::colamd(graph);
   return params;
 }
 

gtsam/nonlinear/ISAM2.cpp

@@ -341,7 +341,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
     Ordering order;
     if(constrainKeys)
     {
-      order = Ordering::colamdConstrained(variableIndex_, *constrainKeys);
+      order = Ordering::ColamdConstrained(variableIndex_, *constrainKeys);
     }
     else
     {
@@ -351,11 +351,11 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
         FastMap<Key, int> constraintGroups;
         BOOST_FOREACH(Key var, observedKeys)
           constraintGroups[var] = 1;
-        order = Ordering::colamdConstrained(variableIndex_, constraintGroups);
+        order = Ordering::ColamdConstrained(variableIndex_, constraintGroups);
       }
       else
       {
-        order = Ordering::colamd(variableIndex_);
+        order = Ordering::Colamd(variableIndex_);
       }
     }
     gttoc(ordering);
@@ -481,7 +481,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
 
     // Generate ordering
     gttic(Ordering);
-    Ordering ordering = Ordering::colamdConstrained(affectedFactorsVarIndex, constraintGroups);
+    Ordering ordering = Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups);
     gttoc(Ordering);
 
     ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree(

gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -254,6 +254,7 @@ void LevenbergMarquardtOptimizer::iterate() {
 
     bool systemSolvedSuccessfully;
     try {
+      // ============ Solve is where most computation happens !! =================
       delta = solve(dampedSystem, state_.values, params_);
       systemSolvedSuccessfully = true;
     } catch (const IndeterminantLinearSystemException& e) {
@@ -281,7 +282,9 @@ void LevenbergMarquardtOptimizer::iterate() {
       if (linearizedCostChange >= 0) { // step is valid
         // update values
         gttic(retract);
+        // ============ This is where the solution is updated ====================
         newValues = state_.values.retract(delta);
+        // =======================================================================
         gttoc(retract);
 
         // compute new error
@@ -361,12 +364,8 @@ void LevenbergMarquardtOptimizer::iterate() {
 /* ************************************************************************* */
 LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering(
     LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const {
-  if (!params.ordering){
+  if (!params.ordering)
-    if (params.orderingType == Ordering::METIS)
+    params.ordering = Ordering::Create(params.orderingType, graph);
-      params.ordering = Ordering::metis(graph);
-    else
-      params.ordering = Ordering::colamd(graph);
-  }
   return params;
 }
 

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -248,13 +248,13 @@ KeySet NonlinearFactorGraph::keys() const {
 /* ************************************************************************* */
 Ordering NonlinearFactorGraph::orderingCOLAMD() const
 {
-  return Ordering::colamd(*this);
+  return Ordering::Colamd(*this);
 }
 
 /* ************************************************************************* */
 Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const
 {
-  return Ordering::colamdConstrained(*this, constraints);
+  return Ordering::ColamdConstrained(*this, constraints);
 }
 
 /* ************************************************************************* */

gtsam/symbolic/tests/testSymbolicBayesTree.cpp

@@ -691,6 +691,76 @@ TEST(SymbolicBayesTree, complicatedMarginal)
   }
 
 }
+
+/* ************************************************************************* */
+TEST(SymbolicBayesTree, COLAMDvsMETIS) {
+
+  // create circular graph
+  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(4, 5);
+  sfg.push_factor(0, 5);
+
+  // COLAMD
+  {
+    Ordering ordering = Ordering::Create(Ordering::COLAMD, sfg);
+    EXPECT(assert_equal(Ordering(list_of(0)(5)(1)(4)(2)(3)), ordering));
+
+    //  - P( 4 2 3)
+    //  | - P( 1 | 2 4)
+    //  | | - P( 5 | 1 4)
+    //  | | | - P( 0 | 1 5)
+    SymbolicBayesTree expected;
+    expected.insertRoot(
+        MakeClique(list_of(4)(2)(3), 3,
+            list_of(
+                MakeClique(list_of(1)(2)(4), 1,
+                    list_of(
+                        MakeClique(list_of(5)(1)(4), 1,
+                            list_of(MakeClique(list_of(0)(1)(5), 1))))))));
+
+    SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering);
+    EXPECT(assert_equal(expected, actual));
+  }
+
+  // METIS
+  {
+    Ordering ordering = Ordering::Create(Ordering::METIS, sfg);
+// Linux and Mac split differently when using mettis
+#if !defined(__APPLE__)
+    EXPECT(assert_equal(Ordering(list_of(3)(2)(5)(0)(4)(1)), ordering));
+#else
+    EXPECT(assert_equal(Ordering(list_of(5)(4)(2)(1)(0)(3)), ordering));
+#endif
+
+    //  - P( 1 0 3)
+    //  | - P( 4 | 0 3)
+    //  | | - P( 5 | 0 4)
+    //  | - P( 2 | 1 3)
+    SymbolicBayesTree expected;
+#if !defined(__APPLE__)
+    expected.insertRoot(
+        MakeClique(list_of(2)(4)(1), 3,
+            list_of(
+                MakeClique(list_of(0)(1)(4), 1,
+                    list_of(MakeClique(list_of(5)(0)(4), 1))))(
+                MakeClique(list_of(3)(2)(4), 1))));
+#else
+    expected.insertRoot(
+            MakeClique(list_of(1)(0)(3), 3,
+                list_of(
+                    MakeClique(list_of(4)(0)(3), 1,
+                        list_of(MakeClique(list_of(5)(0)(4), 1))))(
+                    MakeClique(list_of(2)(1)(3), 1))));
+#endif
+    SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering);
+    EXPECT(assert_equal(expected, actual));
+  }
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp

@@ -205,7 +205,7 @@ void BatchFixedLagSmoother::reorder(const std::set<Key>& marginalizeKeys) {
   }
 
   // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1
-  ordering_ = Ordering::colamdConstrainedFirst(factors_,
+  ordering_ = Ordering::ColamdConstrainedFirst(factors_,
       std::vector<Key>(marginalizeKeys.begin(), marginalizeKeys.end()));
 
   if (debug) {

gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp

@@ -362,9 +362,9 @@ void ConcurrentBatchFilter::reorder(const boost::optional<FastList<Key> >& keysT
 
   // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1
   if(keysToMove && keysToMove->size() > 0) {
-    ordering_ = Ordering::colamdConstrainedFirst(factors_, std::vector<Key>(keysToMove->begin(), keysToMove->end()));
+    ordering_ = Ordering::ColamdConstrainedFirst(factors_, std::vector<Key>(keysToMove->begin(), keysToMove->end()));
   }else{
-    ordering_ = Ordering::colamd(factors_);
+    ordering_ = Ordering::Colamd(factors_);
   }
 
 }

gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp

@@ -231,7 +231,7 @@ void ConcurrentBatchSmoother::reorder() {
   variableIndex_ = VariableIndex(factors_);
 
   KeyVector separatorKeys = separatorValues_.keys();
-  ordering_ = Ordering::colamdConstrainedLast(variableIndex_, std::vector<Key>(separatorKeys.begin(), separatorKeys.end()));
+  ordering_ = Ordering::ColamdConstrainedLast(variableIndex_, std::vector<Key>(separatorKeys.begin(), separatorKeys.end()));
 
 }
 

tests/testNonlinearFactorGraph.cpp

@@ -79,14 +79,14 @@ TEST( NonlinearFactorGraph, GET_ORDERING)
 {
   Ordering expected; expected += L(1), X(2), X(1); // For starting with l1,x1,x2
   NonlinearFactorGraph nlfg = createNonlinearFactorGraph();
-  Ordering actual = Ordering::colamd(nlfg);
+  Ordering actual = Ordering::Colamd(nlfg);
   EXPECT(assert_equal(expected,actual));
 
   // Constrained ordering - put x2 at the end
   Ordering expectedConstrained; expectedConstrained += L(1), X(1), X(2);
   FastMap<Key, int> constraints;
   constraints[X(2)] = 1;
-  Ordering actualConstrained = Ordering::colamdConstrained(nlfg, constraints);
+  Ordering actualConstrained = Ordering::ColamdConstrained(nlfg, constraints);
   EXPECT(assert_equal(expectedConstrained, actualConstrained));
 }