欧拉角和旋转矩阵之间的变换，以及对transforms3d.euler的理解
引言
在3D SOT目标跟踪中，常常涉及3D Bounding Box的计算。已知目标的中心点坐标$[x,y,z]$，尺寸$[l,w,h]$和目标的旋转矩阵，可以计算出目标的3D BB的8个顶点坐标：
def compute_3d_box_vertices(center, dim, rotation):
"""
Compute the 8 vertices of a 3D bounding box.
Args:
center: A numpy array [cx, cy, cz] representing the center of the box.
dim: A numpy array [length, width, height] representing the dimensions of the box.
rotation: A numpy array [roll, pitch, yaw] representing the rotation angles around the x, y, z axes respectively.
Returns:
A numpy array of shape (8, 3) representing the 8 vertices of the 3D bounding box.
"""
cx, cy, cz = center
length, width, height = dim
# Define the 8 vertices of the box in the local coordinate system
local_vertices = np.array([
[length / 2, width / 2, height / 2],
[length / 2, -width / 2, height / 2],
[-length / 2, -width / 2, height / 2],
[-length / 2, width / 2, height / 2],
[length / 2, width / 2, -height / 2],
[length / 2, -width / 2, -height / 2],
[-length / 2, -width / 2, -height / 2],
[-length / 2, width / 2, -height / 2]
])
# Combine the rotation matrices
# r = compute_rotation_matrix(rotation)
r = euler2mat(rotation[0], rotation[1], rotation[2], 'rxyz')
# Rotate the vertices and translate them to the center position
vertices = np.dot(local_vertices, r.T) + np.array([cx, cy, cz])
return vertices
首先，初始化顶点在局部坐标系中相对于中心点的相对坐标local_vertices，然后与旋转矩阵r相乘，最后加上中心点坐标，即可计算出目标顶点在全局坐标系中的坐标。
一般情况下，目标的方向不会以旋转矩阵的形式给出，而是以欧拉角、四元数等形式给出。例如，KITTI给出了3D物体的空间方向（rotation_y），其取值范围为$(-\pi, \pi)$（单位：$\text{rad}$），它表示，在照相机坐标系下，物体的全局方向角（物体前进方向与相机坐标系$x$轴的夹角，前进方向相对于$x^+$，逆时针为负，顺时针为正）。KITTI常用于自动驾驶场景，绝大部分目标的空间方向与地面（雷达坐标系的$xy$平面）“平行”，因此，只给出前进方向，不影响目标3D BB的恢复。
然而，在一般场景（不限定于室内或自动驾驶领域的3D数据集）中，目标的方向可以在全局坐标系中任意变换。此时，需要给出目标的欧拉角或四元数，计算出目标的旋转矩阵，从而准确恢复目标的3D BB。
欧拉角到旋转矩阵的变换
欧拉角的相关知识不再赘述，在这里指定目标的欧拉角为$[roll,pitch,yaw]$，旋转顺序为$ZYX$。一个简单的想法是，直接通过rpy计算出三个方向上的旋转矩阵，代码如下：
def compute_rotation_matrix(rotation):
"""
Compute the rotation matrix from yaw, pitch, and roll angles.
Args:
rotation (array-like): A 3-element array representing the rotation angles (yaw, pitch, roll) in radians.
Returns:
np.ndarray: A 3x3 rotation matrix.
"""
roll, pitch, yaw = rotation
r_x = np.array([
[1, 0, 0],
[0, np.cos(roll), -np.sin(roll)],
[0, np.sin(roll), np.cos(roll)]
])
r_y = np.array([
[np.cos(pitch), 0, np.sin(pitch)],
[0, 1, 0],
[-np.sin(pitch), 0, np.cos(pitch)]
])
r_z = np.array([
[np.cos(yaw), -np.sin(yaw), 0],
[np.sin(yaw), np.cos(yaw), 0],
[0, 0, 1]
])
r = r_z @ r_y @ r_x
return r
这种做法得到的结果在KITTI等场景中是准确的。这类情况的共性是，目标（例如车）的$yaw$对应的$z$轴与地面（雷达坐标系的$xy$平面）垂直。在一般场景中，当目标任意旋转时，无法满足$z$轴始终与地面垂直，此时上述算法恢复出来的方向角可能存在角度的偏差，例如：
左图采用上述算法恢复目标方向，右图为正确结果。可以看出上述算法在一般情况中计算结果有误。
如何正确实现欧拉角到旋转矩阵的转换
transforms3D是一个python包，用于实现一系列3D坐标变换。该库提供了transforms3d.euler.euler2mat(*ai*, *aj*, *ak*, *axes='sxyz'*)函数，能够实现欧拉角到旋转矩阵的变换
这里需要详细解释一下axes参数，该参数默认值为sxyz。文档中对该参数的定义如下：
A triple of Euler angles can be applied/interpreted in 24 ways, which can be specified using a 4 character string or encoded 4-tuple:
Axes 4-string: e.g. ‘sxyz’ or ‘ryxy’
- first character : rotations are applied to ‘s’tatic or ‘r’otating frame
- remaining characters : successive rotation axis ‘x’, ‘y’, or ‘z’
Axes 4-tuple: e.g. (0, 0, 0, 0) or (1, 1, 1, 1)
- inner axis: code of axis (‘x’:0, ‘y’:1, ‘z’:2) of rightmost matrix.
- parity : even (0) if inner axis ‘x’ is followed by ‘y’, ‘y’ is followed by ‘z’, or ‘z’ is followed by ‘x’. Otherwise odd (1).
- repetition : first and last axis are same (1) or different (0).
- frame : rotations are applied to static (0) or rotating (1) frame.
也就是说，该参数表示4个字符编码欧拉角可能的24种形式。24种参数对应的编码表为：
# map axes strings to/from tuples of inner axis, parity, repetition, frame
_AXES2TUPLE = {
'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),
'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),
'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),
'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),
'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),
'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),
'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),
'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}
_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
前文提到了默认的欧拉角旋转顺序为$ZYX$，对应的axes参数中的rxyz。原因如下：
由表可知，rxyz对应的编码为(2,1,0,1)，其含义为：
- 旋转矩阵中三个子子矩阵相乘，最右边的是$z$
- 奇偶校验结果为奇数，即紧跟$z$的不是
x，而是y - 最后一个旋转的轴与第一个轴（
z）不同，为x - 使用目标的全局坐标
因此，参数axes=rxyz时，表示目标在全局坐标系下，按照$ZYX$的顺序进行旋转。这样，就正确计算出了目标的旋转矩阵。
文档信息
- 本文作者：焦逸凡
- 本文链接：https://ailovejinx.github.io/2024/07/16/blog-transforms/
- 版权声明：自由转载-非商用-非衍生-保持署名（创意共享3.0许可证）