From 1251ba9abb7e27ea49e8ba56d3a410eb16c07507 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 6 Jun 2013 01:21:34 +0000
Subject: [PATCH] ostream << operator

---
 gtsam/geometry/Pose3.cpp           | 7 +++++++
 gtsam/geometry/Pose3.h             | 3 +++
 gtsam/geometry/tests/testPose3.cpp | 9 +++++++++
 3 files changed, 19 insertions(+)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index adaa2ce69..faec92a6b 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -348,4 +348,11 @@ namespace gtsam {
     Point3 t = Point3(cq) - R * Point3(cp);
     return Pose3(R, t);
   }
+
+  /* ************************************************************************* */
+  std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
+    os << pose.rotation() << "\n" << pose.translation() << endl;
+    return os;
+  }
+
 } // namespace gtsam
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 9d7444233..234e2cad6 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -290,6 +290,9 @@ namespace gtsam {
      */
     static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(0, 2); }
 
+    /// Output stream operator
+    friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
+
   private:
     /** Serialization function */
     friend class boost::serialization::access;
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index ed8f850d2..5f4a68d3b 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -741,6 +741,15 @@ TEST( Pose3, adjointTranspose) {
   EXPECT(assert_equal(numericalH,actualH,1e-5));
 }
 
+/* ************************************************************************* */
+TEST( Pose3, stream)
+{
+  Pose3 T;
+  std::ostringstream os;
+  os << T;
+  EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n\n(0, 0, 0)\n");
+}
+
 /* ************************************************************************* */
 int main(){ TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */