ostream << operator

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

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;

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);}
 /* ************************************************************************* */