diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 71df0f753..bebe53dfa 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -48,7 +48,13 @@ Matrix3 Pose2::matrix() const { /* ************************************************************************* */ void Pose2::print(const string& s) const { - cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl; + cout << s << this << endl; +} + +/* ************************************************************************* */ +std::ostream &operator<<(std::ostream &os, const Pose2& pose) { + os << "(" << pose.x() << ", " << pose.y() << ", " << pose.theta() << ")"; + return os; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 6372779c3..a54951728 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -287,6 +287,10 @@ public: */ static std::pair rotationInterval() { return std::make_pair(2, 2); } + /// Output stream operator + GTSAM_EXPORT + friend std::ostream &operator<<(std::ostream &os, const Pose2& p); + /// @} private: