Rot3

A gtsam.Rot3 represents an orientation or attitude in 3D space. It can be manipulated and presented as a rotation matrix $R \in \mathbb{R}^{3 \times 3}$, a unit quaternion, roll-pitch-yaw (Euler) angles $(\phi, \theta, \psi)$, or as an axis-angle representation $(\hat{\omega}, \theta)$ with $\hat{\omega} \in \mathbb{R}^3$ and $\theta \in \mathbb{R}$. It models a 3D orientation as both a manifold in $\mathcal{SO}(3)$ and as a Lie group in $\text{SO}(3)$. Internally, it is stored as a $3 \times 3$ rotation matrix but can be configured to use quaternions at build time for efficiency.

Initialization¶

A Rot3 can be initialized in many different ways, which are detailed in this section. Note that printing a Rot3 displays its 3x3 rotation matrix representation, which in general is a 3x3 matrix where the columns are unit vectors that define the orientation’s coordinate frame.

Constructor¶

The Rot3 constructor provides for initialization with no arguments, yielding the identity rotation (equivalent to $I_3$), initialization with a precalculated rotation matrix (either as a 3x3 np.ndarray, as three 3-vectors, or as 9 floats), and initialization with a quaternion’s $w, x, y, z$.

Named constructors¶

In addition to its constructors, Rot3 has several named constructors, or factory functions, that allow instantiation from a wide variety of methods.

Rot3.Identity() returns the 3x3 rotation identity matrix.

Rx, Ry, Rz, and RzRyRx create rotations around these axes.

Similarly, Yaw, Pitch, Roll, and Ypr are available.

Rot3.Quaternion is identical to the four-argument Rot3 constructor.

Rot3.AxisAngle creates a Rot3 from an axis and an angle around that axis.

Rot3.Rodrigues creates a Rot3 from incremental roll, pitch, and yaw values. It is identical to the exponential map at identity.

Rot3.ClosestTo finds the closest valid Rot3 to the input matrix which minimizes the Frobenius norm. The Frobenius norm is a measure of matrix difference:

Properties¶

The following properties are available from the standard interface:

• matrix(): Returns the 3x3 rotation matrix.
• transpose(): Returns the transpose of the 3x3 rotation matrix.
• xyz(): Returns the 3 Euler angles as $x, y, z$.
• ypr(): Returns the 3 Euler angles as yaw, pitch, and roll.
• rpy(): Returns the 3 Euler angles as roll, pitch, and yaw.
• roll(): Returns the roll angle.
• pitch(): Returns the pitch angle.
• yaw(): Returns the yaw angle.
• axisAngle(): Returns the axis-angle representation.
• toQuaternion(): Returns the quaternion representation. The quaternion’s attributes can then be accessed either individually with w(), x(), y(), z() or together with coeffs().

Note that accessing roll(), pitch(), and yaw() separately is less efficient than calling rpy() or ypr().

Basic operations¶

Rot3 can rotate and unrotate a 3D point or vector. Rotation is calculated by the simple matrix product $Rx$, and unrotation by $R^{-1}x$.

Check whether two Rot3 instances are equal within a certain tolerance using equals(). Be careful with the == operator; it does not compare rotational equivalence, it compares object reference. If you wish to use more fine-grained equality comparison, convert to np.ndarray with matrix().

Use SLERP (spherical linear interpolation) to interpolate between two Rot3 instances. In terms of the Lie algebra (see below), SLERP can be calculated by scaling the log mapped relative rotation by the interpolation term $t$, then converting back to $\text{SO}(3)$ using the exponential map. The formula is thus:

where $R_1$ and $R_2$ are the start Rot3 and end Rot3 of the interpolation and $t$ is the interpolation term, usually but not necessarily in the range $[0, 1]$.

Lie group $\text{SO}(3)$¶

Group operations¶

Rot3 implements the group operations inverse, compose, between and identity. For more information on groups and their use here, see GTSAM concepts.

The inverse of an $\text{SO}(3)$ rotation matrix is the same as its transpose.

The product of the composition operation $A * B$ is the rotation matrix which applies the rotation of $A$ and then the rotation of $B$. The composition of two rotation matrices is just the product of the two matrices.

The between operation calculates the rotation from one Rot3 to another. It is defined as simply:

The identity is $I_3$, as described above.

Group operation invariants¶

See that the following group invariants hold:

Lie group operations¶

Rot3 implements the Lie group operations for exponential mapping and log mapping. For more information on Lie groups and their use here, see GTSAM concepts.

The exponential map for $\text{SO}(3)$ converts a 3D rotation vector (Lie algebra element in $\mathfrak{so}(3)$) into a rotation matrix (Lie group element in $\text{SO}(3)$). This is used to map a rotation vector $\boldsymbol{\omega} \in \mathbb{R}^3$ to a rotation matrix $R \in \text{SO}(3)$.

Given a rotation vector $\boldsymbol{\omega} = (\omega_x, \omega_y, \omega_z)$, define:

• The rotation axis as the unit vector:

  $$\hat{\omega} = \frac{\boldsymbol{\omega}}{\theta}, \quad \text{where } \theta = \|\boldsymbol{\omega}\|$$

  (4)

• The skew-symmetric matrix $[\boldsymbol{\omega}]_\times$:

  $$[\boldsymbol{\omega}]_\times = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix}$$

  (5)

Using Rodrigues’ formula, the exponential map is:

which results in the rotation matrix:

where:

• $I$ is the $3 \times 3$ identity matrix.
• $\theta$ is the magnitude of the rotation vector (rotation angle).
• $[\boldsymbol{\omega}]_\times$ represents the skew-symmetric matrix, the infinitesimal rotation generator.

For very small $\theta$, we use the Taylor series approximation:

since $\sin\theta \approx \theta$ and $1 - \cos\theta \approx \frac{\theta^2}{2}$.

The logarithm map for $\text{SO}(3)$ is the inverse of the exponential map It converts a rotation matrix $R \in SO(3)$ into a 3D rotation vector (a Lie algebra element in $\mathfrak{so}(3)$).

Given a rotation matrix $R$, the corresponding rotation vector $\boldsymbol{\omega} \in \mathbb{R}^3$ is computed as:

where:

• $\theta$ is the rotation angle:

  $$\theta = \cos^{-1} \left( \frac{\text{Tr}(R) - 1}{2} \right)$$

  (10)

• $\hat{\omega}$ is the rotation axis, obtained from the skew-symmetric matrix $[\boldsymbol{\omega}]_\times$:

  $$[\boldsymbol{\omega}]_\times = \frac{\theta}{2 \sin\theta} (R - R^T)$$

  (11)

• Extracting the components:

  $$\boldsymbol{\omega} = \frac{\theta}{2 \sin\theta} (R - R^T)^\vee$$

  (12)

For small $\theta$, we use the Taylor series approximation:

In both cases, $(R - R^T)^\vee$ extracts the unique 3D vector from a skew-symmetric matrix:

where $R_{ij}$ are the elements of $R$.

Serialization¶

A Rot3 can be serialized to a string for saving, then later used by deserializing the string.