在 3D 点云处理中,RT 矩阵是一个常用的工具,用于对点云进行旋转和平移操作。本文将详细介绍 RT 矩阵的概念,并通过一个示例程序演示如何基于 PCL 和 Eigen 库将一帧点云进行矩阵变换再输出。
RT 矩阵
RT 矩阵包含旋转矩阵(R)和平移向量(T),组合起来可以描述一个刚体变换。具体来说,RT 矩阵是一个 4x4 的同质坐标变换矩阵,包含两个部分:
旋转矩阵(R) :这是一个 3x3 的矩阵,用于描述点云的旋转。旋转矩阵是一个正交矩阵,表示绕某个轴的旋转。
平移向量(T) :这是一个 3x1 的向量,用于描述点云的平移。平移向量表示在各个方向上的移动距离。
组合起来,RT 矩阵可以表示为:
| ------- > This column is the translation | 1 0 0 x | \ | 0 1 0 y | } - > The identity 3x3 matrix ( no rotation ) on the left | 0 0 1 z | / | 0 0 0 1 | - > We do not use this line ( and it has to stay 0,0 ,0,1 )
其中,R 是 3x3 的旋转矩阵,T 是 3x1 的平移向量,右下角的 1 是为了使矩阵成为同质坐标形式的 4x4 矩阵。
旋转矩阵(R)
旋转矩阵通常可以通过欧拉角、旋转向量或四元数来计算。
欧拉角 :通过绕固定轴(如 X, Y, Z 轴)依次旋转相应的角度来构建旋转矩阵。例如:
绕 X 轴旋转角度(α \alpha α )
R x ( α ) = [ 1 0 0 0 cos α − sin α 0 sin α cos α ] \mathbf{R_x}(\alpha) = \begin{bmatrix}
1 & 0 & 0 \\
0 & \cos\alpha & -\sin\alpha \\
0 & \sin\alpha & \cos\alpha
\end{bmatrix} R x ( α ) = 1 0 0 0 cos α sin α 0 − sin α cos α
绕 Y 轴旋转角度(β \beta β )
R y ( β ) = [ cos β 0 sin β 0 1 0 − sin β 0 cos β ] \mathbf{R_y}(\beta) = \begin{bmatrix}
\cos\beta & 0 & \sin\beta \\
0 & 1 & 0 \\
-\sin\beta & 0 & \cos\beta
\end{bmatrix} R y ( β ) = cos β 0 − sin β 0 1 0 sin β 0 cos β
绕 Z 轴旋转角度(γ \gamma γ )
R z ( γ ) = [ cos γ − sin γ 0 sin γ cos γ 0 0 0 1 ] \mathbf{R_z}(\gamma) = \begin{bmatrix}
\cos\gamma & -\sin\gamma & 0 \\
\sin\gamma & \cos\gamma & 0 \\
0 & 0 & 1
\end{bmatrix} R z ( γ ) = cos γ sin γ 0 − sin γ cos γ 0 0 0 1
通过将这些旋转矩阵按顺序相乘,可以得到最终的旋转矩阵 R \mathbf{R} R 。
旋转向量 :通过旋转轴和旋转角度来构建旋转矩阵。旋转向量表示绕一个单位向量旋转一定角度,使用 Rodrigues 公式可以将其转换为旋转矩阵。
四元数 :四元数是一种表示旋转的方式,能够避免欧拉角的万向节锁问题。 通过四元数转换公式可以得到旋转矩阵。
平移向量(T)
平移向量是一个简单的 3x1 向量,表示在 X, Y, Z 三个方向上的平移量:
T = [ t x t y t z ] \mathbf{T} = \begin{bmatrix}
t_x \\
t_y \\
t_z
\end{bmatrix} T = t x t y t z
应用 RT 矩阵进行点云变换
假设有一个 3D 点 P = [ x y z ] T \mathbf{P} = \begin{bmatrix} x & y & z \end{bmatrix}^T P = [ x y z ] T ,其同质坐标表示为 P h = [ x y z 1 ] T \mathbf{P_h} = \begin{bmatrix} x & y & z & 1 \end{bmatrix}^T P h = [ x y z 1 ] T 。
应用 RT 矩阵进行变换可以表示为:P h ′ = R T ⋅ P h \mathbf{P'_h} = \mathbf{RT} \cdot \mathbf{P_h} P h ′ = RT ⋅ P h 。
其中,P h ′ = [ x ′ y ′ z ′ 1 ] T \mathbf{P'_h} = \begin{bmatrix} x' & y' & z' & 1 \end{bmatrix}^T P h ′ = [ x ′ y ′ z ′ 1 ] T ,展开后为:
[ x ′ y ′ z ′ 1 ] = [ R 11 R 12 R 13 t x R 21 R 22 R 23 t y R 31 R 32 R 33 t z 0 0 0 1 ] ⋅ [ x y z 1 ] \begin{bmatrix} x' \\ y' \\ z' \\ 1 \end{bmatrix} = \begin{bmatrix}
R_{11} & R_{12} & R_{13} & t_x \\
R_{21} & R_{22} & R_{23} & t_y \\
R_{31} & R_{32} & R_{33} & t_z \\
0 & 0 & 0 & 1
\end{bmatrix} \cdot \begin{bmatrix}
x \\ y \\ z \\ 1
\end{bmatrix} x ′ y ′ z ′ 1 = R 11 R 21 R 31 0 R 12 R 22 R 32 0 R 13 R 23 R 33 0 t x t y t z 1 ⋅ x y z 1
经过计算,变换后的点 P ′ \mathbf{P'} P ′ 的坐标为:
P ′ = [ x ′ y ′ z ′ ] = R ⋅ [ x y z ] + T \mathbf{P'} = \begin{bmatrix} x' \\ y' \\ z' \end{bmatrix} = \mathbf{R} \cdot \begin{bmatrix} x \\ y \\ z \end{bmatrix} + \mathbf{T} P ′ = x ′ y