class mrpt::poses::CPose2D

Overview

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.

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

on-manifold optimization”, J.L. Blanco. [4]

See also:

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

#include <mrpt/poses/CPose2D.h>

class CPose2D:
    public mrpt::poses::CPose,
    public mrpt::serialization::CSerializable,
    public mrpt::Stringifyable
{
public:
    // typedefs

    typedef std::shared_ptr<mrpt::poses ::CPose2D> Ptr;
    typedef std::shared_ptr<const mrpt::poses ::CPose2D> ConstPtr;
    typedef std::unique_ptr<mrpt::poses ::CPose2D> UniquePtr;
    typedef std::unique_ptr<const mrpt::poses ::CPose2D> ConstUniquePtr;
    typedef double value_type;
    typedef double& reference;
    typedef double const_reference;
    typedef std::size_t size_type;
    typedef std::ptrdiff_t difference_type;
    typedef CPose2D type_value;

    // enums

    enum
    {
        is_3D_val = 0,
    };

    enum
    {
        rotation_dimensions = 2,
    };

    enum
    {
        is_PDF_val = 0,
    };

    // fields

    static constexpr const char* className = "mrpt::poses" "::" "CPose2D";
    static constexpr std::size_t static_size = 3;
    mrpt::math::CVectorFixedDouble<2> m_coords;

    // construction

    CPose2D();
    CPose2D(const double x, const double y, const double phi);
    CPose2D(const CPoint2D& p);
    CPose2D(const CPose3D& p);
    CPose2D(const mrpt::math::TPose2D& o);
    CPose2D(const CPoint3D& o);
    CPose2D(TConstructorFlags_Poses);

    // methods

    static constexpr auto getClassName();
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    static std::shared_ptr<CObject> CreateObject();

    template <typename... Args>
    static Ptr Create(Args&&... args);

    template <typename Alloc, typename... Args>
    static Ptr CreateAlloc(
        const Alloc& alloc,
        Args&&... args
        );

    template <typename... Args>
    static UniquePtr CreateUnique(Args&&... args);

    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    virtual mrpt::rtti::CObject* clone() const;
    static constexpr size_type size();
    static constexpr bool empty();
    static constexpr size_type max_size();
    static void resize(size_t n);
    static CPose2D Identity();
    static CPose2D FromString(const std::string& s);
    static constexpr bool is_3D();
    static constexpr bool is_PDF();
    mrpt::math::TPose2D asTPose() const;
    double phi() const;
    double& phi();
    double phi_cos() const;
    double phi_sin() const;
    void phi(double angle);
    void phi_incr(const double Aphi);
    void asVector(vector_t& v) const;
    mrpt::math::TPoint2D translation() const;
    void getHomogeneousMatrix(mrpt::math::CMatrixDouble44& out_HM) const;
    mrpt::math::CMatrixDouble44 getHomogeneousMatrix() const;
    void getRotationMatrix(mrpt::math::CMatrixDouble22& R) const;
    void getRotationMatrix(mrpt::math::CMatrixDouble33& R) const;
    mrpt::math::CMatrixDouble22 getRotationMatrix() const;

    template <class MATRIX22>
    MATRIX22 getRotationMatrixAs() const;

    CPose2D operator + (const CPose2D& D) const;
    void composeFrom(const CPose2D& A, const CPose2D& B);
    CPose3D operator + (const CPose3D& D) const;
    CPoint2D operator + (const CPoint2D& u) const;
    void composePoint(double lx, double ly, double& gx, double& gy) const;
    void composePoint(const mrpt::math::TPoint2D& l, mrpt::math::TPoint2D& g) const;
    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;
    void inverseComposePoint(const mrpt::math::TPoint2D& g, mrpt::math::TPoint2D& l) const;
    mrpt::math::TPoint2D inverseComposePoint(const mrpt::math::TPoint2D& g) const;
    CPoint3D operator + (const CPoint3D& u) const;
    void inverseComposeFrom(const CPose2D& A, const CPose2D& B);
    void inverse();
    CPose2D operator - (const CPose2D& b) const;
    CPose3D operator - (const CPose3D& b) const;
    void AddComponents(const CPose2D& p);
    void operator *= (const double s);
    CPose2D& operator += (const CPose2D& b);
    void normalizePhi();
    CPose2D getOppositeScalar() const;
    virtual std::string asString() const;
    void fromString(const std::string& s);
    void fromStringRaw(const std::string& s);
    double operator [] (unsigned int i) const;
    double& operator [] (unsigned int i);
    void changeCoordinatesReference(const CPose2D& p);
    double distance2DFrobeniusTo(const CPose2D& p) const;
    const type_value& getPoseMean() const;
    type_value& getPoseMean();
};

Inherited Members

public:
    // typedefs

    typedef std::shared_ptr<CObject> Ptr;
    typedef std::shared_ptr<const CObject> ConstPtr;
    typedef std::shared_ptr<CSerializable> Ptr;
    typedef std::shared_ptr<const CSerializable> ConstPtr;

    // methods

    double& x();
    double& y();
    void x(const double v);
    void y(const double v);
    void x_incr(const double v);
    void y_incr(const double v);
    CPoseOrPoint& operator = (const CPoseOrPoint&);
    CPoseOrPoint& operator = (CPoseOrPoint&&);
    const DERIVEDCLASS& derived() const;
    DERIVEDCLASS& derived();
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();

Typedefs

typedef std::shared_ptr<mrpt::poses ::CPose2D> Ptr

A type for the associated smart pointer.

typedef double value_type

The type of the elements.

typedef CPose2D type_value

Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses.

Fields

mrpt::math::CVectorFixedDouble<2> m_coords

[x,y]

Construction

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.

CPose2D(const CPoint2D& p)

Constructor from a CPoint2D object.

CPose2D(const CPose3D& p)

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.

CPose2D(const mrpt::math::TPose2D& o)

Constructor from lightweight object.

CPose2D(const CPoint3D& o)

Constructor from CPoint3D with information loss.

CPose2D(TConstructorFlags_Poses)

Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument.

Methods

virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const

Returns information about the class of an object in runtime.

virtual mrpt::rtti::CObject* clone() const

Returns a deep copy (clone) of the object, indepently of its class.

static CPose2D Identity()

Returns the identity transformation.

double phi() const

Get the phi angle of the 2D pose (in radians)

double& phi()

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

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.

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.

void phi(double angle)

Set the phi angle of the 2D pose (in radians)

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].

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).

Deprecated Use getHomogeneousMatrix() returning by value instead.

See also:

getInverseHomogeneousMatrix

mrpt::math::CMatrixDouble44 getHomogeneousMatrix() const

Returns the corresponding 4x4 homogeneous transformation matrix.

void getRotationMatrix(mrpt::math::CMatrixDouble22& R) const

Returns the SE(2) 2x2 rotation matrix.

Deprecated Use getRotationMatrix() returning by value instead.

void getRotationMatrix(mrpt::math::CMatrixDouble33& R) const

Returns the equivalent SE(3) 3x3 rotation matrix, with (2,2)=1.

Deprecated Use getRotationMatrix() returning by value instead.

mrpt::math::CMatrixDouble22 getRotationMatrix() const

Returns the SE(2) 2x2 rotation matrix by value.

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. 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.

An alternative, slightly more efficient way of doing \(G = P \oplus L\) with G and L being 2D points and P this 2D pose.

Computes: (gx, gy) = R * (lx, ly)^T + t

Parameters:

lx
ly	Point coordinates in the local frame.
gx
gy	Point coordinates in the global frame.

See also:

inverseComposePoint

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.

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

overload \(G = P \oplus L\) with G and L being 3D points and P this 2D pose (the “z” coordinate remains unmodified)

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)

Parameters:

gx
gy	Point coordinates in the global frame.
lx
ly	Point coordinates in the local frame.

See also:

composePoint

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.

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. A or B may safely alias this.

See also:

composeFrom, composePoint

void inverse()

Replaces this pose with its SE(2) group inverse in-place.

Convert this pose into its inverse, saving the result in itself.

After calling this, original * this = Identity. Not the same as negating all components individually.

See also:

operator-

operator-

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

Make \(this = this \oplus 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

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]” )

Parameters:

std::exception

On invalid format

See also:

asString

void fromStringRaw(const std::string& s)

Same as fromString, but without requiring the square brackets in the string.

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.