struct mrpt::math::TPose2D

Overview

Lightweight 2D rigid-body pose — an element of SE(2).

Parameterises the group SE(2) = SO(2) ⋉ R² as the triplet (x, y, φ) :

(x, y) — translation in metres.
φ (phi) — counter-clockwise rotation angle in radians, measured from the global X axis.

The equivalent 3x3 homogeneous matrix is:

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

**Composition (operator+)** implements the SE(2) group product: (this ⊕ b) maps a point expressed in frame b into the frame of this.

**Inverse composition (operator-)** implements (this ⊖ b) = b⁻¹ ⊕ this, i.e. the pose of this expressed in the frame of b.

This is a lightweight type without serialization or caching of trig values. Prefer mrpt::poses::CPose2D when composition is called repeatedly with the same pose (it caches cos/sin and supports serialization).

Coordinate access: operator[] with index 0→x, 1→y, 2→phi.

See also:

mrpt::poses::CPose2D, TPose3D, TPoint2D

#include <mrpt/math/TPose2D.h>

struct TPose2D:
    public mrpt::math::TPoseOrPoint,
    public mrpt::math::internal::ProvideStaticResize,
    public mrpt::math::internal::ProvidesStringConversion
{
    // fields

    static constexpr std::size_t static_size = 3;
    double x {.0};
    double y {.0};
    double phi {.0};

    // construction

    TPose2D(const TPoint2D& p);
    TPose2D(const TPoint3D& p);
    TPose2D(const TPose3D& p);
    TPose2D(double xx, double yy, double Phi);
    TPose2D();

    // methods

    static constexpr TPose2D Identity();

    template <typename Vector>
    static TPose2D FromVector(const Vector& v);

    static TPose2D FromString(const std::string& s);
    double& operator [] (size_t i);
    constexpr double operator [] (size_t i) const;

    template <typename Vector>
    void asVector(Vector& v) const;

    template <typename Vector>
    Vector asVector() const;

    void asString(std::string& s) const;
    mrpt::math::TPose2D operator + (const mrpt::math::TPose2D& b) const;
    mrpt::math::TPose2D operator - (const mrpt::math::TPose2D& b) const;
    mrpt::math::TPoint2D composePoint(const TPoint2D l) const;
    mrpt::math::TPoint2D operator + (const mrpt::math::TPoint2D& b) const;
    mrpt::math::TPoint2D inverseComposePoint(const TPoint2D g) const;
    const mrpt::math::TPoint2D translation() const;
    double norm() const;
    void normalizePhi();
    void fromString(const std::string& s);
};

Inherited Members

public:
    // methods

    void resize(std::size_t n);

Fields

double x {.0}

Translation along the global X axis (metres).

double y {.0}

Translation along the global Y axis (metres).

double phi {.0}

Heading angle phi in radians, counter-clockwise from the global X axis.

Range: any real value; use normalizePhi() to wrap to (-pi, pi].

Construction

TPose2D(const TPoint2D& p)

Constructor from TPoint2D: copies (x,y), sets phi=0 (pure translation, no rotation).

TPose2D(const TPoint3D& p)

Constructor from TPoint3D: copies (x,y), sets phi=0; the z coordinate is discarded.

TPose2D(const TPose3D& p)

Constructor from TPose3D : copies (x,y), maps yaw→phi; z, pitch and roll are discarded.

This is a projection from SE(3) onto SE(2); information is irreversibly lost when pitch ≠ 0 or roll ≠ 0.

See also:

TPose3D

TPose2D(double xx, double yy, double Phi)

Constructor from coordinates.

TPose2D()

Default fast constructor.

Initializes to zeros.

Methods

static constexpr TPose2D Identity()

Returns the identity transformation.

template <typename Vector>
static TPose2D FromVector(const Vector& v)

Builds from the first 3 elements of a vector-like object: [x y phi].

Parameters:

Vector

It can be std::vector<double>, Eigen::VectorXd, etc.

double& operator [] (size_t i)

Coordinate access using operator[].

Order: x,y,phi

constexpr double operator [] (size_t i) const

Coordinate access using operator[].

Order: x,y,phi

template <typename Vector>
void asVector(Vector& v) const

Gets the pose as a vector of doubles.

Parameters:

Vector

It can be std::vector<double>, Eigen::VectorXd, etc.

template <typename Vector>
Vector asVector() const

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

void asString(std::string& s) const

Returns a human-readable textual representation of the object (eg: “[x y yaw]”, yaw in degrees)

See also:

fromString

mrpt::math::TPose2D operator + (const mrpt::math::TPose2D& b) const

SE(2) group composition — “⊕” operator: ret = this ⊕ b.

Computes the pose obtained by first applying transformation this, then b. In matrix form: T_ret = T_this · T_b. If this is the pose of frame A in the world W, and b is the pose of frame B in frame A, then ret is the pose of B in W.

See also:

CPose2D, inverseComposeFrom

mrpt::math::TPose2D operator - (const mrpt::math::TPose2D& b) const

SE(2) inverse composition — “⊖” operator: ret = this ⊖ b = b⁻¹ ⊕ this.

Returns the pose of this expressed in the reference frame of b. In matrix form: T_ret = T_b⁻¹ · T_this. Useful for computing relative poses: if both this and b are poses in the world frame, ret is the pose of this as seen from b.

See also:

CPose2D

mrpt::math::TPoint2D composePoint(const TPoint2D l) const

Transforms a point from the local frame of this pose into the global (world) frame.

Equivalent to g = T_this · [l.x l.y 1]ᵀ (homogeneous coords).

Parameters:

l	Point in the local frame.

Returns:

Point in the global frame.

See also:

inverseComposePoint

mrpt::math::TPoint2D operator + (const mrpt::math::TPoint2D& b) const

Alias for composePoint() : pose + point applies the SE(2) transformation to the point.

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

Transforms a point from the global (world) frame into the local frame of this pose.

Equivalent to l = T_this⁻¹ · [g.x g.y 1]ᵀ (homogeneous coords).

Parameters:

g	Point in the global frame.

Returns:

Point in the local frame of this pose.

See also:

composePoint

const mrpt::math::TPoint2D translation() const

Returns the (x,y) translational part of the SE(2) transformation.

double norm() const

Euclidean norm of the translational part: |(x,y)|.

The angle phi is ignored.

void normalizePhi()

Wraps phi to the canonical range (-pi, pi].

void fromString(const std::string& s)

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

Parameters:

std::exception

On invalid format

See also:

asString