fix bug in Pose2 print

gtsam/base/TestableAssertions.h

@@ -372,16 +372,19 @@ bool assert_stdout_equal(const std::string& expected, const V& actual) {
 
 /**
  * Capture print function output and compare against string.
+ *
+ * @param s: Optional string to pass to the print() method.
  */
-template<class V>
+template <class V>
-bool assert_print_equal(const std::string& expected, const V& actual) {
+bool assert_print_equal(const std::string& expected, const V& actual,
+                        const std::string& s = "") {
   // Redirect output to buffer so we can compare
   std::stringstream buffer;
   // Save the original output stream so we can reset later
   std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
 
   // We test against actual std::cout for faithful reproduction
-  actual.print();
+  actual.print(s);
 
   // Get output string and reset stdout
   std::string actual_ = buffer.str();

gtsam/geometry/Pose2.cpp

@@ -48,7 +48,7 @@ Matrix3 Pose2::matrix() const {
 
 /* ************************************************************************* */
 void Pose2::print(const string& s) const {
-  cout << s << this << endl;
+  std::cout << (s.empty() ? s : s + " ") << *this << std::endl;
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testPose2.cpp

@@ -14,16 +14,17 @@
  * @brief  Unit tests for Pose2 class
  */
 
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/geometry/Point2.h>
-#include <gtsam/geometry/Rot2.h>
-#include <gtsam/base/Testable.h>
-#include <gtsam/base/testLie.h>
-#include <gtsam/base/lieProxies.h>
-
 #include <CppUnitLite/TestHarness.h>
-#include <boost/optional.hpp>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/lieProxies.h>
+#include <gtsam/base/testLie.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Rot2.h>
+
 #include <boost/assign/std/vector.hpp>  // for operator +=
+#include <boost/optional.hpp>
 #include <cmath>
 #include <iostream>
 
@@ -910,6 +911,22 @@ TEST(Pose2 , TransformCovariance3) {
   }
 }
 
+/* ************************************************************************* */
+TEST(Pose2, Print) {
+  Pose2 pose(Rot2::identity(), Point2(1, 2));
+
+  // Generate the expected output
+  string s = "Planar Pose";
+  string expected_stdout = "(1, 2, 0)";
+  string expected1 = expected_stdout + "\n";
+  string expected2 = s + " " + expected1;
+
+  EXPECT(assert_stdout_equal(expected_stdout, pose));
+
+  EXPECT(assert_print_equal(expected1, pose));
+  EXPECT(assert_print_equal(expected2, pose, s));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;