diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 10556a073..2da1a12f1 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -128,31 +128,38 @@ namespace gtsam { v.setZero(); } -/* ************************************************************************* */ -bool compare(std::pair& lhs, std::pair& 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> vec; - vec.reserve(size()); - for (const value_type& key_value : *this) { - vec.push_back(std::make_pair(key_value.first, key_value.second)); + /* ************************************************************************* */ + bool compare(std::pair& lhs, std::pair& rhs) { + return lhs.first < rhs.first; } - sort(vec.begin(), vec.end(), compare); - for (const auto& key_value : vec) - cout << " " << formatter(key_value.first) << ": " - << key_value.second.transpose() << "\n"; + + ostream& operator<<(ostream& ss, const VectorValues& v) { + ss << "VectorValues: " + << ": " << v.size() << " elements\n"; + // Change print depending on whether we are using TBB +#ifdef GTSAM_USE_TBB + std::vector> vec; + vec.reserve(v.size()); + for (const auto& key_value : v) { + vec.push_back(std::make_pair(key_value.first, key_value.second)); + } + sort(vec.begin(), vec.end(), compare); + for (const auto& key_value : vec) + ss << " " << 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"; + for (const auto& key_value : v) + ss << " " << key_value.first << ": " << key_value.second.transpose() + << "\n"; #endif - cout.flush(); + return ss; + } + + /* ************************************************************************* */ + void VectorValues::print(const string& str, + const KeyFormatter& formatter) const { + cout << *this; + cout.flush(); } /* ************************************************************************* */ diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index b1d9de585..32a311848 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -29,6 +29,7 @@ #include #include +#include namespace gtsam { @@ -228,6 +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& ss, const VectorValues& v); + /** print required by Testable for unit testing */ void print(const std::string& str = "VectorValues: ", const KeyFormatter& formatter = DefaultKeyFormatter) const; diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index d1d9990b0..28600ecad 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -24,6 +24,8 @@ #include #include +#include + using namespace std; using namespace boost::assign; using boost::adaptors::map_keys; @@ -228,6 +230,24 @@ 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 = + "VectorValues: : 5 elements\n 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); } /* ************************************************************************* */