Template Class SO3

Class representing 3D rotations using the SO(3) group.

This class represents rotations in 3D space as elements of the special orthogonal group SO(3). Internally, rotations are stored as unit quaternions to ensure numerical stability and efficient composition operations.

Template Parameters

K – Scalar type (typically float or double).

Public Types

using Scalar = K

Public Functions

inline SO3()

Default constructor creates an uninitialized quaternion.

Note

Use identity() to create an identity rotation.

inline Vector3<K> axis() const

Get the rotation axis.

Returns

Normalized rotation axis.

inline K angle() const

Get the angle of rotation around the axis.

Note

This calls a trigonometric function.

Returns

Rotation angle in radians.

inline const Quaternion<K> &quaternion() const

Get the quaternion representation of the rotation.

Returns

Unit quaternion representing the rotation.

inline Matrix3<K> matrix() const

Get the 3×3 rotation matrix representation.

Returns

3×3 rotation matrix.

inline Vector3<K> euler_angles_rpy() const

Get the roll, pitch, yaw Euler angles of the rotation.

Returns

Vector containing (roll, pitch, yaw) angles in radians.

inline SO3 inverse() const

Get the inverse rotation.

Returns

Inverse rotation.

template<typename S, typename std::enable_if_t<!std::is_same<S, K>::value, int> = 0>
inline SO3<S> cast() const

Cast to a different scalar type.

Template Parameters

S – Target scalar type.

Returns

Rotation cast to the target type.

template<typename S, typename std::enable_if_t<std::is_same<S, K>::value, int> = 0>
inline const SO3 &cast() const

Cast to the same scalar type (no-op).

Template Parameters

S – Target scalar type (same as K).

Returns

Reference to this rotation.

inline SO2<K> to_so2_xy() const

Convert to a 2D rotation in the XY plane.

Returns

2D rotation representing the Z-axis component of this 3D rotation.

inline Matrix<K, 3, 4> vector_rotation_jacobian(const Vector3<K> &n) const

Compute the Jacobian of the rotation of a normal vector.

Plane normals only have rotation components.

Parameters

n – Normal vector to compute Jacobian for.

Returns

3×4 Jacobian matrix.

Public Static Functions

static inline SO3 identity()

Create the identity rotation.

Returns

Identity rotation (no rotation).

static inline SO3 from_scaled_axis(const Vector3<K> &axis_angle)

Create rotation which rotates around the given axis by the magnitude of the axis.

Parameters

axis_angle – Scaled axis vector where the magnitude represents the rotation angle.

Returns

Rotation around the scaled axis.

static inline SO3 from_axis_angle(const Vector3<K> &axis, K angle)

Create rotation which rotates by an angle around a given axis.

Parameters

• axis – Rotation axis (will be normalized internally).

• angle – Rotation angle in radians.

Returns

Rotation around the given axis by the specified angle.

static inline SO3 from_angle_axis(K angle, const Vector3<K> &axis)

Create rotation from angle and axis (alternative parameter order).

Parameters

• angle – Rotation angle in radians.

• axis – Rotation axis (will be normalized internally).

Returns

Rotation around the given axis by the specified angle.

static inline SO3 from_quaternion(const Quaternion<K> &quaternion)

Create rotation from a (not necessarily normalized) quaternion.

Parameters

quaternion – Quaternion representation (will be normalized internally).

Returns

Rotation represented by the normalized quaternion.

static inline SO3 from_normalized_quaternion(const Quaternion<K> &quaternion)

Create rotation from a normalized quaternion.

Note

This will assert if the quaternion does not have unit length.

Parameters

quaternion – Normalized quaternion representation.

Returns

Rotation represented by the quaternion.

static inline SO3 from_so2_xy(const SO2<K> &rotation)

Create a 3D rotation from a 2D rotation in the XY plane.

Parameters

rotation – 2D rotation in the XY plane.

Returns

3D rotation equivalent to the 2D rotation around the Z-axis.

static inline SO3 from_matrix(const Matrix<K, 3, 3> &matrix)

Create rotation from a 3×3 rotation matrix.

Parameters

matrix – 3×3 rotation matrix.

Returns

Rotation represented by the matrix.

static inline SO3 from_euler_angles_rpy(K roll_angle, K pitch_angle, K yaw_angle)

Create rotation from roll/pitch/yaw Euler angles.

Parameters

• roll_angle – Roll angle in radians (rotation around X-axis).

• pitch_angle – Pitch angle in radians (rotation around Y-axis).

• yaw_angle – Yaw angle in radians (rotation around Z-axis).

Returns

Rotation representing the given Euler angles.

static inline SO3 from_euler_angles_rpy(const Vector3d &roll_pitch_yaw)

Create rotation from Euler angles vector.

Parameters

roll_pitch_yaw – Vector containing (roll, pitch, yaw) angles in radians.

Returns

Rotation representing the given Euler angles.

Public Static Attributes

static constexpr int kDimension = 3

Friends

inline friend SO3 operator*(const SO3 &lhs, const SO3 &rhs)

Compose two rotations.

Parameters

• lhs – Left rotation.

• rhs – Right rotation.

Returns

Composed rotation lhs * rhs.

inline friend Vector3<K> operator*(const SO3 &rot, const Vector3<K> &vec)

Rotate a 3D vector by the given rotation.

Parameters

• rot – Rotation to apply.

• vec – Vector to rotate.

Returns

Rotated vector.