diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 705d84d25..f9a2e83a9 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -142,7 +142,7 @@ void print(const Matrix& A, const string &s, ostream& stream) { static const Eigen::IOFormat matlab( Eigen::StreamPrecision, // precision 0, // flags - " ", // coeffSeparator + ", ", // coeffSeparator ";\n", // rowSeparator " \t", // rowPrefix "", // rowSuffix diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0acadf5bc..31033a027 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -106,7 +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_.x() << ", " << t_.y() << ", " << t_.z() << "]\';"; + cout << t_ << ";" << endl; } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index fedd47620..8433bbb01 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -993,6 +993,32 @@ TEST(Pose3, Create) { EXPECT(assert_equal(numericalDerivative22(create, R, P2), actualH2, 1e-9)); } +/* ************************************************************************* */ +TEST(Pose3, print) { + std::stringstream redirectStream; + std::streambuf* ssbuf = redirectStream.rdbuf(); + std::streambuf* oldbuf = std::cout.rdbuf(); + // redirect cout to redirectStream + std::cout.rdbuf(ssbuf); + + Pose3 pose(Rot3::identity(), Point3(1, 2, 3)); + // output is captured to redirectStream + pose.print(); + + // Generate the expected output + std::stringstream expected; + Point3 translation(1, 2, 3); + expected << '[' << translation.x() << ", " << translation.y() << ", " << translation.z() << "]\';"; + + // reset cout to the original stream + std::cout.rdbuf(oldbuf); + + // Get substring corresponding to translation part + std::string actual = redirectStream.str().substr(47, 11); + + CHECK_EQUAL(expected.str(), actual); +} + /* ************************************************************************* */ int main() { TestResult tr;