diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 444f5d759..e188b2930 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -40,4 +40,10 @@ double Point2::norm() const { return sqrt(x_*x_ + y_*y_); } +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const Point2& p) { + os << '(' << p.x() << ", " << p.y() << ')'; + return os; +} + } // namespace gtsam diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 850f84b01..74ea16638 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -151,6 +151,11 @@ public: Point2 unit() const { return *this/norm(); } /** distance between two points */ + inline double distance(const Point2& p2) const { + return (p2 - *this).norm(); + } + + /** @deprecated The following function has been deprecated, use distance above */ inline double dist(const Point2& p2) const { return (p2 - *this).norm(); } @@ -184,6 +189,9 @@ public: inline void operator *= (double s) {x_*=s;y_*=s;} /// @} + /// Streaming + friend std::ostream &operator<<(std::ostream &os, const Point2& p); + private: /// @name Advanced Interface