Conversions Between Common Orientation Representations

This article presents equations for conversion between orientation representations common for robots. For more information on the pose representations check out Position, Orientation and Coordinate Transformations.

Roll-Pitch-Yaw to Rotation Matrix

Roll-pitch-yaw is a common term to denote an orientation. Each represents an angle of rotation around a single axis, combined they represent a complete rotation. However, they are not explicit in terms of exactly what they represent. They are subject to the following confusions:

Which axis does each rotate about?
Are these axes fixed, or moving?
In which order is the rotation defined (There are multiple possibilities)?

Order of rotations are often denoted x-y-z or z-y'-x''. Here x, y and z denotes the axis for which it is rotated around. ' is used to indicate whether or not the axes are fixed or not. Rotation around fixed axes is called extrinsic rotation. Rotation around moving axes is called intrinsic rotation.

What follows are two different examples.

For x-y-z or z-y'-x'', roll \(\phi\), pitch \(\theta\), and yaw \(\psi\) angles can be converted to a rotation matrix \(R\) as follows:

\[\begin{split}\boldsymbol{R}_{x-y-z} = &\boldsymbol{R}_{z-{y}^{'}-{x}^{''}} =\boldsymbol{R}_{z,\phi}\boldsymbol{R}_{y,\theta}\boldsymbol{R}_{x,\psi} = \\ \begin{bmatrix} cos{\phi} & -sin{\phi} & 0 \\ sin{\phi} & cos{\phi} & 0\\ 0 & 0 & 1\end{bmatrix}& \begin{bmatrix} cos{\theta} & 0 & sin{\theta} \\ 0 & 1 & 0 \\ -sin{\theta} & 0 & cos{\theta} \end{bmatrix} \begin{bmatrix} 1 & 0 & 0\\ 0 & cos{\psi} & -sin{\psi} \\ 0 & sin{\psi} & cos{\psi} \end{bmatrix}\end{split}\]

For z-y-x or x-y'-z'', roll \(\phi\), pitch \(\theta\), and yaw \(\psi\) angles can be converted to a rotation matrix \(R\) as follows:

\[\begin{split}\boldsymbol{R}_{z-y-x} = &\boldsymbol{R}_{x-{y}^{'}-{z}^{''}} =\boldsymbol{R}_{x,\phi}\boldsymbol{R}_{y,\theta}\boldsymbol{R}_{z,\psi} = \\ \begin{bmatrix} 1 & 0 & 0\\ 0 & cos{\phi} & -sin{\phi} \\ 0 & sin{\phi} & cos{\phi} \end{bmatrix}& \begin{bmatrix} cos{\theta} & 0 & sin{\theta} \\ 0 & 1 & 0 \\ -sin{\theta} & 0 & cos{\theta} \end{bmatrix} \begin{bmatrix} cos{\psi} & -sin{\psi} & 0 \\ sin{\psi} & cos{\psi} & 0\\ 0 & 0 & 1\end{bmatrix}\end{split}\]

Note

If one assumes that the angles are the same in the two examples, then they do not represent the same final rotation matrix.

If one assumes that the final rotation matrixes are the same, then the angles are not identical between the two examples.

A definition is introduced: roll angle is assigned to the first rotation about moving axes, pitch is the second and yaw is the third.

C++

rollPitchYawListToRotationMatrix(rpyList);

C#

rollPitchYawListToRotationMatrix(rpyList);

Python

roll_pitch_yaw_to_rotation_matrix(rpy_list)

Rotation Vector to Axis-Angle

A rotation vector \(\boldsymbol{r}\) can be converted to axis \(\boldsymbol{u}\) and angle \(\theta\) as follows:

\[\theta = \sqrt{{r_x}^2+{r_y}^2+{r_z}^2}\]

\[\begin{split}\boldsymbol{u} = \begin{bmatrix} \frac{r_x}{\theta} & \frac{r_y}{\theta} & \frac{r_z}{\theta} \\ \end{bmatrix}\end{split}\]

Axis-Angle to Quaternion

Axis \(\boldsymbol{u}\) and angle \(\theta\) can be converted to a unit quaternion \(\boldsymbol{q}\) as follows:

\[\begin{split}\boldsymbol{q} = \begin{bmatrix} \cos{\frac{\theta}{2}} & u_x \sin{\frac{\theta}{2}} & u_y \sin{\frac{\theta}{2}} & u_z \sin{\frac{\theta}{2}} \\ \end{bmatrix}\end{split}\]

Quaternion to Rotation Matrix

A unit quaternion \(\boldsymbol{q}\) can be converted to a rotation matrix \(\boldsymbol{R}\) as follows:

\[\begin{split}\boldsymbol{R} = \begin{bmatrix} 1 - 2 {q_{y}}^2 - 2 {q_{z}}^2 & 2 q_{x} q_{y} - 2 q_{z} q_{w} & 2 q_{x} q_{z} - 2 q_{y} q_{w} \\ 2 q_{x} q_{y} - 2 q_{z} q_{w} & 1 - 2 {q_{x}}^2 - 2 {q_{z}}^2 & 2 q_{y} q_{z} - 2 q_{x} q_{w} \\ 2 q_{x} q_{z} - 2 q_{y} q_{w} & 2 q_{y} q_{z} - 2 q_{x} q_{w} & 1 - 2 {q_{x}}^2 - 2 {q_{y}}^2\\ \end{bmatrix}\end{split}\]

It is assumed that the quaternion is normalized \((q_w + q_x + q_y + q_z = 1)\). If not, it should be normalized before doing the conversion using this equation:

\[n = \sqrt{{q_w}^2+{q_c}^2+{q_y}^2+{q_z}^2}\]

\[\begin{split}\boldsymbol{q} = \begin{bmatrix} \frac{q_w}{n} & \frac{q_x }{n} & \frac{q_y }{n} & \frac{q_z }{ n}\\ \end{bmatrix}\end{split}\]

C++

const Eigen::Matrix3f rotationMatrixFromQuaternion = quaternion.toRotationMatrix();
std::cout << "Rotation Matrix from Quaternion:\n"
          << rotationMatrixFromQuaternion.format(matrixFormatRules) << std::endl;

C#

var rotationMatrixFromQuaternion = quaternionToRotationMatrix(quaternion);
Console.WriteLine("Rotation Matrix from Quaternion:\n" + matrixToString(rotationMatrixFromQuaternion));

Python

rotation_matrix_from_quaternion = quaternion_to_rotation_matrix(quaternion)
print(f"Rotation Matrix from Quaternion:\n{rotation_matrix_from_quaternion}")

Rotation Matrix to Quaternion

A rotation matrix \(\boldsymbol{R}\) can be converted to a unit quaternion \(\boldsymbol{q}\) as follows:

\[q_w = \frac{\sqrt{1 + r_{11} + r_{22}+ r_{33}}}{2}\]

\[\begin{split}\boldsymbol{q} = \begin{bmatrix} q_w & \frac{r_{32} - r_{23}}{4 q_w} & \frac{r_{13} - r_{31}}{4 q_w} & \frac{r_{21} - r_{12}}{4 q_w} \\ \end{bmatrix}\end{split}\]

C++

const Eigen::Quaternionf quaternion(rotationMatrix);
std::cout << "Quaternion:\n" << quaternion.coeffs().format(vectorFormatRules) << std::endl;

C#

var quaternion = rotationMatrixToQuaternion(rotationMatrix);
Console.WriteLine("Quaternion:\n" + matrixToString(quaternion.Transpose()));

Python

quaternion = rotation_matrix_to_quaternion(rotation_matrix)
print(f"Quaternion:\n{quaternion}")

Quaternion to Axis-Angle

A unit quaternion \(\boldsymbol{q}\) can be converted to axis \(\boldsymbol{u}\) and angle \(\theta\) as follows:

\[\theta = 2 \cos^{-1}{q_w}\]

\[\begin{split}\boldsymbol{u} = \begin{bmatrix} \frac{q_x}{\sqrt{1-{q_w}^2}} & \frac{q_y}{\sqrt{1-{q_w}^2}} & \frac{q_z}{\sqrt{1-{q_w}^2}} \\ \end{bmatrix}\end{split}\]

This is useful for converting from rotation matrix to axis-angle. See our code sample implementation below.

C++

const Eigen::AngleAxisf axisAngle(rotationMatrix);
std::cout << "AxisAngle:\n"
          << axisAngle.axis().format(vectorFormatRules) << ", " << axisAngle.angle() << std::endl;

C#

var axisAngle = rotationMatrixToAxisAngle(rotationMatrix);
Console.WriteLine("AxisAngle:\n" + matrixToString(axisAngle.Axis.Transpose()) + ", " + String.Format(" {0:G4} ", axisAngle.Angle));

Python

axis_angle = rotation_matrix_to_axis_angle(rotation_matrix)
print(f"AxisAngle:\n{axis_angle.axis}, {axis_angle.angle:.4f}")

Axis-Angle to Rotation Vector

Axis \(\boldsymbol{u}\) and angle \(\theta\) can be converted to a rotation vector \(\boldsymbol{r}\) as follows:

\[\begin{split}\boldsymbol{r} = \begin{bmatrix} u_x \theta & u_y \theta & u_z \theta \\ \end{bmatrix}\end{split}\]

This is useful for converting rotation matrix to rotation vector. See our code sample implementation below.

C++

const Eigen::Vector3f rotationVector = rotationMatrixToRotationVector(rotationMatrix);
std::cout << "Rotation Vector:\n" << rotationVector.format(vectorFormatRules) << std::endl;

C#

var rotationVector = axisAngle.Axis * axisAngle.Angle;
Console.WriteLine("Rotation Vector:\n" + matrixToString(rotationVector.Transpose()));

Python

rotation_vector = rotation_matrix_to_rotation_vector(rotation_matrix)
print(f"Rotation Vector:\n{rotation_vector}")

Rotation Matrix to Roll-Pitch-Yaw

Determining roll, pitch, yaw angles from a rotation matrix is not straightforward. There can be multiple and sometimes even infinite solutions. This requires an algorithm that can choose one of the multiple solutions based on some criteria.

C++

const auto rpyList = rotationMatrixToRollPitchYawList(rotationMatrix);

C#

var rpyList = rotationMatrixToRollPitchYawList(rotationMatrix);

Python

rpy_list = rotation_matrix_to_roll_pitch_yaw(rotation_matrix)