print key_values in order

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 {