Class pose_t

• Defined in File pose.hpp

Class Documentation

class pose_t

Represents a 3D pose as rotation and translation.

The pose supports inversion, composition, and point transformation. It represents a transformation from local coordinates into world/camera coordinates used by the SLAM pipeline.

Public Types

using shared_ptr = std::shared_ptr<pose_t>

Typedef for shared pointer to pose_t.

using unique_ptr = std::unique_ptr<pose_t>

Typedef for unique pointer to pose_t.

using weak_ptr = std::weak_ptr<pose_t>

Typedef for weak pointer to pose_t.

Public Functions

inline pose_t()

Construct a new pose_t object.

inline pose_t(const Eigen::Matrix4d &pose_)

Construct a new pose_t object.

Parameters:

pose_ – 4x4 transform matrix containing rotation and translation.

inline pose_t(const Eigen::Matrix3d &rotation_, const Eigen::Vector3d &translation_)

Construct a new pose_t object.

Parameters:

• rotation_ – Rotation matrix used to initialize orientation.

• translation_ – Translation vector used to initialize position.

inline pose_t(std::tuple<rotation_t, translation_t> pose_)

Constructs a pose from a rotation/translation tuple.

Parameters:

pose_ – Tuple containing rotation and translation components.

inline pose_t(const rotation_t &rotation_, const translation_t &translation_)

Construct a new pose_t object.

Parameters:

• rotation_ – Rotation component of the pose.

• translation_ – Translation component of the pose.

inline pose_t(const pose_t::shared_ptr &other_)

Constructs a pose from a shared pose pointer.

Parameters:

other_ – Shared pointer to a pose whose values are copied.

inline pose_t(const pose_t::weak_ptr &other_)

Constructs a pose from a weak pose pointer.

Parameters:

other_ – Weak pointer to a pose whose values are copied if valid.

inline pose_t(const pose_t &other_)

Construct a new pose_t object.

Parameters:

other_ – Pose object copied into this instance.

inline ~pose_t()

Destroy the pose_t object.

inline bool is_unset() const

Check if pose is unset (i.e., has invalid translation).

Returns:

true when translation equals the unset sentinel values.

inline operator bool() const

Check if pose is set.

Returns:

true when the pose contains a valid translation.

inline auto operator->()

Pointer-like access to this pose.

inline auto operator->() const

Pointer-like access to this pose (const).

inline pose_t &operator=(const pose_t &other_)

Assignment operator for pose_t.

Parameters:

other_ – Pose whose values are copied into this object.

Returns:

Reference to this pose.

inline pose_t &operator=(const pose_t::weak_ptr &other_)

Assignment operator for pose_t from a weak_ptr.

Parameters:

other_ – Weak pointer to the pose source.

Returns:

Reference to this pose. (unset if the weak_ptr is expired)

inline bool operator!()

Operator for pose not defined.

inline bool operator==(const pose_t::shared_ptr &other_)

Check if two poses are equal.

Parameters:

other_ – Shared pointer to the pose compared against this one.

Returns:

true when both rotation and translation are equal.

inline void invert()

Invert the pose that is stored in this variable.

inline std::tuple<rotation_t, translation_t> get_invert() const

Outputs the inverse of the pose.

Note

Be aware that this function is allocating a new pose object and returns it. The used of this function should be done with care to avoid memory leaks.

Returns:

both rotation and translation of the inverted pose

inline point3d_t transform(const point3d_t point_) const

Transform a 3D point using the pose.

Parameters:

point_ – 3D point to transform with this pose.

Returns:

The transformed 3D point.

inline array3D_t transform(const array3D_t &point_) const

Transform a 3D point using the pose.

Parameters:

point_ – Point coordinates represented as {x, y, z}.

Returns:

The transformed 3D point.

inline array3D_t operator*(const array3D_t &point_) const

Transform a 3D point using the pose.

Parameters:

point_ – Point coordinates represented as {x, y, z}.

Returns:

The transformed 3D point.

inline point3d_t operator*(const point3d_t &point_) const

Transform a 3D point using the pose.

Parameters:

point_ – 3D point object transformed by this pose.

Returns:

The transformed 3D point.

inline point3d_t operator*(point3d_t::shared_ptr &point_) const

Transform a 3D point using the pose.

Parameters:

point_ – Shared pointer to the point transformed by this pose.

Returns:

The transformed 3D point.

inline Eigen::Vector3d operator*(const Eigen::Vector3d point_) const

Transform a 3D point using the pose.

Parameters:

point_ – Eigen vector representation of the point to transform.

Returns:

The transformed 3D point.

inline pose_t operator*(const pose_t::shared_ptr &other_) const

Overload the multiplication operator to combine two poses.

Parameters:

other_ – Shared pointer to the pose composed with this pose.

Returns:

The combined pose.

inline pose_t operator*(const pose_t &pose_) const

Multiplication operator to transform a 3D point using the pose.

Parameters:

pose_ – Pose composed with this pose (this * pose_).

Returns:

The combined pose.

inline void update_pose(const pose_t::shared_ptr &other_)

Transforms the current pose given as input another pose. T1 = T2*T1.

Parameters:

other_ – Shared pointer to the pose used for in-place composition.

inline void set_pose(const Eigen::Matrix4d &pose_)

Set the pose using a 4x4 Eigen matrix.

Parameters:

pose_ – 4x4 transform matrix containing rotation and translation.

inline void set_pose(const rotation_t &rotation_, const translation_t &translation_)

Set the pose object using a rotation and translation.

Parameters:

• rotation_ – Rotation component assigned to the pose.

• translation_ – Translation component assigned to the pose.

inline void set_pose(const pose_t &other_)

Set the pose object using another pose.

Parameters:

other_ – Pose whose rotation and translation are copied.

inline void set_rotation(const rotation_t &rotation_)

Set the rotation part of the pose.

Parameters:

rotation_ – Rotation quaternion assigned to the pose.

inline void set_rotation(const Eigen::Matrix3d &rotation_)

Set the rotation part of the pose.

Parameters:

rotation_ – Rotation matrix converted and assigned to the pose.

inline void set_translation(const translation_t &translation_)

Set the translation part of the pose.

Parameters:

translation_ – Translation vector assigned to the pose.

inline rotation_t get_rotation() const

Retrieve the rotation.

Returns:

Rotation component currently stored in the pose.

inline translation_t get_translation() const

Return the translation.

Returns:

Translation component currently stored in the pose.

inline std::tuple<rotation_t, translation_t> get_rotation_translation() const

Get both the rotation and translation of the pose.

Returns:

Tuple containing the rotation and translation.

inline Eigen::Matrix4d get_matrix() const

Get the transformation matrix of the pose in the eigen format.

Returns:

The transformation matrix of the pose (Eigen::Matrix4d).

inline array3D_t get_position() const

Get the position (translation) of the pose as an array.

Returns:

The position of the pose as an array (array3D_t).

inline array4D_t get_quaternions() const

Get the rotation (quaternion) of the pose as an array.

Returns:

The rotation of the pose as an array (array4D_t).

inline void print(std::string prefix_ = "") const

Print the pose information to the terminal.

Parameters:

prefix_ – Prefix prepended to each printed output line.

inline void lock() const

Locking functions for thread-safe access to the pose.

inline void unlock() const

Unlock the pose mutex.

inline void lock_read() const

Lock the pose for reading (shared lock).

inline void unlock_read() const

Unlock the pose read lock.

Public Static Functions

static inline pose_t::shared_ptr make_shared()

Make shared pointer function

Returns:

Shared pointer owning a new pose initialized as unset.

static inline pose_t::shared_ptr make_shared(const pose_t &other_)

Make shared pointer function

Parameters:

other_ – Pose value copied into the new shared instance.

Returns:

Shared pointer owning a copy of other_.

static inline fail_t update(const pose_t::weak_ptr &other_, const pose_t &target_)

Update the pose pointed to by the weak_ptr with the values from the target pose

Parameters:

• other_ – Weak pointer to the destination pose to update.

• target_ – Pose data copied into the destination pose.

Returns:

fail_t::NO_FAIL on success, otherwise a pointer-lock failure.

static inline output_t<Eigen::Matrix4d> get_matrix(const pose_t::weak_ptr &ptr_)

Get the pose as a 4x4 transformation matrix.

Parameters:

ptr_ – Weak pointer to the pose to read.

Returns:

4x4 transform matrix, or failure if the pointer is expired.

static inline pose_t Identity()

Get the identity pose.

Returns:

Pose initialized with identity rotation and zero translation.

static inline array2D_t computeError(const pose_t &pose1_, const pose_t &pose2_)

Compute the error between two poses.

Parameters:

• pose1_ – First pose used as the error reference.

• pose2_ – Second pose compared against pose1_.

Returns:

{rotation_error_deg, translation_error_norm}.

Private Members

rotation_t _rotation

Quaternion representing the rotation part of the pose.

translation_t _translation

Vector representing the translation part of the pose.

mutable std::shared_mutex _poseMutex

Mutex for thread-safe access to the pose.