Adding METIS ordering logic to elimination

gtsam/inference/EliminateableFactorGraph-inst.h

@@ -28,7 +28,8 @@ namespace gtsam {
   template<class FACTORGRAPH>
   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
     EliminateableFactorGraph<FACTORGRAPH>::eliminateSequential(
-    OptionalOrdering ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
+	OptionalOrdering ordering, const Eliminate& function,
+	OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
   {
     if(ordering && variableIndex) {
       gttic(eliminateSequential);
@@ -47,13 +48,16 @@ namespace gtsam {
       // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
       // for no variable index first so that it's always computed if we need to call COLAMD because
       // no Ordering is provided.
-      return eliminateSequential(ordering, function, VariableIndex(asDerived()));
+      return eliminateSequential(ordering, function, VariableIndex(asDerived()), orderingType);
     }
     else /*if(!ordering)*/ {
       // If no Ordering provided, compute one and call this function again.  We are guaranteed to
       // have a VariableIndex already here because we computed one if needed in the previous 'else'
       // block.
-      return eliminateSequential(Ordering::COLAMD(*variableIndex), function);
+      if (orderingType == Ordering::Type::METIS_)
+	    return eliminateSequential(Ordering::METIS(asDerived()), function, variableIndex, orderingType);
+	  else
+		  return eliminateSequential(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType);
     }
   }
 
@@ -61,7 +65,8 @@ namespace gtsam {
   template<class FACTORGRAPH>
   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
     EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
-    OptionalOrdering ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
+	OptionalOrdering ordering, const Eliminate& function,
+	OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
   {
     if(ordering && variableIndex) {
       gttic(eliminateMultifrontal);
@@ -81,13 +86,16 @@ namespace gtsam {
       // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
       // for no variable index first so that it's always computed if we need to call COLAMD because
       // no Ordering is provided.
-      return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()));
+		return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType);
     }
     else /*if(!ordering)*/ {
       // If no Ordering provided, compute one and call this function again.  We are guaranteed to
       // have a VariableIndex already here because we computed one if needed in the previous 'else'
       // block.
-      return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function);
+	  if (orderingType == Ordering::Type::METIS_)
+		  return eliminateMultifrontal(Ordering::METIS(asDerived()), function, variableIndex, orderingType);
+	  else
+		  return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType);
     }
   }
 

gtsam/inference/EliminateableFactorGraph.h

@@ -94,6 +94,9 @@ namespace gtsam {
     /// Typedef for an optional variable index as an argument to elimination functions
     typedef boost::optional<const VariableIndex&> OptionalVariableIndex;
 
+	/// Typedef for an optional ordering type
+	typedef boost::optional<Ordering::Type> OptionalOrderingType;
+
     /** Do sequential elimination of all variables to produce a Bayes net.  If an ordering is not
      *  provided, the ordering provided by COLAMD will be used.
      *  
@@ -102,6 +105,10 @@ namespace gtsam {
      *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateCholesky);
      *  \endcode
 	 *
+	 *  <b> Example - METIS ordering for elimination
+	 *  \code
+	 *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(Ordering::Type::METIS_);
+     *  
      *  <b> Example - Full QR elimination in specified order:
      *  \code
      *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateQR, myOrdering);
@@ -117,7 +124,8 @@ namespace gtsam {
     boost::shared_ptr<BayesNetType> eliminateSequential(
       OptionalOrdering ordering = boost::none,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
-      OptionalVariableIndex variableIndex = boost::none) const;
+      OptionalVariableIndex variableIndex = boost::none,
+	  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.
@@ -142,7 +150,8 @@ namespace gtsam {
     boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
       OptionalOrdering ordering = boost::none,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
-      OptionalVariableIndex variableIndex = boost::none) const;
+	  OptionalVariableIndex variableIndex = boost::none,
+	  OptionalOrderingType orderingType = boost::none) const;
 
     /** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net
      *  and a remaining factor graph.  This computes the factorization \f$ p(X) = p(A|B) p(B) \f$,

gtsam/inference/MetisIndex-inl.h

@@ -18,6 +18,7 @@
 #pragma once
 
 #include <map>
+#include <vector>
 
 namespace gtsam {
 
@@ -46,9 +47,9 @@ namespace gtsam {
         
         xadj_.push_back(0);// Always set the first index to zero
         for (adjMapIt = adjMap.begin(); adjMapIt != adjMap.end(); ++adjMapIt) {
-            vector<int> temp;
+            std::vector<int> temp;
             // Copy from the FastSet into a temporary vector
-            copy(adjMapIt->second.begin(), adjMapIt->second.end(), std::back_inserter(temp));
+            std::copy(adjMapIt->second.begin(), adjMapIt->second.end(), std::back_inserter(temp));
             // Insert each index's set in order by appending them to the end of adj_
             adj_.insert(adj_.end(), temp.begin(), temp.end());
             //adj_.push_back(temp);

gtsam/nonlinear/NonlinearOptimizer.cpp

@@ -107,7 +107,8 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg,
     delta = gfg.optimize(*params.ordering, params.getEliminationFunction());
   } else if (params.isSequential()) {
     // Sequential QR or Cholesky (decided by params.getEliminationFunction())
-    delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize();
+    delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(), 
+								     boost::none, params.orderingType)->optimize();
   } else if (params.isIterative()) {
 
     // Conjugate Gradient -> needs params.iterativeParams