From b19ed67545109307c3e0c4db769b6e249ce88ab7 Mon Sep 17 00:00:00 2001
From: Andrew Melim <Andrew.Melim@gatech.edu>
Date: Fri, 21 Nov 2014 15:23:01 -0500
Subject: [PATCH] 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 |  4 ++-
 gtsam/inference/MetisIndex-inl.h   |  4 +--
 gtsam/inference/MetisIndex.h       |  8 +++---
 gtsam/inference/Ordering.cpp       | 43 +++++++++++++++++++++++++-----
 4 files changed, 46 insertions(+), 13 deletions(-)

diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp
index b9ab663d9..4d39a191a 100644
--- a/examples/StereoVOExample_large.cpp
+++ b/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;
diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h
index d9fb4cfd1..006ba075f 100644
--- a/gtsam/inference/MetisIndex-inl.h
+++ b/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<int, FastSet<int> >::iterator adjMapIt;
+  std::map<Key, FastSet<Key> > adjMap;
+  std::map<Key, FastSet<Key> >::iterator adjMapIt;
 	std::set<Key> keySet;
         
 	/* ********** Convert to CSR format ********** */
diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h
index b058b5564..476d34980 100644
--- a/gtsam/inference/MetisIndex.h
+++ b/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<int>  adj_; // Stores ajacency lists of all nodes, appended into a single vector
+	FastVector<Key> xadj_; // Index of node's adjacency list in adj
+  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<int>  adj() const { return    adj_; }
+	std::vector<Key> xadj() const { return   xadj_; }
+  std::vector<Key>  adj() const { return    adj_; }
 	size_t        nValues() const { return  nKeys_; }
 	size_t         minKey() const { return minKey_; }
 
diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp
index 654740163..e54ad3fde 100644
--- a/gtsam/inference/Ordering.cpp
+++ b/gtsam/inference/Ordering.cpp
@@ -204,12 +204,43 @@ namespace gtsam {
   {
     gttic(Ordering_METIS);
 
-    vector<idx_t> xadj = met.xadj();
-    vector<idx_t>  adj = met.adj();
-		size_t minKey = met.minKey();
+    vector<Key> xadj = met.xadj();
+    vector<Key>  adj = met.adj();
+		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;
   }