class mrpt::poses::CPose2D

Overview

A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.

Use this class instead of lightweight mrpt::math::TPose2D when pose/point composition is to be called multiple times with the same pose, since this class caches calls to expensive trigronometric functions.

For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer to this documentation page

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

on-manifold optimization”, Jose-Luis Blanco. http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf

See also:

CPoseOrPoint, CPoint2D

#include <mrpt/poses/CPose2D.h>

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

    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,
    };

    enum
    {
        static_size = 3,
    };

    // fields

    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 size_type size();
    static constexpr bool empty();
    static constexpr size_type max_size();
    static void resize(size_t n);
    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;
    void getRotationMatrix(mrpt::math::CMatrixDouble22& R) const;
    void getRotationMatrix(mrpt::math::CMatrixDouble33& R) const;

    template <class MATRIX22>
    MATRIX22 getRotationMatrix() 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();
    virtual void setToNaN();
    static CPose2D Identity();
    static CPose2D FromString(const std::string& s);
    static constexpr bool is_3D();
    static constexpr bool is_PDF();
};

Inherited Members

public:
    // 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);
    const DERIVEDCLASS& derived() const;
    DERIVEDCLASS& derived();

Typedefs

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)

Aproximation!! Avoid its use, since information is lost.

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

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

Get a (cached) value of cos(phi), recomputing it only once when phi changes.

double phi_sin() const

Get a (cached) value of sin(phi), recomputing it only once when phi changes.

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

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.

CPose2D operator + (const CPose2D& D) const

The operator \(a = this \oplus D\) is the pose compounding operator.

void composeFrom(const CPose2D& A, const CPose2D& B)

Makes \(this = A \oplus B\).

A or B can be “this” without problems.

CPose3D operator + (const CPose3D& D) const

The operator \(a = this \oplus D\) is the pose compounding operator.

CPoint2D operator + (const CPoint2D& u) const

The operator \(u' = this \oplus u\) is the pose/point compounding operator.

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

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

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

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

overload \(G = P \oplus L\) with G and L being 2D points and P this 2D pose

mrpt::math::TPoint3D composePoint(const mrpt::math::TPoint3D& l) 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 composePoint(
    double lx,
    double ly,
    double lz,
    double& gx,
    double& gy,
    double& gz
    ) const

overload (the “z” coordinate remains unmodified)

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

Computes the 2D point L such as \(L = G \ominus this\).

See also:

composePoint, composeFrom

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

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 \oplus u\) is the pose/point compounding operator.

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

Makes \(this = A \ominus B\) this method is slightly more efficient than “this= A - B;” since it avoids the temporary object.

A or B can be “this” without problems.

See also:

composeFrom, composePoint

void inverse()

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

See also:

operator-

CPose2D operator - (const CPose2D& b) const

Compute \(RET = this \ominus b\)

CPose3D operator - (const CPose3D& b) const

The operator \(a \ominus 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 \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.

virtual void setToNaN()

Set all data fields to quiet NaN.

static CPose2D Identity()

Returns the identity transformation.