From e8986ba38266fe1f0d271ca5da0053813b49eebe Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jun 2020 18:18:27 -0500 Subject: [PATCH 01/10] move Eigen format definition to Matrix.h --- gtsam/base/Matrix.cpp | 10 ---------- gtsam/base/Matrix.h | 12 ++++++++++++ 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index e2d8f71d1..1c34a71d4 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -139,16 +139,6 @@ Vector operator^(const Matrix& A, const Vector & v) { /* ************************************************************************* */ //3 argument call void print(const Matrix& A, const string &s, ostream& stream) { - static const Eigen::IOFormat matlab( - Eigen::StreamPrecision, // precision - 0, // flags - ", ", // coeffSeparator - ";\n", // rowSeparator - "\t", // rowPrefix - "", // rowSuffix - "[\n", // matPrefix - "\n]" // matSuffix - ); cout << s << A.format(matlab) << endl; } diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index fa70e5b00..61cbf3e2a 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -76,6 +76,18 @@ GTSAM_MAKE_MATRIX_DEFS(9); typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; +// Matrix formatting arguments when printing. Akin to Matlab style. +const Eigen::IOFormat matlab( + Eigen::StreamPrecision, // precision + 0, // flags + ", ", // coeffSeparator + ";\n", // rowSeparator + "\t", // rowPrefix + "", // rowSuffix + "[\n", // matPrefix + "\n]" // matSuffix +); + /** * equals with a tolerance */ From f235aaf8c4b5334dcb2f02ecb808ec4599835007 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jun 2020 18:18:55 -0500 Subject: [PATCH 02/10] use Eigen formatting for Rot3 ostream --- gtsam/geometry/Rot3.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 01f62b8cb..613c5b40f 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -222,10 +222,7 @@ pair RQ(const Matrix3& A) { /* ************************************************************************* */ ostream &operator<<(ostream &os, const Rot3& R) { - os << "\n"; - os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n"; - os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n"; - os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n"; + os << R.matrix().format(matlab) << endl; return os; } From 944a437272cd622515511ebc029923cb6038cb11 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 17 Jun 2020 14:54:55 -0500 Subject: [PATCH 03/10] don't align matrix columns --- gtsam/base/Matrix.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 61cbf3e2a..e19fb9dd6 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -79,7 +79,7 @@ typedef Eigen::Block ConstSubMatrix; // Matrix formatting arguments when printing. Akin to Matlab style. const Eigen::IOFormat matlab( Eigen::StreamPrecision, // precision - 0, // flags + Eigen::DontAlignCols, // flags set such that rowSpacers are not added ", ", // coeffSeparator ";\n", // rowSeparator "\t", // rowPrefix From f7141f4333ebbebeac7e836ddfff3c62bd6520f2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 17 Jun 2020 14:55:52 -0500 Subject: [PATCH 04/10] fix print tests for Rot3 and Pose3 --- gtsam/geometry/tests/testPose3.cpp | 10 +++++++--- gtsam/geometry/tests/testRot3.cpp | 3 ++- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 5808f36f8..0ad5f47b7 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -839,7 +839,8 @@ TEST( Pose3, stream) Pose3 T; std::ostringstream os; os << T; - EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n\n[0, 0, 0]';\n"); + string expected = "[\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n\n[0, 0, 0]';\n"; + EXPECT(os.str() == expected); } //****************************************************************************** @@ -1009,19 +1010,22 @@ TEST(Pose3, print) { std::stringstream expected; Point3 translation(1, 2, 3); + // Add expected rotation + expected << "R:\n[\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n"; + #ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS expected << "1\n" "2\n" "3;\n"; #else - expected << '[' << translation.x() << ", " << translation.y() << ", " << translation.z() << "]\';"; + expected << '[' << translation.x() << ", " << translation.y() << ", " << translation.z() << "]\';\n"; #endif // reset cout to the original stream std::cout.rdbuf(oldbuf); // Get substring corresponding to translation part - std::string actual = redirectStream.str().substr(38, 11); + std::string actual = redirectStream.str(); CHECK_EQUAL(expected.str(), actual); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 598c57b24..4e3a6d85e 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -608,7 +608,8 @@ TEST( Rot3, stream) Rot3 R; std::ostringstream os; os << R; - EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n"); + string expected = "[\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n"; + EXPECT(os.str() == expected); } /* ************************************************************************* */ From 0e42a96294a9c8ecbd9649876a89670427e68455 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 18 Jun 2020 11:09:37 -0500 Subject: [PATCH 05/10] Cleaned up printing of Rot3 --- gtsam/geometry/Rot3.cpp | 5 +++-- gtsam/geometry/Rot3.h | 2 +- gtsam/geometry/tests/testRot3.cpp | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 613c5b40f..318655491 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -32,7 +32,8 @@ namespace gtsam { /* ************************************************************************* */ void Rot3::print(const std::string& s) const { - gtsam::print((Matrix)matrix(), s); + cout << (s.empty() ? "R: " : s + " "); + gtsam::print((Matrix)matrix()); } /* ************************************************************************* */ @@ -222,7 +223,7 @@ pair RQ(const Matrix3& A) { /* ************************************************************************* */ ostream &operator<<(ostream &os, const Rot3& R) { - os << R.matrix().format(matlab) << endl; + os << R.matrix().format(matlab); return os; } diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index fc3a8b3f2..8ab7dd377 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -254,7 +254,7 @@ namespace gtsam { /// @{ /** print */ - void print(const std::string& s="R") const; + void print(const std::string& s="") const; /** equals with an tolerance */ bool equals(const Rot3& p, double tol = 1e-9) const; diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 4e3a6d85e..d5400494e 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -608,7 +608,7 @@ TEST( Rot3, stream) Rot3 R; std::ostringstream os; os << R; - string expected = "[\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n"; + string expected = "[\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]"; EXPECT(os.str() == expected); } From 7815a27e2159e7d816e0072bfeb38dc908ca4f48 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 18 Jun 2020 11:10:24 -0500 Subject: [PATCH 06/10] Cleaned up printing of Pose3 --- gtsam/geometry/Pose3.cpp | 10 ++++------ gtsam/geometry/tests/testPose3.cpp | 6 +++--- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 31033a027..e0fb6e5a5 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -104,9 +104,7 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, /* ************************************************************************* */ void Pose3::print(const string& s) const { - cout << s; - R_.print("R:\n"); - cout << t_ << ";" << endl; + cout << (s.empty() ? s : s + " ") << *this << endl; } /* ************************************************************************* */ @@ -436,9 +434,9 @@ boost::optional align(const vector& baPointPairs) { /* ************************************************************************* */ std::ostream &operator<<(std::ostream &os, const Pose3& pose) { - os << pose.rotation() << "\n"; - const Point3& t = pose.translation(); - os << '[' << t.x() << ", " << t.y() << ", " << t.z() << "]\';\n"; + // Both Rot3 and Point3 have ostream definitions so we use them. + os << "R: " << pose.rotation() << "\n"; + os << "t: " << pose.translation(); return os; } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 0ad5f47b7..caeed5770 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -839,7 +839,7 @@ TEST( Pose3, stream) Pose3 T; std::ostringstream os; os << T; - string expected = "[\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n\n[0, 0, 0]';\n"; + string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: [0, 0, 0]'"; EXPECT(os.str() == expected); } @@ -1011,14 +1011,14 @@ TEST(Pose3, print) { Point3 translation(1, 2, 3); // Add expected rotation - expected << "R:\n[\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n"; + expected << "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n"; #ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS expected << "1\n" "2\n" "3;\n"; #else - expected << '[' << translation.x() << ", " << translation.y() << ", " << translation.z() << "]\';\n"; + expected << "t: [" << translation.x() << ", " << translation.y() << ", " << translation.z() << "]'\n"; #endif // reset cout to the original stream From d3ac33ac18617cb7f8a6484a025f4ab34df0b65e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 18 Jun 2020 11:10:44 -0500 Subject: [PATCH 07/10] Cleaned up printing of NavState --- gtsam/navigation/NavState.cpp | 8 ++++---- gtsam/navigation/tests/testNavState.cpp | 12 ++++++++++++ 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 50949c761..1d191416f 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -88,15 +88,15 @@ Matrix7 NavState::matrix() const { //------------------------------------------------------------------------------ ostream& operator<<(ostream& os, const NavState& state) { - os << "R:" << state.attitude(); - os << "p:" << state.position() << endl; - os << "v:" << Point3(state.velocity()) << endl; + os << "R: " << state.attitude() << "\n"; + os << "p: " << state.position() << "\n"; + os << "v: " << Point3(state.velocity()); return os; } //------------------------------------------------------------------------------ void NavState::print(const string& s) const { - cout << s << *this << endl; + cout << (s.empty() ? s : s + " ") << *this << endl; } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 8ea8ce9b5..57945020c 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -237,6 +237,18 @@ TEST(NavState, CorrectPIM) { EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2)); } +/* ************************************************************************* */ +TEST(NavState, Stream) +{ + NavState state; + + std::ostringstream os; + os << state; + string 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); +} + + /* ************************************************************************* */ int main() { TestResult tr; From d7522ab97027d578ff1f76e1025e901edb57fb26 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 19 Jun 2020 16:03:40 -0500 Subject: [PATCH 08/10] moved matlab-style matrix format definition back to cpp, updated all formatters --- gtsam/base/Matrix.cpp | 16 +++++++++++++++- gtsam/base/Matrix.h | 14 +++----------- gtsam/geometry/Rot3.cpp | 2 +- gtsam/linear/JacobianFactor.cpp | 12 +----------- 4 files changed, 20 insertions(+), 24 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 1c34a71d4..551bdac10 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -136,10 +136,24 @@ Vector operator^(const Matrix& A, const Vector & v) { return A.transpose() * v; } +const Eigen::IOFormat& matlabFormat() { + static const Eigen::IOFormat matlab( + Eigen::StreamPrecision, // precision + Eigen::DontAlignCols, // flags set such that rowSpacers are not added + ", ", // coeffSeparator + ";\n", // rowSeparator + "\t", // rowPrefix + "", // rowSuffix + "[\n", // matPrefix + "\n]" // matSuffix + ); + return matlab; +} + /* ************************************************************************* */ //3 argument call void print(const Matrix& A, const string &s, ostream& stream) { - cout << s << A.format(matlab) << endl; + cout << s << A.format(matlabFormat()) << endl; } /* ************************************************************************* */ diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index e19fb9dd6..776badcd1 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -76,17 +76,9 @@ GTSAM_MAKE_MATRIX_DEFS(9); typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; -// Matrix formatting arguments when printing. Akin to Matlab style. -const Eigen::IOFormat matlab( - Eigen::StreamPrecision, // precision - Eigen::DontAlignCols, // flags set such that rowSpacers are not added - ", ", // coeffSeparator - ";\n", // rowSeparator - "\t", // rowPrefix - "", // rowSuffix - "[\n", // matPrefix - "\n]" // matSuffix -); +// Matrix formatting arguments when printing. +// Akin to Matlab style. +const Eigen::IOFormat& matlabFormat(); /** * equals with a tolerance diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 318655491..c1247da2e 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -223,7 +223,7 @@ pair RQ(const Matrix3& A) { /* ************************************************************************* */ ostream &operator<<(ostream &os, const Rot3& R) { - os << R.matrix().format(matlab); + os << R.matrix().format(matlabFormat()); return os; } diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 09a9a6103..bb83b672d 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -421,21 +421,11 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, /* ************************************************************************* */ void JacobianFactor::print(const string& s, const KeyFormatter& formatter) const { - static const Eigen::IOFormat matlab( - Eigen::StreamPrecision, // precision - 0, // flags - " ", // coeffSeparator - ";\n", // rowSeparator - "\t", // rowPrefix - "", // rowSuffix - "[\n", // matPrefix - "\n ]" // matSuffix - ); if (!s.empty()) cout << s << "\n"; for (const_iterator key = begin(); key != end(); ++key) { cout << boost::format(" A[%1%] = ") % formatter(*key); - cout << getA(key).format(matlab) << endl; + cout << getA(key).format(matlabFormat()) << endl; } cout << formatMatrixIndented(" b = ", getb(), true) << "\n"; if (model_) From e4b30fd5805aa2cb027a609b72a777c948653708 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 19 Jun 2020 16:06:33 -0500 Subject: [PATCH 09/10] use static_cast for Rot3 matrix --- gtsam/geometry/Rot3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index c1247da2e..aaf23e685 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -33,7 +33,7 @@ namespace gtsam { /* ************************************************************************* */ void Rot3::print(const std::string& s) const { cout << (s.empty() ? "R: " : s + " "); - gtsam::print((Matrix)matrix()); + gtsam::print(static_cast(matrix())); } /* ************************************************************************* */ From aaf632f583090cbacc9f6683538d68ab0cd79152 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 24 Jul 2020 02:30:35 -0500 Subject: [PATCH 10/10] fix test for FunctorizedFactor printing --- gtsam/nonlinear/tests/testFunctorizedFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp index 12dd6b91c..48ab73ad0 100644 --- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -131,7 +131,7 @@ TEST(FunctorizedFactor, Print) { "FunctorizedFactor(X0)\n" " measurement: [\n" " 1, 0;\n" - " 0, 1\n" + " 0, 1\n" "]\n" " noise model sigmas: 1 1 1 1 1 1 1 1 1\n";