From afd3330acb932b78ad5b2d55238d1f7418d804e0 Mon Sep 17 00:00:00 2001
From: Adam Rutkowski <arutkowski@users.noreply.github.com>
Date: Mon, 25 Mar 2024 16:05:47 -0500
Subject: [PATCH] added VectorValues::sorted()

---
 gtsam/linear/VectorValues.cpp | 22 ++++++++++++++--------
 gtsam/linear/VectorValues.h   |  3 +++
 2 files changed, 17 insertions(+), 8 deletions(-)

diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp
index 075e3b9ec7..7440c0b2b0 100644
--- a/gtsam/linear/VectorValues.cpp
+++ b/gtsam/linear/VectorValues.cpp
@@ -64,6 +64,13 @@ namespace gtsam {
     }
   }
 
+  /* ************************************************************************ */
+  std::map<Key, Vector> VectorValues::sorted() const {
+    std::map<Key, Vector> ordered;
+    for (const auto& kv : *this) ordered.emplace(kv);
+    return ordered;
+  }
+
   /* ************************************************************************ */
   VectorValues VectorValues::Zero(const VectorValues& other)
   {
@@ -130,11 +137,7 @@ namespace gtsam {
   GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const VectorValues& v) {
     // Change print depending on whether we are using TBB
 #ifdef GTSAM_USE_TBB
-    std::map<Key, Vector> sorted;
-    for (const auto& [key, value] : v) {
-      sorted.emplace(key, value);
-    }
-    for (const auto& [key, value] : sorted)
+    for (const auto& [key, value] : v.sorted())
 #else
     for (const auto& [key,value] : v)
 #endif
@@ -176,7 +179,12 @@ namespace gtsam {
     // Copy vectors
     Vector result(totalDim);
     DenseIndex pos = 0;
+#ifdef GTSAM_USE_TBB
+    // TBB uses un-ordered map, so inefficiently order them:
+    for (const auto& [key, value] : sorted()) {
+#else
     for (const auto& [key, value] : *this) {
+#endif
       result.segment(pos, value.size()) = value;
       pos += value.size();
     }
@@ -392,9 +400,7 @@ namespace gtsam {
     // Print out all rows.
 #ifdef GTSAM_USE_TBB
     // TBB uses un-ordered map, so inefficiently order them:
-    std::map<Key, Vector> ordered;
-    for (const auto& kv : *this) ordered.emplace(kv);
-    for (const auto& kv : ordered) {
+    for (const auto& kv : sorted()) {
 #else
     for (const auto& kv : *this) {
 #endif
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index 478cfd770c..7fbd43ffc6 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -77,6 +77,9 @@ namespace gtsam {
     typedef ConcurrentMap<Key, Vector> Values;  ///< Collection of Vectors making up a VectorValues
     Values values_;                             ///< Vectors making up this VectorValues
 
+    /** Sort by key (primarily for use with TBB, which uses an unordered map)*/
+    std::map<Key, Vector> sorted() const;
+
    public:
     typedef Values::iterator iterator;              ///< Iterator over vector values
     typedef Values::const_iterator const_iterator;  ///< Const iterator over vector values