From 7815a27e2159e7d816e0072bfeb38dc908ca4f48 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 18 Jun 2020 11:10:24 -0500
Subject: [PATCH] Cleaned up printing of Pose3

---
 gtsam/geometry/Pose3.cpp           | 10 ++++------
 gtsam/geometry/tests/testPose3.cpp |  6 +++---
 2 files changed, 7 insertions(+), 9 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 31033a027..e0fb6e5a5 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -104,9 +104,7 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
 
 /* ************************************************************************* */
 void Pose3::print(const string& s) const {
-  cout << s;
-  R_.print("R:\n");
-  cout << t_ << ";" << endl;
+  cout << (s.empty() ? s : s + " ") << *this << endl;
 }
 
 /* ************************************************************************* */
@@ -436,9 +434,9 @@ boost::optional<Pose3> align(const vector<Point3Pair>& baPointPairs) {
 
 /* ************************************************************************* */
 std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
-  os << pose.rotation() << "\n";
-  const Point3& t = pose.translation();
-  os << '[' << t.x() << ", " << t.y() << ", " << t.z() << "]\';\n";
+  // Both Rot3 and Point3 have ostream definitions so we use them.
+  os << "R: " << pose.rotation() << "\n";
+  os << "t: " << pose.translation();
   return os;
 }
 
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index 0ad5f47b7..caeed5770 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -839,7 +839,7 @@ TEST( Pose3, stream)
   Pose3 T;
   std::ostringstream os;
   os << T;
-  string expected = "[\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n\n[0, 0, 0]';\n";
+  string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: [0, 0, 0]'";
   EXPECT(os.str() == expected);
 }
 
@@ -1011,14 +1011,14 @@ TEST(Pose3, print) {
   Point3 translation(1, 2, 3);
 
   // Add expected rotation
-  expected << "R:\n[\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n";
+  expected << "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n";
 
 #ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
   expected << "1\n"
               "2\n"
               "3;\n";
 #else
-  expected << '[' << translation.x() << ", " << translation.y() << ", " << translation.z() << "]\';\n";
+  expected << "t: [" << translation.x() << ", " << translation.y() << ", " << translation.z() << "]'\n";
 #endif
 
   // reset cout to the original stream