Added stream operator <<

gtsam/geometry/Point3.cpp

@@ -14,10 +14,11 @@
  * @brief 3D Point
  */
 
-#include <cmath>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/base/Lie-inl.h>
 
+using namespace std;
+
 namespace gtsam {
 
 /** Explicit instantiation of base class to export members */
@@ -30,8 +31,8 @@ bool Point3::equals(const Point3 & q, double tol) const {
 
 /* ************************************************************************* */
 
-void Point3::print(const std::string& s) const {
-  std::cout << s << "(" << x_ << ", " << y_ <<  ", " << z_ << ")" << std::endl;
+void Point3::print(const string& s) const {
+  cout << s << "(" << x_ << ", " << y_ <<  ", " << z_ << ")" << endl;
 }
 
 /* ************************************************************************* */
@@ -49,14 +50,17 @@ Point3 Point3::operator+(const Point3& q) const {
 Point3 Point3::operator- (const Point3& q) const {
   return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ );
 }
+
 /* ************************************************************************* */
 Point3 Point3::operator*(double s) const {
   return Point3(x_ * s, y_ * s, z_ * s);
 }
+
 /* ************************************************************************* */
 Point3 Point3::operator/(double s) const {
   return Point3(x_ / s, y_ / s, z_ / s);
 }
+
 /* ************************************************************************* */
 Point3 Point3::add(const Point3 &q,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
@@ -64,6 +68,7 @@ Point3 Point3::add(const Point3 &q,
   if (H2) *H2 = eye(3,3);
   return *this + q;
 }
+
 /* ************************************************************************* */
 Point3 Point3::sub(const Point3 &q,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
@@ -71,20 +76,30 @@ Point3 Point3::sub(const Point3 &q,
   if (H2) *H2 = -eye(3,3);
   return *this - q;
 }
+
 /* ************************************************************************* */
 Point3 Point3::cross(const Point3 &q) const {
   return Point3( y_*q.z_ - z_*q.y_,
       z_*q.x_ - x_*q.z_,
       x_*q.y_ - y_*q.x_ );
 }
+
 /* ************************************************************************* */
 double Point3::dot(const Point3 &q) const {
   return ( x_*q.x_ + y_*q.y_ + z_*q.z_ );
 }
+
 /* ************************************************************************* */
 double Point3::norm() const {
   return sqrt( x_*x_ + y_*y_ + z_*z_ );
 }
+
+/* ************************************************************************* */
+ostream &operator<<(ostream &os, const Point3& p) {
+  os << '(' << p.x() << ", " << p.y() << ", " << p.z() << ')';
+  return os;
+}
+
 /* ************************************************************************* */
 
 } // namespace gtsam

gtsam/geometry/Point3.h

@@ -21,13 +21,14 @@
 
 #pragma once
 
-#include <cmath>
-#include <boost/serialization/nvp.hpp>
-
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/DerivedValue.h>
 #include <gtsam/base/Lie.h>
 
+#include <boost/serialization/nvp.hpp>
+
+#include <cmath>
+
 namespace gtsam {
 
   /**
@@ -202,6 +203,9 @@ namespace gtsam {
 
     /// @}
 
+    /// Output stream operator
+    friend std::ostream &operator<<(std::ostream &os, const Point3& p);
+
   private:
 
     /// @name Advanced Interface

gtsam/geometry/tests/testPoint3.cpp

@@ -71,6 +71,15 @@ TEST( Point3, dot)
   CHECK(ones.dot(Point3(1,1,0)) == 2);
 }
 
+/* ************************************************************************* */
+TEST( Point3, stream)
+{
+  Point3 p(1,2, -3);
+  std::ostringstream os;
+  os << p;
+  EXPECT(os.str() == "(1, 2, -3)");
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */