12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Cleaned up printing of Pose3

release/4.3a0
Varun Agrawal 2020-06-18 11:10:24 -05:00
parent 0e42a96294
commit 7815a27e21
2 changed files with 7 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/Pose3.cpp
View File

@ -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<Pose3> align(const vector<Point3Pair>& 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;
}

6
gtsam/geometry/tests/testPose3.cpp
View File

@ -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