Work in progress on correcting bug with key casting to int32. Causes overflow on cast, causing bad array indexing in metis

examples/StereoVOExample_large.cpp

@@ -103,7 +103,9 @@ int main(int argc, char** argv){
 
   cout << "Optimizing" << endl;
   //create Levenberg-Marquardt optimizer to optimize the factor graph
-  LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
+  LevenbergMarquardtParams params;
+  params.orderingType = OrderingType::METIS;
+  LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params);
   Values result = optimizer.optimize();
 
   cout << "Final result sample:" << endl;

gtsam/inference/MetisIndex-inl.h

@@ -27,8 +27,8 @@ namespace gtsam {
 template<class FACTOR>
 void MetisIndex::augment(const FactorGraph<FACTOR>& factors)
 {
-	std::map<int, FastSet<int> > adjMap;
+  std::map<Key, FastSet<Key> > adjMap;
-	std::map<int, FastSet<int> >::iterator adjMapIt;
+  std::map<Key, FastSet<Key> >::iterator adjMapIt;
 	std::set<Key> keySet;
         
 	/* ********** Convert to CSR format ********** */

gtsam/inference/MetisIndex.h

@@ -40,8 +40,8 @@ public:
 	typedef boost::shared_ptr<MetisIndex> shared_ptr;
 
 private:
-	FastVector<int> xadj_; // Index of node's adjacency list in adj
+	FastVector<Key> xadj_; // Index of node's adjacency list in adj
-	FastVector<int>  adj_; // Stores ajacency lists of all nodes, appended into a single vector
+  FastVector<Key>  adj_; // Stores ajacency lists of all nodes, appended into a single vector
 	size_t nFactors_;      // Number of factors in the original factor graph
 	size_t nKeys_;         // 
 	size_t minKey_;
@@ -69,8 +69,8 @@ public:
 	template<class FACTOR>
 	void augment(const FactorGraph<FACTOR>& factors);
 
-	std::vector<int> xadj() const { return   xadj_; }
+	std::vector<Key> xadj() const { return   xadj_; }
-	std::vector<int>  adj() const { return    adj_; }
+  std::vector<Key>  adj() const { return    adj_; }
 	size_t        nValues() const { return  nKeys_; }
 	size_t         minKey() const { return minKey_; }
 

gtsam/inference/Ordering.cpp

@@ -204,12 +204,43 @@ namespace gtsam {
   {
     gttic(Ordering_METIS);
 
-    vector<idx_t> xadj = met.xadj();
+    vector<Key> xadj = met.xadj();
-    vector<idx_t>  adj = met.adj();
+    vector<Key>  adj = met.adj();
-		size_t minKey = met.minKey();
+		Key minKey      = met.minKey();
+
+    // TODO(Andrew): Debug
+    Key min = std::numeric_limits<Key>::max();
+    for (int i = 0; i < adj.size(); i++)
+    {
+      if (adj[i] < min)
+        min = adj[i];
+    }
+
+    std::cout << "Min: " << min << " minkey: " << minKey << std::endl;
 
 		// Normalize, subtract the smallest key
-		std::transform(adj.begin(), adj.end(), adj.begin(), std::bind2nd(std::minus<size_t>(), minKey));
+		//std::transform(adj.begin(), adj.end(), adj.begin(), std::bind2nd(std::minus<Key>(), minKey));
+    for (vector<Key>::iterator it = adj.begin(); it != adj.end(); ++it)
+      *it = *it - minKey;
+
+    // Cast the adjacency formats into idx_t (int32)
+    // NOTE: Keys can store quite large values and hence may overflow during conversion to int
+    //       It's important that the normalization is performed first.
+    vector<idx_t> adj_idx;
+    for (vector<Key>::iterator it = adj.begin(); it != adj.end(); ++it)
+      adj_idx.push_back(static_cast<int>(*it));
+
+    vector<idx_t> xadj_idx;
+    for (vector<Key>::iterator it = xadj.begin(); it != xadj.end(); ++it)
+      xadj_idx.push_back(static_cast<int>(*it));
+
+    // TODO(Andrew): Debug
+    for (int i = 0; i < adj.size(); i++)
+    {
+      assert(adj[i] >= 0);
+      if (adj[i] < 0)
+        std::cout << adj[i] << std::endl;
+    }
 
     vector<idx_t> perm, iperm;
 
@@ -222,7 +253,7 @@ namespace gtsam {
 
     int outputError;
       
-    outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], &iperm[0]);
+    outputError = METIS_NodeND(&size, &xadj_idx[0], &adj_idx[0], NULL, NULL, &perm[0], &iperm[0]);
     Ordering result;
 
     if (outputError != METIS_OK)
@@ -234,7 +265,7 @@ namespace gtsam {
 	  result.resize(size);
 		for (size_t j = 0; j < size; ++j){
 			// We have to add the minKey value back to obtain the original key in the Values
-			result[j] = perm[j] + minKey;
+			result[j] = (Key)perm[j] + minKey;
 		}
     return result;
   }