diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index d8060cfcf..8f2b6c3e2 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -23,6 +23,12 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const Point2& p) { + os << p.transpose(); + return os; +} + /* ************************************************************************* */ double norm2(const Point2& p, OptionalJacobian<1,2> H) { double r = std::sqrt(p.x() * p.x() + p.y() * p.y()); diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index e6574fe41..312e65b3b 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -26,6 +26,8 @@ namespace gtsam { /// it is now possible to just typedef Point2 to Vector2 typedef Vector2 Point2; +GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2 &p); + /// Distance of the point from the origin, with Jacobian GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none); diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 7a46f5988..c976f8bcc 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -22,6 +22,12 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const Point3& p) { + os << p.transpose(); + return os; +} + /* ************************************************************************* */ double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) { diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 19e328022..df0feff4f 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -33,6 +33,8 @@ namespace gtsam { /// it is now possible to just typedef Point3 to Vector3 typedef Vector3 Point3; +GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3 &p); + // Convenience typedef typedef std::pair Point3Pair; GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index d38b76255..48dd3bc3e 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -215,7 +215,7 @@ TEST(NavState, Stream) os << state; string expected; - expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: 0\n0\n0\nv: 0\n0\n0"; + expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: 0 0 0\nv: 0 0 0"; EXPECT(os.str() == expected); }