Class PoseSE2

Class Documentation

class PoseSE2

This class implements a pose in the domain SE2: \( \mathbb{R}^2 \times S^1 \) The pose consist of the position x and y and an orientation given as angle theta [-pi, pi].

Construct PoseSE2 instances

inline PoseSE2()

Default constructor.

inline PoseSE2(const Eigen::Ref<const Eigen::Vector2d> &position, double theta)

Construct pose given a position vector and an angle theta.

Parameters:

  • position – 2D position vector

  • theta – angle given in rad

inline PoseSE2(double x, double y, double theta)

Construct pose using single components x, y, and the yaw angle.

Parameters:

  • x – x-coordinate

  • y – y-coordinate

  • theta – yaw angle in rad

inline explicit PoseSE2(const geometry_msgs::Pose &pose)

Construct pose using a geometry_msgs::Pose.

Parameters:

pose – geometry_msgs::Pose object

inline explicit PoseSE2(const tf::Pose &pose)

Construct pose using a tf::Pose.

Parameters:

pose – tf::Pose object

inline PoseSE2(const PoseSE2 &pose)

Copy constructor.

Parameters:

pose – PoseSE2 instance

Access and modify values

inline Eigen::Vector2d &position()

Access the 2D position part.

See also

estimate

Returns:

reference to the 2D position part

inline const Eigen::Vector2d &position() const

Access the 2D position part (read-only)

See also

estimate

Returns:

const reference to the 2D position part

inline double &x()

Access the x-coordinate the pose.

Returns:

reference to the x-coordinate

inline const double &x() const

Access the x-coordinate the pose (read-only)

Returns:

const reference to the x-coordinate

inline double &y()

Access the y-coordinate the pose.

Returns:

reference to the y-coordinate

inline const double &y() const

Access the y-coordinate the pose (read-only)

Returns:

const reference to the y-coordinate

inline double &theta()

Access the orientation part (yaw angle) of the pose.

Returns:

reference to the yaw angle

inline const double &theta() const

Access the orientation part (yaw angle) of the pose (read-only)

Returns:

const reference to the yaw angle

inline void setZero()

Set pose to [0,0,0].

inline void toPoseMsg(geometry_msgs::Pose &pose) const

Convert PoseSE2 to a geometry_msgs::Pose.

Parameters:

pose – [out] Pose message

inline Eigen::Vector2d orientationUnitVec() const

Return the unit vector of the current orientation.

Returns:

[cos(theta), sin(theta))]^T

Arithmetic operations for which operators are not always reasonable

inline void scale(double factor)

Scale all SE2 components (x,y,theta) and normalize theta afterwards to [-pi, pi].

Parameters:

factor – scale factor

inline void plus(const double *pose_as_array)

Increment the pose by adding a double[3] array The angle is normalized afterwards.

Parameters:

pose_as_array – 3D double array [x, y, theta]

inline void averageInPlace(const PoseSE2 &pose1, const PoseSE2 &pose2)

Get the mean / average of two poses and store it in the caller class For the position part: 0.5*(x1+x2) For the angle: take the angle of the mean direction vector.

Parameters:

  • pose1 – first pose to consider

  • pose2 – second pose to consider

inline void rotateGlobal(double angle, bool adjust_theta = true)

Rotate pose globally.

Compute [pose_x, pose_y] = Rot(angle) * [pose_x, pose_y]. if adjust_theta, pose_theta is also rotated by angle

Parameters:

  • angle – the angle defining the 2d rotation

  • adjust_theta – if true, the orientation theta is also rotated

static inline PoseSE2 average(const PoseSE2 &pose1, const PoseSE2 &pose2)

Get the mean / average of two poses and return the result (static) For the position part: 0.5*(x1+x2) For the angle: take the angle of the mean direction vector.

Parameters:

  • pose1 – first pose to consider

  • pose2 – second pose to consider

Returns:

mean / average of pose1 and pose2

Operator overloads / Allow some arithmetic operations

inline PoseSE2 &operator=(const PoseSE2 &rhs)

Asignment operator.

Parameters:

rhs – PoseSE2 instance

inline PoseSE2 &operator+=(const PoseSE2 &rhs)

Compound assignment operator (addition)

Parameters:

rhs – addend

inline PoseSE2 &operator-=(const PoseSE2 &rhs)

Compound assignment operator (subtraction)

Parameters:

rhs – value to subtract

inline friend PoseSE2 operator+(PoseSE2 lhs, const PoseSE2 &rhs)

Arithmetic operator overload for additions.

Parameters:

  • lhs – First addend

  • rhs – Second addend

inline friend PoseSE2 operator-(PoseSE2 lhs, const PoseSE2 &rhs)

Arithmetic operator overload for subtractions.

Parameters:

  • lhs – First term

  • rhs – Second term

inline friend PoseSE2 operator*(PoseSE2 pose, double scalar)

Multiply pose with scalar and return copy without normalizing theta This operator is useful for calculating velocities …

Warning

theta is not normalized after multiplying

Parameters:

  • pose – pose to scale

  • scalar – factor to multiply with

inline friend PoseSE2 operator*(double scalar, PoseSE2 pose)

Multiply pose with scalar and return copy without normalizing theta This operator is useful for calculating velocities …

Warning

theta is not normalized after multiplying

Parameters:

  • scalar – factor to multiply with

  • pose – pose to scale

inline friend std::ostream &operator<<(std::ostream &stream, const PoseSE2 &pose)

Output stream operator.

Parameters:

  • stream – output stream

  • pose – to be used

Public Functions

~PoseSE2() = default

Destructs the PoseSE2.

Private Members

Eigen::Vector2d position_
double theta_