A \(4 \times 4\) Homogeneous Transformation Matrix encapsulates translation, rotation, and scaling operations. This matrix is defined as:
\[\mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix}\]
where:
- \(\mathbf{R}\) is a \(3 \times 3\) matrix containing rotation and scaling
- \(\mathbf{t}\) is a \(3 \times 1\) translation vector
- The bottom filler row is \([0,0,0,1]\)
Extracting Translation
The translation vector \(\mathbf{t}\) can be directly extracted from the right column of matrix \(\mathbf{T}\):
\[\mathbf{t} = \begin{bmatrix} \mathbf{T}_{14} \\ \mathbf{T}_{24} \\ \mathbf{T}_{34} \end{bmatrix}\]
Extracting Rotation and Scaling
To separate the rotation and scaling components from the \(3 \times 3\) matrix \(\mathbf{R}\), we use Singular Value Decomposition (SVD) if necessary. Before performing SVD, we check if \(\mathbf{R}\) is already a valid rotation matrix.
Validating the Rotation Matrix
A square matrix \(\mathbf{R}\) is a valid rotation matrix if it satisfies:
- Orthonormality: The columns (and rows) of \(\mathbf{R}\) are mutually perpendicular and have unit length, which is the case iff \(\mathbf{R}^T\) equals \(\mathbf{R}^{-1}\), or: \[\mathbf{R}^T \mathbf{R} = \mathbf{I}\]
- Determinant: The determinant of \(\mathbf{R}\) is \(+1\) and therefore preserve lengths and angles: \[\det(\mathbf{R}) = 1\]
If \(\mathbf{R}\) is not a valid rotation matrix, we decompose it using SVD:
\[\mathbf{R} = \mathbf{U} \mathbf{\Sigma} \mathbf{V}^T\]
where:
- \(\mathbf{U}\) and \(\mathbf{V}\) are orthogonal matrices.
- \(\mathbf{\Sigma}\) is a diagonal matrix containing the scaling factors.
The rotation matrix \(\mathbf{R}_{\text{rot}}\) can be reconstructed as:
\[\mathbf{R}_{\text{rot}} = \mathbf{U} \mathbf{V}^T\]
Extracting Rotation Angles
To extract the rotation angles around the X, Y, and Z axes from the rotation matrix, we use the Euler angles representation (ZYX convention).
Given the rotation matrix
\[ \mathbf{R}_{\text{rot}} = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} \]
The Euler angles can be extracted as:
- Yaw (Z-axis rotation): \[ \text{yaw} = \alpha = \arctan2(r_{21}, r_{11}) \]
- Pitch (Y-axis rotation): \[ \text{pitch} = \beta = \arcsin(-r_{31}) \]
- Roll (X-axis rotation): \[ \text{roll} = \gamma = \arctan2(r_{32}, r_{33}) \]
Implementation in Python
Here is a compact Python example demonstrating the extraction of translation, rotation, and scaling components from a homogeneous transformation matrix, and obtaining the rotation angles:
import numpy as np
def rotation_matrix_to_euler_angles(R):
"""
Convert a rotation matrix to Euler angles in the ZYX order.
:param R: 3x3 rotation matrix
:return: tuple of Euler angles (yaw, pitch, roll)
"""
assert R.shape == (3, 3), "Input matrix must be 3x3"
yaw = np.arctan2(R[1, 0], R[0, 0])
pitch = np.arcsin(-R[2, 0])
roll = np.arctan2(R[2, 1], R[2, 2])
return yaw, pitch, roll
# Define the rotation angle in radians
theta = np.radians(30)
# Create a rotation matrix around the Z-axis
R_z = np.array([
[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0],
[0, 0, 1]
])
# Introduce scaling factors for each axis
scaling_factors = np.array([1.2, 0.8, 1.0])
S = np.diag(scaling_factors)
# Combine rotation and scaling
R_combined = np.dot(R_z, S)
# Define a homogeneous transformation matrix with translation
T = np.array([
[R_combined[0,0], R_combined[0,1], R_combined[0,2], 1],
[R_combined[1,0], R_combined[1,1], R_combined[1,2], 2],
[R_combined[2,0], R_combined[2,1], R_combined[2,2], 3],
[0, 0, 0, 1]
])
## Now try to reconstruct the inputs:
# Extract the rotation and scaling matrix R from T
R = T[:3, :3]
# Check if R already is a valid rotation matrix
if np.allclose(np.dot(R.T, R), np.eye(3)) and np.isclose(np.linalg.det(R), 1.0):
R_rot = R
scaling = np.ones(3)
else:
# Perform SVD on the extracted matrix
U, Sigma, Vt = np.linalg.svd(R)
R_rot = np.dot(U, Vt)
scaling = Sigma
# Extract the translation vector t from T
t = T[:3, 3]
# Extract Euler angles from the rotation matrix
yaw, pitch, roll = rotation_matrix_to_euler_angles(R_rot)
# Print results
print("Extracted Rotation Matrix R_rot:")
print(R_rot)
print("Extracted Scaling Factors:")
print(scaling)
print("Extracted Translation Vector t:")
print(t)
print("Extracted Euler Angles (yaw, pitch, roll):")
print(np.degrees([yaw, pitch, roll]))