diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 496400a04..0e4bf4824 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -79,7 +79,9 @@ public: *this = Expmap(v); } - // Testable requirements + + /// @name Testable + /// @{ /** print with optional string */ void print(const std::string& s = "") const; @@ -87,30 +89,19 @@ public: /** assert equality up to a tolerance */ bool equals(const Pose2& pose, double tol = 1e-9) const; - // Manifold requirements + /// @} + /// @name Group + /// @{ - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /// identity for group operation + inline static Pose2 identity() { + return Pose2(); + } - - /** return DOF, dimensionality of tangent space = 3 */ - inline size_t dim() const { return dimension; } - - /** Apply tangent space update to an object */ - Pose2 retract(const Vector& v) const; - - /** @return local coordinates of p2 centered at *this */ - Vector localCoordinates(const Pose2& p2) const; - - // Group Requirements - - /** inverse transformation with derivatives */ + /// inverse transformation with derivatives Pose2 inverse(boost::optional H1=boost::none) const; - /** - * compose this transformation onto another (first *this and then p2) - * With optional derivatives - */ + /// compose this transformation onto another (first *this and then p2) Pose2 compose(const Pose2& p2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; @@ -120,11 +111,43 @@ public: return boost::shared_ptr(new Pose2(compose(p2))); } - /** compose syntactic sugar */ + /// compose syntactic sugar inline Pose2 operator*(const Pose2& p2) const { return Pose2(r_*p2.r(), t_ + r_*p2.t()); } + /// @} + /// @name Manifold + /// @{ + + /// Dimensionality of tangent space = 3 DOF - used to autodetect sizes + inline static size_t Dim() { return dimension; } + + + /// Dimensionality of tangent space = 3 DOF + inline size_t dim() const { return dimension; } + + /// Retraction from R^3 to Pose2 manifold neighborhood around current pose + Pose2 retract(const Vector& v) const; + + /// Local 3D coordinates of Pose2 manifold neighborhood around current pose + Vector localCoordinates(const Pose2& p2) const; + + /// @} + /// @name Lie Group + /// @{ + + /// Exponential map from Lie algebra se(2) to SE(2) + static Pose2 Expmap(const Vector& xi); + + /// Exponential map from SE(2) to Lie algebra se(2) + static Vector Logmap(const Pose2& p); + + /// @} + + /** return transformation matrix */ + Matrix matrix() const; + /** * Return relative pose between p1 and p2, in p1 coordinate frame */ @@ -137,22 +160,6 @@ public: return boost::shared_ptr(new Pose2(between(p2))); } - /** identity */ - inline static Pose2 identity() { - return Pose2(); - } - - // Lie Group requirements - - /** Exponential map using canonical coordinates */ - static Pose2 Expmap(const Vector& xi); - - /** Log map returning local coordinates */ - static Vector Logmap(const Pose2& p); - - /** return transformation matrix */ - Matrix matrix() const; - /** * Return point coordinates in pose coordinate frame */