Class representing a pose in 3D space. More...

#include <pose_lib.h>

Collaboration diagram for MoveG::Pose:

Public Member Functions
	Pose () noexcept
	Default constructor.

	Pose (const Eigen::Vector3d &position, const Eigen::Quaterniond &orientation)
	Constructor with position and quaternion orientation.

	Pose (const Eigen::Vector3d &position, const Eigen::Matrix3d &rotation_matrix)
	Constructor with position and rotation matrix.

	Pose (const Eigen::Affine3d &transformation)
	Constructor with affine transformation.

	Pose (const Eigen::Vector3d &position, const Rotation &orientation)
	Constructor with position and Rotation object.

	Pose (const Eigen::Matrix4d &homogeneousT)
	Constructor with homogeneous transformation matrix 4x4.

	~Pose ()
	Destructor.

	Pose (const Pose &other)
	Copy constructor.

	Pose (Pose &&other) noexcept
	Move constructor.

Pose &	operator= (const Pose &other)
	Copy assignment operator.

Pose &	operator= (Pose &&other) noexcept
	Move assignment operator.

Eigen::Vector3d	getPosition () const
	Gets the position.

Eigen::Quaterniond	getQuaternion () const
	Gets the orientation quaternion.

Eigen::Matrix3d	getRotationMatrix () const
	Gets the rotation matrix.

Eigen::Affine3d	getAffineTransformation () const
	Gets the affine transformation.

Eigen::Matrix4d	getHomogeneousT () const
	Gets the homogeneous transformation matrix.

double	getX () const
	Gets the X coordinate of the position.

double	getY () const
	Gets the Y coordinate of the position.

double	getZ () const
	Gets the Z coordinate of the position.

double	getQx () const
	Gets the X component of the quaternion.

double	getQy () const
	Gets the Y component of the quaternion.

double	getQz () const
	Gets the Z component of the quaternion.

double	getQw () const
	Gets the W component of the quaternion.

void	setPosition (const Eigen::Vector3d &position)
	Sets the position.

void	setOrientation (const Eigen::Quaterniond &orientation)
	Sets the orientation via quaternion.

void	setRotationMatrix (const Eigen::Matrix3d &rotation_matrix)
	Sets the rotation matrix.

void	setAffineTransformation (const Eigen::Affine3d &transformation)
	Sets the affine transformation.

void	setHomogeneousT (const Eigen::Matrix4d &homogeneousT)
	Sets the homogeneous transformation matrix.

double	positionDistance (const Pose &other) const
	Calculates the Euclidean distance between the positions of two poses.

double	orientationDistance (const Pose &other) const
	Calculates the angular distance between the orientations of two poses.

Pose	operator* (const Pose &other) const
	Composition of two poses.

Pose	inverse () const
	Calculates the inverse of the pose.

Pose	transformPose (const Pose &transform) const
	Transforms this pose from one coordinate frame to another.

Eigen::Vector3d	localToGlobal (const Eigen::Vector3d &local_point) const
	Transforms a point from the pose's local frame to the global frame.

Eigen::Vector3d	globalToLocal (const Eigen::Vector3d &global_point) const
	Transforms a point from the global frame to the pose's local frame.

Static Private Member Functions
static void	validateHomogeneousMatrix (const Eigen::Matrix4d &homogeneousT)
	Validates a homogeneous transformation matrix.

Private Attributes
Eigen::Vector3d	position_
	Position in 3D space.

Eigen::Quaterniond	orientation_
	Orientation represented as quaternion.

Friends
std::ostream &	operator<< (std::ostream &os, const Pose &pose)
	Overloads the stream insertion operator.

Detailed Description

Class representing a pose in 3D space.

The Pose class encapsulates a position and orientation in three-dimensional space. It uses Eigen for mathematical operations related to geometry.

Definition at line 34 of file pose_lib.h.

Constructor & Destructor Documentation

◆ Pose() [1/8]

MoveG::Pose::Pose ( )

noexcept

Default constructor.

Initializes the pose at position (0,0,0) with neutral orientation (identity quaternion).

Definition at line 14 of file pose_lib.cpp.

    : position_(Eigen::Vector3d::Zero()), orientation_(Eigen::Quaterniond::Identity())
{
}

Referenced by inverse(), and operator*().

Here is the caller graph for this function:

◆ Pose() [2/8]

MoveG::Pose::Pose	(	const Eigen::Vector3d &	position,
		const Eigen::Quaterniond &	orientation
	)

Constructor with position and quaternion orientation.

Parameters

position	The position in space.
orientation	The quaternion representing the orientation.

Definition at line 20 of file pose_lib.cpp.

    : position_(position),
      orientation_(orientation.normalized()) // Ensure the quaternion is normalized
{
}

◆ Pose() [3/8]

MoveG::Pose::Pose	(	const Eigen::Vector3d &	position,
		const Eigen::Matrix3d &	rotation_matrix
	)

Constructor with position and rotation matrix.

Parameters

position	The position in space.
rotation_matrix	The 3x3 rotation matrix.

Definition at line 27 of file pose_lib.cpp.

    : position_(position), orientation_(Eigen::Quaterniond(rotation_matrix))
{
    orientation_.normalize(); // Ensure the quaternion is normalized
}

References orientation_.

◆ Pose() [4/8]

MoveG::Pose::Pose ( const Eigen::Affine3d & transformation )

explicit

Constructor with affine transformation.

Parameters

transformation The 3D affine transformation.

Definition at line 34 of file pose_lib.cpp.

    : position_(transformation.translation()),
      orientation_(Eigen::Quaterniond(transformation.rotation()))
{
    orientation_.normalize(); // Ensure the quaternion is normalized
}

References orientation_.

◆ Pose() [5/8]

MoveG::Pose::Pose	(	const Eigen::Vector3d &	position,
		const Rotation &	orientation
	)

Constructor with position and Rotation object.

Parameters

position	The position in space.
orientation	The Rotation object representing the orientation.

Definition at line 42 of file pose_lib.cpp.

    : position_(position), orientation_(orientation.toQuaternion())
{
    orientation_.normalize(); // Ensure the quaternion is normalized
}

References orientation_.

◆ Pose() [6/8]

MoveG::Pose::Pose ( const Eigen::Matrix4d & homogeneousT )

explicit

Constructor with homogeneous transformation matrix 4x4.

Parameters

homogeneousT The 4x4 homogeneous transformation matrix.

Exceptions

std::invalid_argument If the matrix is not a valid homogeneous transformation matrix.

Definition at line 49 of file pose_lib.cpp.

{
    validateHomogeneousMatrix(homogeneousT);
 
    // Extract position
    position_ = homogeneousT.block<3, 1>(0, 3);
 
    // Extract rotation matrix
    const Eigen::Matrix3d rotation_matrix = homogeneousT.block<3, 3>(0, 0);
    orientation_ = Eigen::Quaterniond(rotation_matrix).normalized(); // Ensure normalization
}

References orientation_, position_, and validateHomogeneousMatrix().

Here is the call graph for this function:

◆ ~Pose()

MoveG::Pose::~Pose ( )

Destructor.

Definition at line 62 of file pose_lib.cpp.

{
    // No dynamic memory to clean up
}

◆ Pose() [7/8]

MoveG::Pose::Pose ( const Pose & other )

Copy constructor.

Parameters

other The Pose object to copy.

Definition at line 68 of file pose_lib.cpp.

                            : position_(other.position_), orientation_(other.orientation_)
{
}

◆ Pose() [8/8]

MoveG::Pose::Pose ( Pose && other )

noexcept

Move constructor.

Parameters

other The Pose object to move.

Definition at line 73 of file pose_lib.cpp.

    : position_(std::move(other.position_)), orientation_(std::move(other.orientation_))
{
}

Member Function Documentation

◆ getAffineTransformation()

Eigen::Affine3d MoveG::Pose::getAffineTransformation ( ) const

Gets the affine transformation.

Returns: The 3D affine transformation.

Definition at line 117 of file pose_lib.cpp.

{
    Eigen::Affine3d transformation = Eigen::Affine3d::Identity();
    transformation.translation() = position_;
    transformation.linear() = orientation_.toRotationMatrix();
    return transformation;
}

References orientation_, and position_.

Referenced by main().

Here is the caller graph for this function:

◆ getHomogeneousT()

Eigen::Matrix4d MoveG::Pose::getHomogeneousT ( ) const

Gets the homogeneous transformation matrix.

Returns: The 4x4 homogeneous matrix.

Definition at line 125 of file pose_lib.cpp.

{
    Eigen::Matrix4d homogeneous = Eigen::Matrix4d::Identity();
    homogeneous.block<3, 3>(0, 0) = orientation_.toRotationMatrix();
    homogeneous.block<3, 1>(0, 3) = position_;
    return homogeneous;
}

References orientation_, and position_.

Referenced by main().

Here is the caller graph for this function:

◆ getPosition()

Eigen::Vector3d MoveG::Pose::getPosition ( ) const

Gets the position.

Returns: The position as a 3D vector.

Definition at line 102 of file pose_lib.cpp.

{
    return position_;
}

References position_.

Referenced by main().

Here is the caller graph for this function:

◆ getQuaternion()

Eigen::Quaterniond MoveG::Pose::getQuaternion ( ) const

Gets the orientation quaternion.

Returns: The quaternion representing the orientation.

Definition at line 107 of file pose_lib.cpp.

{
    return orientation_;
}

References orientation_.

Referenced by main().

Here is the caller graph for this function:

◆ getQw()

double MoveG::Pose::getQw ( ) const

Gets the W component of the quaternion.

Returns: The W component.

Definition at line 165 of file pose_lib.cpp.

{
    return orientation_.w();
}

References orientation_.

Referenced by main().

Here is the caller graph for this function:

◆ getQx()

double MoveG::Pose::getQx ( ) const

Gets the X component of the quaternion.

Returns: The X component.

Definition at line 150 of file pose_lib.cpp.

{
    return orientation_.x();
}

References orientation_.

Referenced by main().

Here is the caller graph for this function:

◆ getQy()

double MoveG::Pose::getQy ( ) const

Gets the Y component of the quaternion.

Returns: The Y component.

Definition at line 155 of file pose_lib.cpp.

{
    return orientation_.y();
}

References orientation_.

Referenced by main().

Here is the caller graph for this function:

◆ getQz()

double MoveG::Pose::getQz ( ) const

Gets the Z component of the quaternion.

Returns: The Z component.

Definition at line 160 of file pose_lib.cpp.

{
    return orientation_.z();
}

References orientation_.

Referenced by main().

Here is the caller graph for this function:

◆ getRotationMatrix()

Eigen::Matrix3d MoveG::Pose::getRotationMatrix ( ) const

Gets the rotation matrix.

Returns: The 3x3 rotation matrix.

Definition at line 112 of file pose_lib.cpp.

{
    return orientation_.toRotationMatrix();
}

References orientation_.

Referenced by main().

Here is the caller graph for this function:

◆ getX()

double MoveG::Pose::getX ( ) const

Gets the X coordinate of the position.

Returns: The X coordinate.

Definition at line 135 of file pose_lib.cpp.

{
    return position_.x();
}

References position_.

Referenced by main().

Here is the caller graph for this function:

◆ getY()

double MoveG::Pose::getY ( ) const

Gets the Y coordinate of the position.

Returns: The Y coordinate.

Definition at line 140 of file pose_lib.cpp.

{
    return position_.y();
}

References position_.

Referenced by main().

Here is the caller graph for this function:

◆ getZ()

double MoveG::Pose::getZ ( ) const

Gets the Z coordinate of the position.

Returns: The Z coordinate.

Definition at line 145 of file pose_lib.cpp.

{
    return position_.z();
}

References position_.

Referenced by main().

Here is the caller graph for this function:

◆ globalToLocal()

Eigen::Vector3d MoveG::Pose::globalToLocal ( const Eigen::Vector3d & global_point ) const

Transforms a point from the global frame to the pose's local frame.

Parameters

global_point The point in the global coordinate frame.

Returns: The point expressed in the local coordinate frame.

Definition at line 305 of file pose_lib.cpp.

{
    // Transform a point from the global coordinate frame to the local frame
    // This applies the inverse transformation (first translate, then rotate)
    return orientation_.conjugate() * (global_point - position_);
}

References orientation_, and position_.

Referenced by main().

Here is the caller graph for this function:

◆ inverse()

Pose MoveG::Pose::inverse ( ) const

Calculates the inverse of the pose.

Returns: The inverse pose.

Definition at line 229 of file pose_lib.cpp.

{
    const Eigen::Quaterniond inv_orientation = orientation_.conjugate();
    const Eigen::Vector3d inv_position = -(inv_orientation * position_);
    return Pose(inv_position, inv_orientation);
}

References orientation_, Pose(), and position_.

Referenced by main().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ localToGlobal()

Eigen::Vector3d MoveG::Pose::localToGlobal ( const Eigen::Vector3d & local_point ) const

Transforms a point from the pose's local frame to the global frame.

Parameters

local_point The point in the local coordinate frame.

Returns: The point expressed in the global coordinate frame.

Definition at line 298 of file pose_lib.cpp.

{
    // Transform a point from the local coordinate frame to the global frame
    // This applies the rotation and then the translation
    return position_ + orientation_ * local_point;
}

References orientation_, and position_.

Referenced by main().

Here is the caller graph for this function:

◆ operator*()

Pose MoveG::Pose::operator* ( const Pose & other ) const

Composition of two poses.

Parameters

other The second pose to compose.

Returns: A new pose resulting from the composition.

Definition at line 221 of file pose_lib.cpp.

{
    const Eigen::Quaterniond combined_orientation = orientation_ * other.orientation_;
    const Eigen::Vector3d combined_position = position_ + orientation_ * other.position_;
    return Pose(combined_position, combined_orientation);
}

References orientation_, Pose(), and position_.

Here is the call graph for this function:

◆ operator=() [1/2]

Pose & MoveG::Pose::operator= ( const Pose & other )

Copy assignment operator.

Parameters

other The Pose object to assign.

Returns: Reference to the assigned object.

Definition at line 79 of file pose_lib.cpp.

{
    if (this != &other)
    {
        position_ = other.position_;
        orientation_ = other.orientation_;
    }
    return *this;
}

References orientation_, and position_.

◆ operator=() [2/2]

Pose & MoveG::Pose::operator= ( Pose && other )

noexcept

Move assignment operator.

Parameters

other The Pose object to move and assign.

Returns: Reference to the assigned object.

Definition at line 90 of file pose_lib.cpp.

{
    if (this != &other)
    {
        position_ = std::move(other.position_);
        orientation_ = std::move(other.orientation_);
    }
    return *this;
}

◆ orientationDistance()

double MoveG::Pose::orientationDistance ( const Pose & other ) const

Calculates the angular distance between the orientations of two poses.

Parameters

other The pose to calculate the angular distance with.

Returns: The angular distance in radians.

Definition at line 211 of file pose_lib.cpp.

{
    // Compute the angle between quaternions
    double dot = std::abs(orientation_.dot(other.orientation_));
    // Clamp dot to [-1, 1] to avoid numerical issues
    dot = std::max(-1.0, std::min(1.0, dot));
    return 2.0 * std::acos(dot);
}

References orientation_.

Referenced by main().

Here is the caller graph for this function:

◆ positionDistance()

double MoveG::Pose::positionDistance ( const Pose & other ) const

Calculates the Euclidean distance between the positions of two poses.

Parameters

other The pose to calculate the distance with.

Returns: The Euclidean distance between the positions.

Definition at line 206 of file pose_lib.cpp.

{
    return (position_ - other.position_).norm();
}

References position_.

Referenced by main().

Here is the caller graph for this function:

◆ setAffineTransformation()

void MoveG::Pose::setAffineTransformation ( const Eigen::Affine3d & transformation )

Sets the affine transformation.

Parameters

transformation The new 3D affine transformation.

Definition at line 187 of file pose_lib.cpp.

{
    position_ = transformation.translation();
    orientation_ = Eigen::Quaterniond(transformation.rotation()).normalized();
}

References orientation_, and position_.

◆ setHomogeneousT()

void MoveG::Pose::setHomogeneousT ( const Eigen::Matrix4d & homogeneousT )

Sets the homogeneous transformation matrix.

Parameters

homogeneousT The new 4x4 homogeneous matrix.

Exceptions

std::invalid_argument If the matrix is not a valid homogeneous transformation matrix.

Definition at line 193 of file pose_lib.cpp.

{
    validateHomogeneousMatrix(homogeneousT);
 
    // Extract position
    position_ = homogeneousT.block<3, 1>(0, 3);
 
    // Extract rotation matrix
    const Eigen::Matrix3d rotation_matrix = homogeneousT.block<3, 3>(0, 0);
    orientation_ = Eigen::Quaterniond(rotation_matrix).normalized();
}

References orientation_, position_, and validateHomogeneousMatrix().

Referenced by main().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setOrientation()

void MoveG::Pose::setOrientation ( const Eigen::Quaterniond & orientation )

Sets the orientation via quaternion.

Parameters

orientation The new quaternion representing the orientation.

Definition at line 177 of file pose_lib.cpp.

{
    orientation_ = orientation.normalized(); // Ensure the quaternion is normalized
}

References orientation_.

Referenced by main().

Here is the caller graph for this function:

◆ setPosition()

void MoveG::Pose::setPosition ( const Eigen::Vector3d & position )

Sets the position.

Parameters

position The new position as a 3D vector.

Definition at line 172 of file pose_lib.cpp.

{
    position_ = position;
}

References position_.

Referenced by main().

Here is the caller graph for this function:

◆ setRotationMatrix()

void MoveG::Pose::setRotationMatrix ( const Eigen::Matrix3d & rotation_matrix )

Sets the rotation matrix.

Parameters

rotation_matrix The new 3x3 rotation matrix.

Definition at line 182 of file pose_lib.cpp.

{
    orientation_ = Eigen::Quaterniond(rotation_matrix).normalized(); // Ensure normalization
}

References orientation_.

Referenced by main().

Here is the caller graph for this function:

◆ transformPose()

Pose MoveG::Pose::transformPose ( const Pose & transform ) const

Transforms this pose from one coordinate frame to another.

Parameters

transform The transformation between the two coordinate frames.

Returns: A new pose expressed in the target coordinate frame.

Definition at line 291 of file pose_lib.cpp.

{
    // This method transforms this pose by the given transformation
    // This is useful for changing coordinate frames
    return transform * (*this);
}

Referenced by main().

Here is the caller graph for this function:

◆ validateHomogeneousMatrix()

void MoveG::Pose::validateHomogeneousMatrix ( const Eigen::Matrix4d & homogeneousT )

staticprivate

Validates a homogeneous transformation matrix.

Checks that the last row is [0,0,0,1] and that the rotation part is orthogonal.

Parameters

homogeneousT The homogeneous transformation matrix to validate.

Exceptions

std::invalid_argument If the matrix is not a valid homogeneous transformation matrix.

Definition at line 237 of file pose_lib.cpp.

{
    // Check the last row
    Eigen::RowVector4d expected_last_row(0, 0, 0, 1);
    Eigen::RowVector4d actual_last_row = homogeneousT.block<1, 4>(3, 0);
 
    if (!actual_last_row.isApprox(expected_last_row, 1e-6))
    {
        std::stringstream error_msg;
        error_msg << "Invalid homogeneous transformation matrix. The last row must be [0, 0, 0, "
                     "1], but got ["
                  << actual_last_row(0) << ", " << actual_last_row(1) << ", " << actual_last_row(2)
                  << ", " << actual_last_row(3) << "].";
        throw std::invalid_argument(error_msg.str());
    }
 
    // Extract the rotation matrix part
    const Eigen::Matrix3d rotation_part = homogeneousT.block<3, 3>(0, 0);
 
    // Check if the rotation matrix is orthogonal
    // A rotation matrix multiplied by its transpose should give the identity matrix
    Eigen::Matrix3d orthogonality_check = rotation_part * rotation_part.transpose();
    Eigen::Matrix3d identity = Eigen::Matrix3d::Identity();
 
    if (!orthogonality_check.isApprox(identity, 1e-6))
    {
        std::stringstream error_msg;
        error_msg
            << "Invalid homogeneous transformation matrix. The rotation part is not orthogonal.";
        throw std::invalid_argument(error_msg.str());
    }
 
    // Additionally, check the determinant is approximately 1 (proper rotation)
    double det = rotation_part.determinant();
    if (std::abs(det - 1.0) > 1e-6)
    {
        std::stringstream error_msg;
        error_msg << "Invalid homogeneous transformation matrix. The rotation part has determinant "
                  << det << ", which differs from 1.0 (expected for a proper rotation).";
        throw std::invalid_argument(error_msg.str());
    }
}

Referenced by Pose(), and setHomogeneousT().

Here is the caller graph for this function:

Friends And Related Symbol Documentation

◆ operator<<

std::ostream & operator<<	(	std::ostream &	os,
		const Pose &	pose
	)

friend

Overloads the stream insertion operator.

Parameters

os	The output stream.
pose	The Pose object to insert into the stream.

Returns: Reference to the output stream.

Definition at line 281 of file pose_lib.cpp.

{
    os << "Position: [" << pose.position_.x() << ", " << pose.position_.y() << ", "
       << pose.position_.z() << "]\n";
    os << "Orientation (quaternion): [" << pose.orientation_.x() << ", " << pose.orientation_.y()
       << ", " << pose.orientation_.z() << ", " << pose.orientation_.w() << "]";
    return os;
}

Member Data Documentation

◆ orientation_

Eigen::Quaterniond MoveG::Pose::orientation_

private

Orientation represented as quaternion.

Definition at line 314 of file pose_lib.h.

Referenced by getAffineTransformation(), getHomogeneousT(), getQuaternion(), getQw(), getQx(), getQy(), getQz(), getRotationMatrix(), globalToLocal(), inverse(), localToGlobal(), operator*(), operator=(), orientationDistance(), Pose(), Pose(), Pose(), Pose(), setAffineTransformation(), setHomogeneousT(), setOrientation(), and setRotationMatrix().

◆ position_

Eigen::Vector3d MoveG::Pose::position_

private

Position in 3D space.

Definition at line 313 of file pose_lib.h.

Referenced by getAffineTransformation(), getHomogeneousT(), getPosition(), getX(), getY(), getZ(), globalToLocal(), inverse(), localToGlobal(), operator*(), operator=(), Pose(), positionDistance(), setAffineTransformation(), setHomogeneousT(), and setPosition().

The documentation for this class was generated from the following files:

/home/runner/work/MoveG/MoveG/src/MoveG_Lib/pose_lib.h
/home/runner/work/MoveG/MoveG/src/MoveG_Lib/pose_lib.cpp

Public Member Functions

Static Private Member Functions

Private Attributes

Friends

Detailed Description

Constructor & Destructor Documentation

◆ Pose() [1/8]

◆ Pose() [2/8]

◆ Pose() [3/8]

◆ Pose() [4/8]

◆ Pose() [5/8]

◆ Pose() [6/8]

◆ ~Pose()

◆ Pose() [7/8]

◆ Pose() [8/8]

Member Function Documentation

◆ getAffineTransformation()

◆ getHomogeneousT()

◆ getPosition()

◆ getQuaternion()

◆ getQw()

◆ getQx()

◆ getQy()

◆ getQz()

◆ getRotationMatrix()

◆ getX()

◆ getY()

◆ getZ()

◆ globalToLocal()

◆ inverse()

◆ localToGlobal()

◆ operator*()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ orientationDistance()

◆ positionDistance()

◆ setAffineTransformation()

◆ setHomogeneousT()

◆ setOrientation()

◆ setPosition()

◆ setRotationMatrix()

◆ transformPose()

◆ validateHomogeneousMatrix()

Friends And Related Symbol Documentation

◆ operator<<

Member Data Documentation

◆ orientation_

◆ position_