From c520bf2b47055ca196c1919d14df9375ec1bb17a Mon Sep 17 00:00:00 2001
From: Andrew Melim <Andrew.Melim@gatech.edu>
Date: Fri, 7 Nov 2014 22:38:37 -0500
Subject: [PATCH] Working METIS ordering example.

---
 gtsam/inference/MetisIndex-inl.h       | 19 ++++++++-----------
 gtsam/inference/MetisIndex.h           |  8 +++++---
 gtsam/inference/Ordering.cpp           | 12 +++++++++---
 gtsam/inference/tests/testOrdering.cpp |  8 +++++++-
 4 files changed, 29 insertions(+), 18 deletions(-)

diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h
index cab184ad0..d9fb4cfd1 100644
--- a/gtsam/inference/MetisIndex-inl.h
+++ b/gtsam/inference/MetisIndex-inl.h
@@ -55,21 +55,18 @@ void MetisIndex::augment(const FactorGraph<FACTOR>& factors)
 		
 	// Starting with a nonzero key crashes METIS
 	// Find the smallest key in the graph
-	size_t minKey = *keySet.begin(); // set is ordered
+	minKey_ = *keySet.begin(); // set is ordered
 		
 	xadj_.push_back(0);// Always set the first index to zero
 	for (adjMapIt = adjMap.begin(); adjMapIt != adjMap.end(); ++adjMapIt) {
-			std::vector<Key> temp;
-			// Copy from the FastSet into a temporary vector
-			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);
-			xadj_.push_back(adj_.size());
+		std::vector<Key> temp;
+		// Copy from the FastSet into a temporary vector
+		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);
+		xadj_.push_back(adj_.size());
 	}
-
-	// Normalize, subtract the smallest key
-	std::transform(adj_.begin(), adj_.end(), adj_.begin(), std::bind2nd(std::minus<size_t>(), minKey));
 }
 
 }
diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h
index eaff2546f..57d097876 100644
--- a/gtsam/inference/MetisIndex.h
+++ b/gtsam/inference/MetisIndex.h
@@ -45,6 +45,7 @@ namespace gtsam {
 		FastVector<int>  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_;
 
 	public:
 		/// @name Standard Constructors
@@ -69,9 +70,10 @@ namespace gtsam {
 		template<class FACTOR>
 		void augment(const FactorGraph<FACTOR>& factors);
 
-		std::vector<int> xadj() const { return  xadj_; }
-		std::vector<int>  adj() const { return   adj_; }
-		size_t        nValues() const { return nKeys_; }
+		std::vector<int> xadj() const { return   xadj_; }
+		std::vector<int>  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 2618d8f50..654740163 100644
--- a/gtsam/inference/Ordering.cpp
+++ b/gtsam/inference/Ordering.cpp
@@ -206,6 +206,10 @@ namespace gtsam {
 
     vector<idx_t> xadj = met.xadj();
     vector<idx_t>  adj = met.adj();
+		size_t minKey = met.minKey();
+
+		// Normalize, subtract the smallest key
+		std::transform(adj.begin(), adj.end(), adj.begin(), std::bind2nd(std::minus<size_t>(), minKey));
 
     vector<idx_t> perm, iperm;
 
@@ -228,9 +232,11 @@ namespace gtsam {
     }
 
 	  result.resize(size);
-	  for (size_t j = 0; j < size; ++j)
-		  result[j] = perm[j];
-      return result;
+		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;
+		}
+    return result;
   }
 
   /* ************************************************************************* */
diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp
index f2a0e1443..7e1ccb838 100644
--- a/gtsam/inference/tests/testOrdering.cpp
+++ b/gtsam/inference/tests/testOrdering.cpp
@@ -156,10 +156,16 @@ TEST(Ordering, csr_format_3) {
 
   vector<int> xadjExpected{ 0, 1, 4, 6, 8, 10 };
   vector<int> adjExpected{ 1, 0, 2, 4, 1, 3, 2, 4, 1, 3 };
+	size_t minKey = mi.minKey();
+
+	vector<int> adjAcutal = mi.adj();
+
+	// Normalize, subtract the smallest key
+	std::transform(adjAcutal.begin(), adjAcutal.end(), adjAcutal.begin(), std::bind2nd(std::minus<size_t>(), minKey));
 
   EXPECT(xadjExpected == mi.xadj());
   EXPECT(adjExpected.size() == mi.adj().size());
-  EXPECT(adjExpected == mi.adj());
+	EXPECT(adjExpected == adjAcutal);
 
 }