class mrpt::poses::CPose3D

Overview

SE(3) rigid-body pose with cached rotation matrix — the preferred class for 3D transformations.

Represents an element of SE(3) = SO(3) ⋉ R³. Internally stored as:

• A 3-vector m_coords = (x, y, z) — translation in metres.

• A 3×3 rotation matrix m_ROT ∈ SO(3) — always kept normalised.

The corresponding 4x4 homogeneous matrix is:

*   T = | R  t |   where R in SO(3), t = (x,y,z)^T
*       | 0  1 |
*

Angle parameterisation (intrinsic ZYX / Tait-Bryan): Yaw/Pitch/Roll angles (yaw=psi, pitch=theta, roll=phi) are derived on demand from m_ROT via getYawPitchRoll() :

1. Rotate about the body Z axis by yaw (psi).

2. Rotate about the new body Y axis by pitch (theta).

3. Rotate about the new body X axis by roll (phi).

So R = Rz(yaw) * Ry(pitch) * Rx(roll). This is equivalent to extrinsic rotations about the fixed axes in the reverse order (X then Y then Z). The representation is singular (gimbal lock) when pitch = +-pi/2.

To change pose components, always use setFromValues() — never assign to m_ROT directly using yaw/pitch/roll since the cached angles would become stale. The rotation matrix is always the primary storage; yaw/pitch/roll are recomputed lazily.

Operator semantics (SE(3) 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 — pose of a as seen from b. In matrix form: T_b⁻¹ · T_a.

• Unary operator- : SE(3) group inverse. Not elementwise negation of (x,y,z,yaw,pitch,roll).

• pose + point : applies the SE(3) transformation to a 3D point (composePoint).

Alternative representations:

• As a unit quaternion + translation: CPose3DQuat / getAsQuaternion().

• As a Lie algebra twist (se(3) tangent vector): see mrpt::poses::Lie.

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::TPose3D, CPoseOrPoint, CPoint3D, CPose3DQuat, mrpt::math::CQuaternion, mrpt::poses::Lie

#include <mrpt/poses/CPose3D.h>

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

    typedef std::shared_ptr<mrpt::poses ::CPose3D> Ptr;
    typedef std::shared_ptr<const mrpt::poses ::CPose3D> ConstPtr;
    typedef std::unique_ptr<mrpt::poses ::CPose3D> UniquePtr;
    typedef std::unique_ptr<const mrpt::poses ::CPose3D> 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 CPose3D type_value;

    // enums

    enum
    {
        is_3D_val = 1,
    };

    enum
    {
        rotation_dimensions = 3,
    };

    enum
    {
        is_PDF_val = 0,
    };

    // fields

    static constexpr const char* className = "mrpt::poses" "::" "CPose3D";
    static constexpr std::size_t static_size = 6;

    // construction

    CPose3D();

    CPose3D(
        const double x,
        const double y,
        const double z,
        const double yaw = 0,
        const double pitch = 0,
        const double roll = 0
        );

    CPose3D(const math::CMatrixDouble& m);
    CPose3D(const math::CMatrixDouble44& m);

    template <class MATRIX33, class VECTOR3>
    CPose3D(
        const MATRIX33& rot,
        const VECTOR3& xyz
        );

    CPose3D(const mrpt::math::CMatrixDouble33& rot, const mrpt::math::CVectorFixedDouble<3>& xyz);
    CPose3D(const CPose2D& p);
    CPose3D(const CPoint3D& p);
    CPose3D(const mrpt::math::TPose3D& o);
    CPose3D(const mrpt::math::CQuaternionDouble& q, const double x, const double y, const double z);
    CPose3D(const CPose3DQuat& p);
    CPose3D(TConstructorFlags_Poses);
    CPose3D(const mrpt::math::CVectorFixedDouble<12>& vec12);

    // 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 CPose3D Identity();

    static CPose3D FromXYZYawPitchRoll(
        double x,
        double y,
        double z,
        double yaw,
        double pitch,
        double roll
        );

    static CPose3D FromYawPitchRoll(double yaw, double pitch, double roll);
    static CPose3D FromTranslation(double x, double y, double z);
    static CPose3D FromTranslation(const mrpt::math::TPoint3D& t);

    template <class MATRIX>
    static CPose3D FromHomogeneousMatrix(const MATRIX& m);

    template <class MATRIX, class VECTOR>
    static CPose3D FromRotationAndTranslation(
        const MATRIX& rot,
        const VECTOR& t
        );

    static CPose3D FromQuaternion(const mrpt::math::CQuaternionDouble& q);
    static CPose3D FromQuaternionAndTranslation(const mrpt::math::CQuaternionDouble& q, double x, double y, double z);

    template <typename Point3DLike>
    static CPose3D FromQuaternionAndTranslation(
        const mrpt::math::CQuaternionDouble& q,
        const Point3DLike& pt
        );

    mrpt::math::TPose3D asTPose() const;
    static CPose3D FromString(const std::string& s);
    void addComponents(const CPose3D& p);
    void normalizeAngles();
    void operator *= (const double s);

    void setFromValues(
        const double x0,
        const double y0,
        const double z0,
        const double yaw = 0,
        const double pitch = 0,
        const double roll = 0
        );

    template <typename VECTORLIKE>
    void setFromXYZQ(
        const VECTORLIKE& v,
        size_t index_offset = 0
        );

    void setYawPitchRoll(const double yaw_, const double pitch_, const double roll_);

    template <class ARRAYORVECTOR>
    void setFrom12Vector(const ARRAYORVECTOR& vec12);

    template <class ARRAYORVECTOR>
    void getAs12Vector(ARRAYORVECTOR& vec12) const;

    std::tuple<double, double, double> getYawPitchRoll() const;
    void getYawPitchRoll(double& yaw, double& pitch, double& roll) const;
    double yaw() const;
    double pitch() const;
    double roll() const;
    void asVector(vector_t& v) const;
    void getAsQuaternion(mrpt::math::CQuaternionDouble& q, mrpt::optional_ref<mrpt::math::CMatrixDouble43> out_dq_dr = std::nullopt) const;
    mrpt::math::CMatrixDouble33 jacobian_rodrigues_from_YPR() const;
    mrpt::math::CMatrixDouble66 jacobian_pose_rodrigues_from_YPR() const;
    double operator [] (unsigned int i) const;
    virtual std::string asString() const;
    void fromString(const std::string& s);
    void fromStringRaw(const std::string& s);
    bool isHorizontal(const double tolerance = 0) const;
    double distanceEuclidean6D(const CPose3D& o) const;
    void getHomogeneousMatrix(mrpt::math::CMatrixDouble44& out_HM) const;
    mrpt::math::CMatrixDouble44 getHomogeneousMatrix() const;
    void getRotationMatrix(mrpt::math::CMatrixDouble33& ROT) const;
    const mrpt::math::CMatrixDouble33& getRotationMatrix() const;
    void setRotationMatrix(const mrpt::math::CMatrixDouble33& ROT);
    const mrpt::math::TPoint3D translation() const;
    CPose3D operator + (const CPose3D& b) const;
    CPoint3D operator + (const CPoint3D& b) const;
    CPoint3D operator + (const CPoint2D& b) const;
    void sphericalCoordinates(const mrpt::math::TPoint3D& point, double& out_range, double& out_yaw, double& out_pitch) const;

    void composePoint(
        double lx,
        double ly,
        double lz,
        double& gx,
        double& gy,
        double& gz,
        mrpt::optional_ref<mrpt::math::CMatrixDouble33> out_jacobian_df_dpoint = std::nullopt,
        mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dpose = std::nullopt,
        mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dse3 = std::nullopt,
        bool use_small_rot_approx = false
        ) const;

    void composePoint(const mrpt::math::TPoint3D& local_point, mrpt::math::TPoint3D& global_point) const;
    mrpt::math::TPoint3D composePoint(const mrpt::math::TPoint3D& l) const;
    void composePoint(const mrpt::math::TPoint3D& local_point, mrpt::math::TPoint2D& global_point) const;
    void composePoint(double lx, double ly, double lz, float& gx, float& gy, float& gz) const;
    mrpt::math::TVector3D rotateVector(const mrpt::math::TVector3D& local) const;
    mrpt::math::TVector3D inverseRotateVector(const mrpt::math::TVector3D& global) const;

    void inverseComposePoint(
        const double gx,
        const double gy,
        const double gz,
        double& lx,
        double& ly,
        double& lz,
        mrpt::optional_ref<mrpt::math::CMatrixDouble33> out_jacobian_df_dpoint = std::nullopt,
        mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dpose = std::nullopt,
        mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dse3 = std::nullopt
        ) const;

    void inverseComposePoint(const mrpt::math::TPoint3D& g, mrpt::math::TPoint3D& l) const;
    mrpt::math::TPoint3D inverseComposePoint(const mrpt::math::TPoint3D& g) const;
    void inverseComposePoint(const mrpt::math::TPoint2D& g, mrpt::math::TPoint2D& l, const double eps = 1e-6) const;
    void composeFrom(const CPose3D& A, const CPose3D& B);
    CPose3D& operator += (const CPose3D& b);
    void inverseComposeFrom(const CPose3D& A, const CPose3D& B);
    CPose3D operator - (const CPose3D& b) const;
    void inverse();
    void changeCoordinatesReference(const CPose3D& p);
    static constexpr bool is_3D();
    static constexpr bool is_PDF();
    const mrpt::math::CVectorFixedDouble<3>& translationVector() const;
    CPose3D getOppositeScalar() 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 ::CPose3D> Ptr

A type for the associated smart pointer.

typedef double value_type

The type of the elements.

typedef double& reference

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

typedef CPose3D type_value

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

Construction

CPose3D()

Default constructor, with all the coordinates set to zero.

CPose3D(
    const double x,
    const double y,
    const double z,
    const double yaw = 0,
    const double pitch = 0,
    const double roll = 0
    )

Constructor with Initialization of the pose, translation (x,y,z) in meters, (yaw,pitch,roll) angles in radians.

See also:

FromXYZYawPitchRoll()

CPose3D(const math::CMatrixDouble& m)

Constructor from a 4x4 homogeneous matrix - the passed matrix can be actually of any size larger than or equal 3x4, since only those first values are used (the last row of a homogeneous 4x4 matrix are always fixed).

CPose3D(const math::CMatrixDouble44& m)

Constructor from a 4x4 homogeneous matrix:

template <class MATRIX33, class VECTOR3>
CPose3D(
    const MATRIX33& rot,
    const VECTOR3& xyz
    )

Constructor from a 3x3 rotation matrix and a the translation given as a 3-vector, a 3-array, a CPoint3D or a mrpt::math::TPoint3D.

CPose3D(const mrpt::math::CMatrixDouble33& rot, const mrpt::math::CVectorFixedDouble<3>& xyz)

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

CPose3D(const CPose2D& p)

Constructor from a CPose2D object.

CPose3D(const CPoint3D& p)

Constructor from a CPoint3D object.

CPose3D(const mrpt::math::TPose3D& o)

Constructor from lightweight object.

CPose3D(const mrpt::math::CQuaternionDouble& q, const double x, const double y, const double z)

Constructor from a quaternion (which only represents the 3D rotation part) and a 3D displacement.

CPose3D(const CPose3DQuat& p)

Constructor from a CPose3DQuat.

Constructor from a quaternion-based full pose.

CPose3D(TConstructorFlags_Poses)

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

CPose3D(const mrpt::math::CVectorFixedDouble<12>& vec12)

Constructor from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose.

See also:

setFrom12Vector, getAs12Vector

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 CPose3D Identity()

Returns the identity transformation.

static CPose3D FromXYZYawPitchRoll(
    double x,
    double y,
    double z,
    double yaw,
    double pitch,
    double roll
    )

Builds a pose from a translation (x,y,z) in meters and (yaw,pitch,roll) angles in radians.

(New in MRPT 2.1.8)

static CPose3D FromYawPitchRoll(double yaw, double pitch, double roll)

Builds a pose with a null translation and (yaw,pitch,roll) angles in radians.

(New in MRPT 2.1.8)

static CPose3D FromTranslation(double x, double y, double z)

Builds a pose with a translation without rotation.

(New in MRPT 2.1.8)

template <class MATRIX>
static CPose3D FromHomogeneousMatrix(const MATRIX& m)

Builds a pose with a 4x4 homogeneous matrix.

(New in MRPT 2.1.8)

template <class MATRIX, class VECTOR>
static CPose3D FromRotationAndTranslation(
    const MATRIX& rot,
    const VECTOR& t
    )

Builds a pose with a 3x3 SO(3) rotation matrix and a translation vector.

(New in MRPT 2.1.8)

static CPose3D FromQuaternion(const mrpt::math::CQuaternionDouble& q)

Builds a pose from a quaternion (and no translation).

(New in MRPT 2.1.8)

static CPose3D FromQuaternionAndTranslation(
    const mrpt::math::CQuaternionDouble& q,
    double x,
    double y,
    double z
    )

Builds a pose from a quaternion and a (x,y,z) translation.

(New in MRPT 2.1.8)

template <typename Point3DLike>
static CPose3D FromQuaternionAndTranslation(
    const mrpt::math::CQuaternionDouble& q,
    const Point3DLike& pt
    )

Builds a pose from a quaternion and a 3D translation.

(New in MRPT 2.3.3)

void addComponents(const CPose3D& p)

Scalar sum of all 6 components: This is different from poses composition, which is implemented as “+” operators.

See also:

normalizeAngles

void normalizeAngles()

Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be called after using addComponents)

See also:

addComponents

void operator *= (const double s)

Scalar multiplication of x,y,z,yaw,pitch & roll (angles will be wrapped to the ]-pi,pi] interval).

void setFromValues(
    const double x0,
    const double y0,
    const double z0,
    const double yaw = 0,
    const double pitch = 0,
    const double roll = 0
    )

Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes the internal rotation matrix.

See also:

getYawPitchRoll, setYawPitchRoll

template <typename VECTORLIKE>
void setFromXYZQ(
    const VECTORLIKE& v,
    size_t index_offset = 0
    )

Set the pose from a 3D position (meters) and a quaternion, stored as [x y z qr qx qy qz] in a 7-element vector.

See also:

setFromValues, getYawPitchRoll, setYawPitchRoll, CQuaternion, getAsQuaternion

void setYawPitchRoll(const double yaw_, const double pitch_, const double roll_)

Set the 3 angles of the 3D pose (in radians) - This method recomputes the internal rotation coordinates matrix.

See also:

getYawPitchRoll, setFromValues

template <class ARRAYORVECTOR>
void setFrom12Vector(const ARRAYORVECTOR& vec12)

Set pose from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose.

See also:

getAs12Vector

template <class ARRAYORVECTOR>
void getAs12Vector(ARRAYORVECTOR& vec12) const

Get the pose representation as an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose.

See also:

setFrom12Vector

std::tuple<double, double, double> getYawPitchRoll() const

Returns the three angles (yaw, pitch, roll), in radians, from the rotation matrix.

See also:

setFromValues, yaw, pitch, roll Returns the three angles (yaw, pitch, roll), in radians, as a tuple.

setFromValues, yaw, pitch, roll

void getYawPitchRoll(double& yaw, double& pitch, double& roll) const

Deprecated Use getYawPitchRoll() returning a tuple instead.

double yaw() const

Get the YAW angle (in radians)

See also:

setFromValues

double pitch() const

Get the PITCH angle (in radians)

See also:

setFromValues

double roll() const

Get the ROLL angle (in radians)

See also:

setFromValues

void asVector(vector_t& v) const

Returns a 6x1 vector with [x y z yaw pitch roll]’.

void getAsQuaternion(mrpt::math::CQuaternionDouble& q, mrpt::optional_ref<mrpt::math::CMatrixDouble43> out_dq_dr = std::nullopt) const

Converts the SO(3) rotation to a unit quaternion q = (qr, qx, qy, qz).

The translation (x,y,z) is ignored — only m_ROT is converted.

The mapping from intrinsic ZYX angles (yaw, pitch, roll) to the Hamilton unit quaternion (scalar-first) is:

*   Let cy=cos(yaw/2), sy=sin(yaw/2)
*       cp=cos(pitch/2), sp=sin(pitch/2)
*       cr=cos(roll/2),  sr=sin(roll/2)
*
*   q = [ cr*cp*cy + sr*sp*sy ]   (qr, real/scalar part)
*       [ sr*cp*cy - cr*sp*sy ]   (qx)
*       [ cr*sp*cy + sr*cp*sy ]   (qy)
*       [ cr*cp*sy - sr*sp*cy ]   (qz)
*

Parameters:

q	The resulting unit quaternion in (qr, qx, qy, qz) / Hamilton / scalar-first order.
out_dq_dr	If provided, the 4x3 Jacobian dq/d(yaw,pitch,roll) is computed and stored here.

virtual std::string asString() const

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

See also:

fromString

void fromString(const std::string& s)

Set the current object value from a string generated by ‘asString’ (eg: “[x y z yaw pitch roll]”, angles in deg.

)

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.

bool isHorizontal(const double tolerance = 0) const

Return true if the 6D pose represents a Z axis almost exactly vertical (upwards or downwards), with a given tolerance (if set to 0 exact horizontality is tested).

double distanceEuclidean6D(const CPose3D& o) const

The euclidean distance between two poses taken as two 6-length vectors (angles in radians).

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, getRotationMatrix

mrpt::math::CMatrixDouble44 getHomogeneousMatrix() const

Returns the corresponding 4x4 homogeneous transformation matrix.

See also:

getInverseHomogeneousMatrix, getRotationMatrix

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

Get the 3x3 rotation matrix.

Deprecated Use getRotationMatrix() returning by value instead.

See also:

getHomogeneousMatrix

void setRotationMatrix(const mrpt::math::CMatrixDouble33& ROT)

Sets the 3x3 rotation matrix.

See also:

getRotationMatrix, getHomogeneousMatrix

const mrpt::math::TPoint3D translation() const

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

CPose3D operator + (const CPose3D& b) const

SE(3) group composition: ret = this ⊕ b.

If this is the pose of frame A in world W, and b is the pose of frame B in A, then ret is the pose of B in W. In matrix form: T_this · T_b.

CPoint3D operator + (const CPoint3D& b) const

Applies the SE(3) transformation to a CPoint3D : ret = this ⊕ b.

CPoint3D operator + (const CPoint2D& b) const

Applies the SE(3) transformation to a CPoint2D (z=0): ret = this ⊕ b.

void sphericalCoordinates(
    const mrpt::math::TPoint3D& point,
    double& out_range,
    double& out_yaw,
    double& out_pitch
    ) const

Computes the spherical (range, yaw, pitch) coordinates of a 3D point expressed in the global frame, as observed from the sensor frame defined by this pose.

The angles follow the same ZY intrinsic convention as CPose3D.

void composePoint(
    double lx,
    double ly,
    double lz,
    double& gx,
    double& gy,
    double& gz,
    mrpt::optional_ref<mrpt::math::CMatrixDouble33> out_jacobian_df_dpoint = std::nullopt,
    mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dpose = std::nullopt,
    mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dse3 = std::nullopt,
    bool use_small_rot_approx = false
    ) const

Transforms a 3D point from the local frame of this pose into the global frame.

Computes: g = R * l + t

Optional Jacobians (all w.r.t. the input quantities):

• out_jacobian_df_dpoint (3x3): dg/dl = R.

• out_jacobian_df_dpose (3x6): dg/d(t, yaw, pitch, roll) in the yaw/pitch/roll parameterisation. Set use_small_rot_approx=true for the first-order linearisation (valid only for small rotations).

• out_jacobian_df_dse3 (3x6): dg/dxi where xi is the 6D locally-Euclidean tangent-space vector of SE(3) (see [4]).

See also:

inverseComposePoint, composePoint(const TPoint3D&)

void composePoint(const mrpt::math::TPoint3D& local_point, mrpt::math::TPoint3D& global_point) const

An alternative, slightly more efficient way of doing G = P (+) L with G and L being 3D points and P this 6D pose.

local_point is passed by value to allow global and local point to be the same variable

void composePoint(const mrpt::math::TPoint3D& local_point, mrpt::math::TPoint2D& global_point) const

This version of the method assumes that the resulting point has no Z component (use with caution!)

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

An alternative, slightly more efficient way of doing G = P (+) L with G and L being 3D points and P this 6D pose.

mrpt::math::TVector3D rotateVector(const mrpt::math::TVector3D& local) const

Rotates a vector (i.e.

like composePoint(), but ignoring translation)

mrpt::math::TVector3D inverseRotateVector(const mrpt::math::TVector3D& global) const

Inverse of rotateVector(), i.e.

using the inverse rotation matrix

void inverseComposePoint(
    const double gx,
    const double gy,
    const double gz,
    double& lx,
    double& ly,
    double& lz,
    mrpt::optional_ref<mrpt::math::CMatrixDouble33> out_jacobian_df_dpoint = std::nullopt,
    mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dpose = std::nullopt,
    mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dse3 = std::nullopt
    ) const

Transforms a 3D point from the global frame into the local frame of this pose.

Computes: l = R^T * (g - t)

Optional Jacobians follow the same conventions as composePoint() :

• out_jacobian_df_dpoint (3x3): dl/dg = R^T.

• out_jacobian_df_dpose (3×6): w.r.t. yaw/pitch/roll parameterisation.

• out_jacobian_df_dse3 (3×6): w.r.t. SE(3) tangent-space vector ξ (see [4]).

See also:

composePoint, composeFrom

void inverseComposePoint(const mrpt::math::TPoint3D& g, 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 inverseComposePoint(const mrpt::math::TPoint2D& g, mrpt::math::TPoint2D& l, const double eps = 1e-6) const

overload for 2D points

Parameters:

If	the z component of the result is greater than some epsilon

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

In-place SE(3) group composition: computes this = A ⊕ B (matrix form: T_A · T_B).

Slightly more efficient than this = A + B as it avoids a temporary. A or B may safely alias this.

CPose3D& operator += (const CPose3D& b)

Compound-assignment composition: this = this ⊕ b.

b may safely alias this.

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

In-place relative pose: computes this = A ⊖ B = B⁻¹ ⊕ A (matrix form: T_B⁻¹ · T_A).

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

Slightly more efficient than this = A - B as it avoids a temporary. A or B may safely alias this.

A or B can be “this” without problems.

See also:

composeFrom, composePoint

composeFrom, composePoint

CPose3D operator - (const CPose3D& b) const

SE(3) relative pose: ret = this ⊖ b = b⁻¹ ⊕ this.

Returns the pose of this expressed in the frame of b.

void inverse()

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

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

If T = [R|t], then T⁻¹ = [Rᵀ | -Rᵀ·t]. Not the same as negating all 6 components individually.

See also:

operator-

void changeCoordinatesReference(const CPose3D& p)

makes: this = p (+) this

const mrpt::math::CVectorFixedDouble<3>& translationVector() const

Read-only access to the raw [x,y,z] translation vector storage.

CPose3D getOppositeScalar() const

Return the opposite of the current pose instance by taking the negative of all its components individually.