Merge pull request #678 from borglab/fix/better-printing

Better printing

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

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 */

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

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: