From 6f6588457b7330799e261c66c5514fd963ff8623 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 19 Jan 2021 15:22:41 -0500
Subject: [PATCH 1/3] use streams instead of printf

---
 gtsam/base/Testable.h | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h
index 92c464940..2145360df 100644
--- a/gtsam/base/Testable.h
+++ b/gtsam/base/Testable.h
@@ -33,9 +33,9 @@
 
 #pragma once
 
-#include <boost/shared_ptr.hpp>
 #include <boost/concept_check.hpp>
-#include <stdio.h>
+#include <boost/shared_ptr.hpp>
+#include <iostream>
 #include <string>
 
 #define GTSAM_PRINT(x)((x).print(#x))
@@ -72,10 +72,10 @@ namespace gtsam {
   }; // \ Testable
 
   inline void print(float v, const std::string& s = "") {
-    printf("%s%f\n",s.c_str(),v);
+    std::cout << (s == "" ? s : s + " ") << v << std::endl;
   }
   inline void print(double v, const std::string& s = "") {
-    printf("%s%lf\n",s.c_str(),v);
+    std::cout << (s == "" ? s : s + " ") << v << std::endl;
   }
 
   /** Call equal on the object */

From 56eb1bb808483bcbb1a369034ca5e73fe06e4640 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 19 Jan 2021 15:23:14 -0500
Subject: [PATCH 2/3] use of passed in stream for print capture

---
 gtsam/base/Matrix.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp
index 551bdac10..41a80629b 100644
--- a/gtsam/base/Matrix.cpp
+++ b/gtsam/base/Matrix.cpp
@@ -153,7 +153,7 @@ const Eigen::IOFormat& matlabFormat() {
 /* ************************************************************************* */
 //3 argument call
 void print(const Matrix& A, const string &s, ostream& stream) {
-  cout << s << A.format(matlabFormat()) << endl;
+  stream << s << A.format(matlabFormat()) << endl;
 }
 
 /* ************************************************************************* */

From 2168cd4a04da47478eb1985d1268b157c03c1fc5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 19 Jan 2021 15:39:37 -0500
Subject: [PATCH 3/3] stream printing for Pose2

---
 gtsam/geometry/Pose2.cpp | 8 +++++++-
 gtsam/geometry/Pose2.h   | 4 ++++
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 71df0f753..bebe53dfa 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -48,7 +48,13 @@ Matrix3 Pose2::matrix() const {
 
 /* ************************************************************************* */
 void Pose2::print(const string& s) const {
-  cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl;
+  cout << s << this << endl;
+}
+
+/* ************************************************************************* */
+std::ostream &operator<<(std::ostream &os, const Pose2& pose) {
+  os << "(" << pose.x() << ", " << pose.y() << ", " << pose.theta() << ")";
+  return os;
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 6372779c3..a54951728 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -287,6 +287,10 @@ public:
    */
   static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(2, 2); }
 
+  /// Output stream operator
+  GTSAM_EXPORT
+  friend std::ostream &operator<<(std::ostream &os, const Pose2& p);
+
   /// @}
 
  private: