Class CPose2D

Defined in File CPose2D.h

Inheritance Relationships

Base Types

public mrpt::poses::CPose< CPose2D, 3 > (Template Class CPose)
public mrpt::serialization::CSerializable
public mrpt::Stringifyable

Class Documentation

class CPose2D : public mrpt::poses::CPose<CPose2D, 3>, public mrpt::serialization::CSerializable, public mrpt::Stringifyable

SE(2) rigid-body pose with cached trigonometry — the preferred class for repeated composition.

Represents an element of SE(2) = SO(2) ⋉ R² as the triplet (x, y, φ):

(x, y) — translation in metres.
φ (phi) — counter-clockwise heading angle in radians.

The equivalent 3x3 homogeneous matrix is:

*   T = | cos(phi)  -sin(phi)  x |
*       | sin(phi)   cos(phi)  y |
*       |    0          0      1 |
*

Use this class instead of mrpt::math::TPose2D when pose–point composition (composePoint, operator+) is called repeatedly with the same pose. CPose2D caches cos(φ) and sin(φ), recomputing them only when φ changes, making repeated transforms significantly cheaper.

Operator semantics (SE(2) group):

a + b (⊕): pose composition — maps the frame of b into the frame of a. In matrix form: T_a · T_b.
a - b (⊖): relative pose — the pose of a expressed in the frame of b. In matrix form: T_b⁻¹ · T_a. Equivalent to b.inverse() + a.
operator- (unary): SE(2) group inverse. Not elementwise negation.
pose + point: applies the SE(2) transformation to a point (composePoint).

Serializable: supports mrpt::serialization::CArchive and YAML schema serialization.

../../output_staging/generated/doxygen/xml/CPose2D.gif

See also

mrpt::math::TPose2D, CPoseOrPoint, CPoint2D, CPose3D

Note

See also: “A tutorial on SE(3) transformation parameterizations and

on-manifold optimization”, J.L. Blanco. blanco_se3_tutorial

STL-like methods and typedefs

using value_type = double: The type of the elements

using reference = double&

using const_reference = double

using size_type = std::size_t

using difference_type = std::ptrdiff_t

static constexpr std::size_t static_size = 3

static inline constexpr size_type size()

static inline constexpr bool empty()

static inline constexpr size_type max_size()

static inline void resize(size_t n)

Public Types

enum [anonymous]

Values:

enumerator is_3D_val

enum [anonymous]

Values:

enumerator rotation_dimensions

enum [anonymous]

Values:

enumerator is_PDF_val

using type_value = CPose2D : Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses

Public Functions

CPose2D(): Default constructor (all coordinates to 0)

CPose2D(const double x, const double y, const double phi): Constructor from an initial value of the pose.

explicit CPose2D(const CPoint2D&): Constructor from a CPoint2D object.

explicit CPose2D(const CPose3D&): Projects a CPose3D into SE(2) by copying (x,y) and mapping yaw→phi; z, pitch and roll are discarded. Information is irreversibly lost when pitch ≠ 0 or roll ≠ 0.

explicit CPose2D(const mrpt::math::TPose2D&): Constructor from lightweight object.

mrpt::math::TPose2D asTPose() const

explicit CPose2D(const CPoint3D&): Constructor from CPoint3D with information loss.

inline CPose2D(TConstructorFlags_Poses): Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument

inline double phi() const: Get the phi angle of the 2D pose (in radians)

inline double &phi(): This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

inline double phi_cos() const: Returns cos(φ), using a cached value that is recomputed only when φ changes. This avoids repeated calls to std::cos() in tight composition loops.

inline double phi_sin() const: Returns sin(φ), using a cached value that is recomputed only when φ changes. This avoids repeated calls to std::sin() in tight composition loops.

inline void phi(double angle): Set the phi angle of the 2D pose (in radians)

inline void phi_incr(const double Aphi): Increment the PHI angle (without checking the 2 PI range, call normalizePhi is needed)

void asVector(vector_t &v) const: Returns a 1x3 vector with [x y phi]

inline mrpt::math::TPoint2D translation() const: Returns the (x,y) translational part of the SE(2) transformation.

void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const

Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).

See also

getInverseHomogeneousMatrix

void getRotationMatrix(mrpt::math::CMatrixDouble22 &R) const: Returns the SE(2) 2x2 rotation matrix

void getRotationMatrix(mrpt::math::CMatrixDouble33 &R) const: Returns the equivalent SE(3) 3x3 rotation matrix, with (2,2)=1.

template<class MATRIX22> inline MATRIX22 getRotationMatrix() const

CPose2D operator+(const CPose2D &D) const

SE(2) group composition: ret = this ⊕ D.

If this is the pose of frame A in world W, and D is the pose of frame B in frame A, then ret is the pose of B in W. In matrix form: T_ret = T_this · T_D.

void composeFrom(const CPose2D &A, const CPose2D &B): In-place SE(2) group composition: computes this = A ⊕ B and stores the result in this. Slightly more efficient than this = A + B as it avoids a temporary.

Note

A or B may safely alias this.

CPose3D operator+(const CPose3D &D) const: Lifts this SE(2) pose into SE(3) (z=0, pitch=roll=0), then composes with D in SE(3). Returns the full SE(3) result.

CPoint2D operator+(const CPoint2D &u) const: Applies the SE(2) transformation to a CPoint2D (pose–point composition): ret = this ⊕ u.

void composePoint(double lx, double ly, double &gx, double &gy) const

Transforms a 2D point from the local frame of this pose into the global frame. Computes: (gx, gy) = R * (lx, ly)^T + t

See also

inverseComposePoint

Parameters:

lx, ly – Point coordinates in the local frame.
gx, gy – [out] Point coordinates in the global frame.

void composePoint(const mrpt::math::TPoint2D &l, mrpt::math::TPoint2D &g) const: This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

mrpt::math::TPoint3D composePoint(const mrpt::math::TPoint3D &l) const

void composePoint(const mrpt::math::TPoint3D &l, mrpt::math::TPoint3D &g) const

void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz) const

void inverseComposePoint(const double gx, const double gy, double &lx, double &ly) const

Transforms a 2D point from the global frame into the local frame of this pose. Computes: (lx, ly) = R^T * ((gx, gy)^T - t)

See also

composePoint

Parameters:

gx, gy – Point coordinates in the global frame.
lx, ly – [out] Point coordinates in the local frame.

void inverseComposePoint(const mrpt::math::TPoint2D &g, mrpt::math::TPoint2D &l) const: This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

mrpt::math::TPoint2D inverseComposePoint(const mrpt::math::TPoint2D &g) const

CPoint3D operator+(const CPoint3D &u) const: The operator u = this (+) u` is the pose/point compounding operator.

void inverseComposeFrom(const CPose2D &A, const CPose2D &B)

In-place relative pose: computes this = A ⊖ B = B⁻¹ ⊕ A and stores the result in this. Slightly more efficient than this = A - B as it avoids a temporary.

See also

composeFrom, composePoint

Note

A or B may safely alias this.

void inverse()

Replaces this pose with its SE(2) group inverse in-place. After calling this, original * this = Identity.

See also

operator-

Note

Not the same as negating all components individually.

inline CPose2D operator-(const CPose2D &b) const: SE(2) relative pose: ret = this ⊖ b = b⁻¹ ⊕ this. Returns the pose of this expressed in the frame of b.

CPose3D operator-(const CPose3D &b) const: The operator a (-) b is the pose inverse compounding operator.

void AddComponents(const CPose2D &p): Scalar sum of components: This is different from poses composition, which is implemented as “+” operators in “CPose” derived classes.

void operator*=(const double s): Scalar multiplication.

CPose2D &operator+=(const CPose2D &b): Make this = this (+) b

void normalizePhi(): Forces “phi” to be in the range [-pi,pi];

CPose2D getOppositeScalar() const: Return the opposite of the current pose instance by taking the negative of all its components individually

virtual std::string asString() const override

Returns a human-readable textual representation of the object (eg: “[x y

yaw]”, yaw in degrees)

See also

fromString

void fromString(const std::string &s)

Set the current object value from a string generated by ‘asString’ (eg: “[0.02 1.04 -0.8]” )

See also

asString

Throws:: std::exception – On invalid format

void fromStringRaw(const std::string &s): Same as fromString, but without requiring the square brackets in the string

inline double operator[](unsigned int i) const

inline double &operator[](unsigned int i)

inline void changeCoordinatesReference(const CPose2D &p): makes: this = p (+) this

double distance2DFrobeniusTo(const CPose2D &p) const: Returns the 2D distance from this pose/point to a 2D pose using the Frobenius distance.

inline const type_value &getPoseMean() const

inline type_value &getPoseMean()

Public Members

mrpt::math::CVectorFixedDouble<2> m_coords: [x,y]

Public Static Functions

static inline CPose2D Identity(): Returns the identity transformation

static inline CPose2D FromString(const std::string &s)

static inline constexpr bool is_3D()

static inline constexpr bool is_PDF()

Protected Functions

void update_cached_cos_sin() const

Protected Attributes

double m_phi = {.0}: The orientation of the pose, in radians.

mutable double m_cosphi = {1.0}: Precomputed cos() & sin() of phi.

double m_sinphi = {.0}

mutable bool m_cossin_uptodate = {false}