From 1cba2dc3e7996e3422b357373971eaea1d3bb9cf Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Aug 2020 13:06:32 -0400 Subject: [PATCH 1/2] Print Vectors horizontally for easier reading --- gtsam/geometry/Point2.cpp | 6 ++++++ gtsam/geometry/Point2.h | 2 ++ gtsam/geometry/Point3.cpp | 6 ++++++ gtsam/geometry/Point3.h | 2 ++ gtsam/navigation/tests/testNavState.cpp | 2 +- 5 files changed, 17 insertions(+), 1 deletion(-) 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); } From 597026f58ab9541868de831144bf0e1a630cc028 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Aug 2020 17:03:41 -0400 Subject: [PATCH 2/2] Undo Point operator overloading and instead update NavState print --- gtsam/geometry/Point2.cpp | 6 ------ gtsam/geometry/Point2.h | 2 -- gtsam/geometry/Point3.cpp | 6 ------ gtsam/geometry/Point3.h | 2 -- gtsam/navigation/NavState.cpp | 4 ++-- 5 files changed, 2 insertions(+), 18 deletions(-) diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 8f2b6c3e2..d8060cfcf 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -23,12 +23,6 @@ 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 312e65b3b..e6574fe41 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -26,8 +26,6 @@ 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 c976f8bcc..7a46f5988 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -22,12 +22,6 @@ 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 df0feff4f..19e328022 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -33,8 +33,6 @@ 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/NavState.cpp b/gtsam/navigation/NavState.cpp index 1d191416f..7d4797132 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -89,8 +89,8 @@ Matrix7 NavState::matrix() const { //------------------------------------------------------------------------------ ostream& operator<<(ostream& os, const NavState& state) { os << "R: " << state.attitude() << "\n"; - os << "p: " << state.position() << "\n"; - os << "v: " << Point3(state.velocity()); + os << "p: " << state.position().transpose() << "\n"; + os << "v: " << state.velocity().transpose(); return os; }