diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index e8304e6e7..cd3ae815b 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/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 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()) diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index b1d9de585..fe6b5fcb2 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -29,6 +29,7 @@ #include #include +#include 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 */ diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index d1d9990b0..f97f96aaf 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -17,6 +17,7 @@ #include #include +#include #include @@ -24,6 +25,8 @@ #include #include +#include + 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); } /* ************************************************************************* */