From 7b4100794197bf8f0ff9984cd68b80966bbc46fc Mon Sep 17 00:00:00 2001
From: mxie32 <mxie32@gatech.edu>
Date: Fri, 14 Jun 2019 11:14:01 -0400
Subject: [PATCH]         print key_values in order

---
 gtsam/linear/VectorValues.cpp | 31 +++++++++++++++++++++++++------
 1 file changed, 25 insertions(+), 6 deletions(-)

diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp
index e8304e6e7..10556a073 100644
--- a/gtsam/linear/VectorValues.cpp
+++ b/gtsam/linear/VectorValues.cpp
@@ -128,13 +128,32 @@ namespace gtsam {
       v.setZero();
   }
 
-  /* ************************************************************************* */
-  void VectorValues::print(const string& str, const KeyFormatter& formatter) const {
-    cout << str << ": " << size() << " elements\n";
-    for(const value_type& key_value: *this)
-      cout << "  " << formatter(key_value.first) << ": " << key_value.second.transpose() << "\n";
-    cout.flush();
+/* ************************************************************************* */
+bool compare(std::pair<Key, Vector>& lhs, std::pair<Key, Vector>& rhs) {
+  return lhs.first < rhs.first;
+}
+
+void VectorValues::print(const string& str,
+                         const KeyFormatter& formatter) const {
+  cout << str << ": " << size() << " elements\n";
+  // Change print depending on whether we are using TBB
+#ifdef GTSAM_USE_TBB
+  std::vector<std::pair<Key, Vector>> vec;
+  vec.reserve(size());
+  for (const value_type& key_value : *this) {
+    vec.push_back(std::make_pair(key_value.first, key_value.second));
   }
+  sort(vec.begin(), vec.end(), compare);
+  for (const auto& key_value : vec)
+    cout << "  " << formatter(key_value.first) << ": "
+         << key_value.second.transpose() << "\n";
+#else
+  for (const value_type& key_value : *this)
+    cout << "  " << formatter(key_value.first) << ": "
+         << key_value.second.transpose() << "\n";
+#endif
+  cout.flush();
+}
 
   /* ************************************************************************* */
   bool VectorValues::equals(const VectorValues& x, double tol) const {