Merge pull request #68 from borglab/feature/VectorValuePrint

Feature/vector value print

gtsam/linear/VectorValues.cpp

@@ -129,13 +129,32 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  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();
+  ostream& operator<<(ostream& os, const VectorValues& v) {
+    // Change print depending on whether we are using TBB
+#ifdef GTSAM_USE_TBB
+    map<Key, Vector> sorted;
+    for (const auto& key_value : v) {
+      sorted.emplace(key_value.first, key_value.second);
+    }
+    for (const auto& key_value : sorted)
+#else
+    for (const auto& key_value : v)
+#endif
+    {
+      os << "  " << StreamedKey(key_value.first) << ": " << key_value.second.transpose()
+         << "\n";
+    }
+    return os;
   }
 
+  /* ************************************************************************* */
+  void VectorValues::print(const string& str,
+                           const KeyFormatter& formatter) const {
+    cout << str << ": " << size() << " elements\n";
+    cout << key_formatter(formatter) << *this;
+    cout.flush();
+}
+
   /* ************************************************************************* */
   bool VectorValues::equals(const VectorValues& x, double tol) const {
     if(this->size() != x.size())

gtsam/linear/VectorValues.h

@@ -29,6 +29,7 @@
 
 #include <map>
 #include <string>
+#include <iosfwd>
 
 namespace gtsam {
 
@@ -228,8 +229,11 @@ namespace gtsam {
      */
     const_iterator find(Key j) const { return values_.find(j); }
 
+    /// overload operator << to print to stringstream
+    friend std::ostream& operator<<(std::ostream&, const VectorValues&);
+
     /** print required by Testable for unit testing */
-    void print(const std::string& str = "VectorValues: ",
+    void print(const std::string& str = "VectorValues",
         const KeyFormatter& formatter = DefaultKeyFormatter) const;
 
     /** equals required by Testable for unit testing */

gtsam/linear/tests/testVectorValues.cpp

@@ -17,6 +17,7 @@
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/linear/VectorValues.h>
+#include <gtsam/inference/LabeledSymbol.h>
 
 #include <CppUnitLite/TestHarness.h>
 
@@ -24,6 +25,8 @@
 #include <boost/assign/list_of.hpp>
 #include <boost/range/adaptor/map.hpp>
 
+#include <sstream>
+
 using namespace std;
 using namespace boost::assign;
 using boost::adaptors::map_keys;
@@ -228,6 +231,23 @@ TEST(VectorValues, vector_sub)
   EXPECT(assert_equal(expected, vv.vector(dims)));
 }
 
+/* ************************************************************************* */
+TEST(VectorValues, print)
+{
+  VectorValues vv;
+  vv.insert(0, (Vector(1) << 1).finished());
+  vv.insert(1, Vector2(2, 3));
+  vv.insert(2, Vector2(4, 5));
+  vv.insert(5, Vector2(6, 7));
+  vv.insert(7, Vector2(8, 9));
+
+  string expected =
+      "  0: 1\n  1: 2 3\n  2: 4 5\n  5: 6 7\n  7: 8 9\n";
+  stringstream actual;
+  actual << vv;
+  EXPECT(expected == actual.str());
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */