From a4fac2ab621092c57da7a70ea8bce968f41d4ee5 Mon Sep 17 00:00:00 2001
From: nsrinivasan7 <natesh.rvce@gmail.com>
Date: Sun, 12 Apr 2015 15:38:40 -0400
Subject: [PATCH] added cout style print statements for similarity

---
 gtsam_unstable/geometry/Similarity3.cpp | 6 ++++++
 gtsam_unstable/geometry/Similarity3.h   | 2 ++
 2 files changed, 8 insertions(+)

diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp
index cfc508ac7..8d75d767c 100644
--- a/gtsam_unstable/geometry/Similarity3.cpp
+++ b/gtsam_unstable/geometry/Similarity3.cpp
@@ -100,6 +100,12 @@ void Similarity3::print(const std::string& s) const {
   std::cout << "s: " << scale() << std::endl;
 }
 
+std::ostream &operator<<(std::ostream &os, const Similarity3& p) {
+  os << "[" << p.rotation().xyz().transpose() << " " << p.translation().vector().transpose() << " " <<
+      p.scale() << "]\';";
+  return os;
+}
+
 Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v,  ChartJacobian H) {
   // Will retracting or localCoordinating R work if R is not a unit rotation?
   // Also, how do we actually get s out?  Seems like we need to store it somewhere.
diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h
index 89f1010c4..65de32589 100644
--- a/gtsam_unstable/geometry/Similarity3.h
+++ b/gtsam_unstable/geometry/Similarity3.h
@@ -70,6 +70,8 @@ public:
   /// Print with optional string
   void print(const std::string& s) const;
 
+  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p);
+
   /// @}
   /// @name Group
   /// @{