added VectorValues::sorted()

Author: arutkowski

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

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