刚体变换与欧拉角：旋转矩阵、平移向量、欧拉角的物理意义

说到外参标定，绕不开的就是刚体变换。说白了，就是把摄像头看到的图像，跟真实世界的坐标系对齐。我刚开始做自动驾驶时，总觉得这玩意儿就是套公式，直到有一次标定出来的车道线歪了十几厘米，才意识到——理解物理意义比背公式重要得多。

刚体变换是什么？

刚体变换，顾名思义，就是物体在空间中只做旋转和平移，不发生形变。你想想看，摄像头装在车上，它不会变形吧？所以摄像头坐标系到车身坐标系的转换，就是一个标准的刚体变换。

数学上，刚体变换可以写成：

P_world = R * P_camera + T

其中：

• R 是旋转矩阵（3x3）
• T 是平移向量（3x1）
• P_camera 是摄像头坐标系下的点
• P_world 是世界坐标系下的点

嗯，这里要注意：旋转矩阵 R 是正交矩阵，它的逆等于它的转置。这个性质在后续计算中非常有用。

旋转矩阵的物理意义

旋转矩阵 R 的每一列，其实代表的是摄像头坐标系的基向量在世界坐标系下的投影。我个人习惯这样理解：

• R 的第一列：摄像头 X 轴在世界坐标系下的方向
• R 的第二列：摄像头 Y 轴在世界坐标系下的方向
• R 的第三列：摄像头 Z 轴在世界坐标系下的方向

我在项目中遇到过一个问题：标定出来的旋转矩阵不满足正交性，结果车道线投影出来全是扭曲的。后来发现是标定板上的角点检测有误差。所以，拿到旋转矩阵后，先检查一下 R * R^T 是不是单位矩阵，这个小习惯能帮你省不少事。

平移向量的物理意义

平移向量 T 就简单多了——它表示摄像头坐标系原点在世界坐标系下的位置。单位是米（或者毫米，取决于你的标定板尺寸）。

举个例子：如果 T = [1.2, 0.5, 1.0]^T，意思是摄像头原点在世界坐标系的 X 方向偏移了 1.2 米，Y 方向偏移了 0.5 米，Z 方向偏移了 1.0 米。

欧拉角：俯仰角、偏航角、翻滚角

旋转矩阵虽然数学上很优雅，但人眼看起来不直观。所以工程上更常用欧拉角来描述旋转。欧拉角有三个分量：

你想想看，如果摄像头装歪了，翻滚角不为零，那图像里的车道线就是斜的。如果摄像头俯仰角不对，远处的车道线位置就会算错。偏航角偏差则会导致左右车道线不对称。

欧拉角的旋转顺序

这里有个坑：欧拉角的旋转顺序很重要。不同的旋转顺序会得到不同的结果。在自动驾驶领域，常用的旋转顺序是 Z-Y-X（先绕 Z 轴转，再绕 Y 轴，最后绕 X 轴）。

我曾经在项目中吃过这个亏：用 OpenCV 的 Rodrigues 函数把旋转矩阵转成欧拉角，结果发现角度值跟预期差很多。后来一查，原来是旋转顺序没对上。OpenCV 默认的旋转顺序是 Z-Y-X，而我用的标定工具是 X-Y-Z 顺序。

从旋转矩阵到欧拉角的转换

假设旋转顺序是 Z-Y-X，那么旋转矩阵 R 可以写成：

R = R_z(ψ) * R_y(θ) * R_x(φ)

其中：

R_x(φ) = [1, 0, 0;
           0, cos(φ), -sin(φ);
           0, sin(φ), cos(φ)]

R_y(θ) = [cos(θ), 0, sin(θ);
           0, 1, 0;
           -sin(θ), 0, cos(θ)]

R_z(ψ) = [cos(ψ), -sin(ψ), 0;
           sin(ψ), cos(ψ), 0;
           0, 0, 1]

从旋转矩阵反推欧拉角的公式：

φ = atan2(R[2][1], R[2][2])
θ = -asin(R[2][0])
ψ = atan2(R[1][0], R[0][0])

嗯，这里要注意 atan2 和 asin 的取值区间。特别是俯仰角 θ，当它接近 ±90° 时会出现万向锁问题。不过好在车载摄像头的俯仰角一般不会那么大，所以实际应用中很少遇到。

知识体系结构图

下面这张图展示了刚体变换与欧拉角的核心逻辑关系：

实际应用中的注意事项

在车道线检测的外参标定中，我总结了几个关键点：

1. 俯仰角最敏感：俯仰角偏差 0.5°，50 米外的车道线位置就会偏移约 0.44 米。所以标定时要重点保证俯仰角的精度。
2. 偏航角影响左右对称性：如果标定出来的偏航角不为零，左右车道线在图像中会不对称。这时候可以检查一下摄像头安装是否居中。
3. 翻滚角容易忽略：翻滚角偏差会导致车道线在图像中倾斜。虽然视觉上不明显，但会影响后续的曲率计算。

好了，关于刚体变换和欧拉角就聊到这里。记住，公式是工具，物理意义才是灵魂。下次标定时，多想想每个参数在真实世界中代表什么，你会发现很多问题其实不用算就能看出来。