diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index e2d8f71d1..551bdac10 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -136,20 +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) { - 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; + cout << s << A.format(matlabFormat()) << endl; } /* ************************************************************************* */ diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 1c1138438..21017b575 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -76,6 +76,10 @@ 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& matlabFormat(); + /** * equals with a tolerance */ diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 1b9285100..6361adc90 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -106,9 +106,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; } /* ************************************************************************* */ @@ -439,9 +437,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/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 01f62b8cb..aaf23e685 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(static_cast(matrix())); } /* ************************************************************************* */ @@ -222,10 +223,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(matlabFormat()); return os; } diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 8f24f07c8..9ba4e5df8 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/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index a169c833c..bb64192ef 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -862,7 +862,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 = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: [0, 0, 0]'"; + EXPECT(os.str() == expected); } //****************************************************************************** @@ -1032,19 +1033,22 @@ TEST(Pose3, print) { std::stringstream expected; Point3 translation(1, 2, 3); + // Add expected rotation + 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() << "]\';"; + expected << "t: [" << 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..d5400494e 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]"; + EXPECT(os.str() == expected); } /* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index b8a08be9e..2e0ffb034 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_) 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; 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";