METIS ordering only works on values that are 0 indexed. Otherwise heap corruption occurs inside metis ordering function. Not sure how to fix/enforce

examples/METISOrderingExample.cpp

@@ -86,7 +86,10 @@ int main(int argc, char** argv) {
   initial.print("\nInitial Estimate:\n"); // print
 
   // optimize using Levenberg-Marquardt optimization
-  Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
+  LevenbergMarquardtParams params;
+  params.orderingType = Ordering::Type::METIS_;
+  LevenbergMarquardtOptimizer optimizer(graph, initial, params);
+  Values result = optimizer.optimize();
   result.print("Final Result:\n");
 
   // Calculate and print marginal covariances for all variables

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::Type::METIS_)
-	    return eliminateSequential(Ordering::METIS(asDerived()), function, variableIndex, orderingType);
-	  else
-		  return eliminateSequential(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType);
+	      return eliminateSequential(Ordering::METIS(asDerived()), function, variableIndex, orderingType);
+	    else
+		    return eliminateSequential(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType);
     }
   }
 

gtsam/inference/MetisIndex-inl.h

@@ -29,6 +29,7 @@ namespace gtsam {
     {
         std::map<int, FastSet<int> > adjMap;
         std::map<int, FastSet<int> >::iterator adjMapIt;
+        std::set<int> values;
         
         /* ********** Convert to CSR format ********** */
         // Assuming that vertex numbering starts from 0 (C style),
@@ -37,13 +38,19 @@ namespace gtsam {
         // index xadj[i + 1](i.e., adjncy[xadj[i]] through
         // and including adjncy[xadj[i + 1] - 1]).
         for (size_t i = 0; i < factors.size(); i++){
-            if (factors[i])
-                BOOST_FOREACH(const Key& k1, *factors[i])
-                    BOOST_FOREACH(const Key& k2, *factors[i])
-                        if (k1 != k2)
-                            adjMap[k1].insert(adjMap[k1].end(), k2); // Insert at the end
+          if (factors[i]){
+            BOOST_FOREACH(const Key& k1, *factors[i]){
+              BOOST_FOREACH(const Key& k2, *factors[i]){
+                if (k1 != k2)
+                  adjMap[k1].insert(adjMap[k1].end(), k2); // Insert at the end
+              }
+              values.insert(values.end(), k1); // Keep a track of all unique values
+            }
+          }
         }
 
+        // Number of values referenced in this factorgraph
+        nValues_ = values.size();
         
         xadj_.push_back(0);// Always set the first index to zero
         for (adjMapIt = adjMap.begin(); adjMapIt != adjMap.end(); ++adjMapIt) {

gtsam/inference/MetisIndex.h

@@ -71,6 +71,7 @@ public:
 
     std::vector<int> xadj() const { return xadj_; }
     std::vector<int>  adj() const { return  adj_; }
+    size_t        nValues() const { return nValues_; }
 
     /// @}
 };

gtsam/inference/Ordering.cpp

@@ -202,30 +202,30 @@ namespace gtsam {
   /* ************************************************************************* */
   Ordering Ordering::METIS(const MetisIndex& met)
   {
-      gttic(Ordering_METIS);
+    gttic(Ordering_METIS);
 
-      vector<int> xadj = met.xadj();
-      vector<int>  adj = met.adj();
+    vector<idx_t> xadj = met.xadj();
+    vector<idx_t>  adj = met.adj();
 
-      vector<int> perm, iperm;
+    vector<idx_t> perm, iperm;
 
-	  idx_t size = xadj.size() - 1;
+	  idx_t size = met.nValues();
 	  for (idx_t i = 0; i < size; i++)
 	  {
 		  perm.push_back(0);
 		  iperm.push_back(0);
 	  }
 
-      int outputError;
+    int outputError;
       
-      outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], &iperm[0]);
-      Ordering result;
+    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;
-      }
+    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; ++j)

gtsam/inference/tests/testOrdering.cpp

@@ -116,17 +116,52 @@ TEST(Ordering, csr_format) {
 
     EXPECT(xadjExpected  == mi.xadj());
     EXPECT(adjExpected.size() == mi.adj().size());
-    EXPECT( adjExpected  == mi.adj());
+    EXPECT(adjExpected  == mi.adj());
 }
+/* ************************************************************************* */
+
+TEST(Ordering, csr_format_2) {
+  SymbolicFactorGraph sfg;
+
+  sfg.push_factor(0);
+  sfg.push_factor(0, 1);
+  sfg.push_factor(1, 2);
+  sfg.push_factor(2, 3);
+  sfg.push_factor(3, 4);
+  sfg.push_factor(4, 1);
+
+  MetisIndex mi(sfg);
+
+  vector<int> xadjExpected { 0, 1, 4, 6, 8, 10 };
+  vector<int> 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());
+
+  //Ordering metis = Ordering::METIS(sfg);
+
+}
+
 /* ************************************************************************* */
 TEST(Ordering, metis) {
 	
-    SymbolicFactorGraph sfg;
+  SymbolicFactorGraph sfg;
 
-    sfg.push_factor(0, 1);
+  sfg.push_factor(0);
+  sfg.push_factor(0, 1);
 	sfg.push_factor(1, 2);
 
-    Ordering metis = Ordering::METIS(sfg);
+  MetisIndex mi(sfg);
+
+  vector<int> xadjExpected{ 0, 1, 3, 4 };
+  vector<int> adjExpected { 1, 0, 2, 1 };
+
+  EXPECT(xadjExpected == mi.xadj());
+  EXPECT(adjExpected.size() == mi.adj().size());
+  EXPECT(adjExpected == mi.adj());
+
+  Ordering metis = Ordering::METIS(sfg);
 }
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -341,7 +341,10 @@ void LevenbergMarquardtOptimizer::iterate() {
 LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering(
     LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const {
   if (!params.ordering)
-    params.ordering = Ordering::COLAMD(graph);
+    if (params.orderingType = Ordering::Type::METIS_)
+      params.ordering = Ordering::METIS(graph);
+    else
+      params.ordering = Ordering::COLAMD(graph);
   return params;
 }
 

gtsam/nonlinear/NonlinearOptimizerParams.h

@@ -154,7 +154,7 @@ public:
 
   void setOrdering(const Ordering& ordering) {
     this->ordering = ordering;
-	this->orderingType = Ordering::Type::CUSTOM_;
+	  this->orderingType = Ordering::Type::CUSTOM_;
   }
 
   std::string getOrderingType() const {