/* ---------------------------------------------------------------------------- * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) * See LICENSE for the license information * -------------------------------------------------------------------------- */ /** * @file Pose2.h * @brief 2D Pose * @author: Frank Dellaert * @author: Richard Roberts */ // \callgraph #pragma once #include #include #include #include namespace gtsam { /** * A 2D pose (Point2,Rot2) * @ingroup geometry */ class Pose2 { public: static const size_t dimension = 3; /** Pose Concept requirements */ typedef Rot2 Rotation; typedef Point2 Translation; private: Rot2 r_; Point2 t_; public: /** default constructor = origin */ Pose2() {} // default is origin /** copy constructor */ Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {} /** * construct from (x,y,theta) * @param x x coordinate * @param y y coordinate * @param theta angle with positive X-axis */ Pose2(double x, double y, double theta) : r_(Rot2::fromAngle(theta)), t_(x, y) { } /** construct from rotation and translation */ Pose2(double theta, const Point2& t) : r_(Rot2::fromAngle(theta)), t_(t) { } Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {} /** Constructor from 3*3 matrix */ Pose2(const Matrix &T) : r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { assert(T.rows() == 3 && T.cols() == 3); } /** Construct from canonical coordinates (Lie algebra) */ Pose2(const Vector& v) { *this = Expmap(v); } /** print with optional string */ void print(const std::string& s = "") const; /** assert equality up to a tolerance */ bool equals(const Pose2& pose, double tol = 1e-9) const; /** compose syntactic sugar */ inline Pose2 operator*(const Pose2& p2) const { return Pose2(r_*p2.r(), t_ + r_*p2.t()); } /** dimension of the variable - used to autodetect sizes */ inline static size_t Dim() { return dimension; } /** Lie requirements */ /** return DOF, dimensionality of tangent space = 3 */ inline size_t dim() const { return dimension; } /** * 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 */ Pose2 compose(const Pose2& p2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; /// MATLAB version returns shared pointer boost::shared_ptr compose_(const Pose2& p2) { return boost::shared_ptr(new Pose2(compose(p2))); } /** syntactic sugar for transform_from */ inline Point2 operator*(const Point2& point) const { return transform_from(point);} /** * Retraction from se(2) to SE(2) */ static Pose2 Retract(const Vector& v); /** * Inverse of retraction, from SE(2) to se(2) */ static Vector Unretract(const Pose2& p); /** Real versions of Expmap/Logmap */ static Pose2 Expmap(const Vector& xi); static Vector Logmap(const Pose2& p); /** default implementations of binary functions */ inline Pose2 retract(const Vector& v) const { return compose(Retract(v)); } inline Vector unretract(const Pose2& p2) const { return Unretract(between(p2));} /** non-approximated versions of expmap/logmap */ inline Pose2 expmap(const Vector& v) const { return compose(Expmap(v)); } inline Vector logmap(const Pose2& p2) const { return Logmap(between(p2));} /** * Return relative pose between p1 and p2, in p1 coordinate frame */ Pose2 between(const Pose2& p2, boost::optional H1=boost::none, boost::optional H2=boost::none) const; /// MATLAB version returns shared pointer boost::shared_ptr between_(const Pose2& p2) { return boost::shared_ptr(new Pose2(between(p2))); } /** return transformation matrix */ Matrix matrix() const; /** * Return point coordinates in pose coordinate frame */ Point2 transform_to(const Point2& point, boost::optional H1=boost::none, boost::optional H2=boost::none) const; /** * Return point coordinates in global frame */ Point2 transform_from(const Point2& point, boost::optional H1=boost::none, boost::optional H2=boost::none) const; /** * Calculate bearing to a landmark * @param point 2D location of landmark * @return 2D rotation \in SO(2) */ Rot2 bearing(const Point2& point, boost::optional H1=boost::none, boost::optional H2=boost::none) const; /** * Calculate bearing to another pose * @param point SO(2) location of other pose * @return 2D rotation \in SO(2) */ Rot2 bearing(const Pose2& point, boost::optional H1=boost::none, boost::optional H2=boost::none) const; /** * Calculate range to a landmark * @param point 2D location of landmark * @return range (double) */ double range(const Point2& point, boost::optional H1=boost::none, boost::optional H2=boost::none) const; /** * Calculate range to another pose * @param point 2D location of other pose * @return range (double) */ double range(const Pose2& point, boost::optional H1=boost::none, boost::optional H2=boost::none) const; /** * Calculate Adjoint map * Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) */ Matrix AdjointMap() const; inline Vector Adjoint(const Vector& xi) const { assert(xi.size() == 3); return AdjointMap()*xi; } /** * wedge for SE(2): * @param xi 3-dim twist (v,omega) where * omega is angular velocity * v (vx,vy) = 2D velocity * @return xihat, 3*3 element of Lie algebra that can be exponentiated */ static inline Matrix wedge(double vx, double vy, double w) { return Matrix_(3,3, 0.,-w, vx, w, 0., vy, 0., 0., 0.); } /** get functions for x, y, theta */ inline double x() const { return t_.x(); } inline double y() const { return t_.y(); } inline double theta() const { return r_.theta(); } /** shorthand access functions */ inline const Point2& t() const { return t_; } inline const Rot2& r() const { return r_; } /** full access functions required by Pose concept */ inline const Point2& translation() const { return t_; } inline const Rot2& rotation() const { return r_; } private: // Serialization function friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(t_); ar & BOOST_SERIALIZATION_NVP(r_); } }; // Pose2 /** specialization for pose2 wedge function (generic template in Lie.h) */ template <> inline Matrix wedge(const Vector& xi) { return Pose2::wedge(xi(0),xi(1),xi(2)); } /** * Calculate pose between a vector of 2D point correspondences (p,q) * where q = Pose2::transform_from(p) = t + R*p */ typedef std::pair Point2Pair; boost::optional align(const std::vector& pairs); } // namespace gtsam