Print Vectors horizontally for easier reading

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());

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);
 

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) {

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<Point3, Point3> Point3Pair;
 GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);

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);
 }