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;