刚体变换：旋转矩阵、平移向量、齐次坐标、欧拉角与旋转顺序

说到车道线检测，你想想看，车在动，摄像头也在动，我们怎么知道图像里的那条线到底对应地面上的哪个位置？

说白了，核心就两个字——变换。把世界坐标系里的一个点，映射到图像坐标系里。反过来也一样，从图像反推回真实世界。这个映射过程，就是刚体变换。

我刚开始做自动驾驶项目时，总觉得这玩意儿太数学了，不就是矩阵乘来乘去嘛。直到有一次，标定参数写反了旋转顺序，车道线在弯道处直接飞到了对面车道……嗯，从那以后我再也不敢轻视这个环节了。

1. 旋转矩阵：不只是数学符号

旋转矩阵，说白了就是一个3x3的方阵。它描述了一个坐标系相对于另一个坐标系的朝向。

举个例子，车身坐标系和相机坐标系之间，就靠它来对齐。你想想看，相机装在车顶上，它看出去的方向和车头方向肯定不一样。这个差异，就是旋转矩阵来描述的。

我在项目中遇到过一个问题：用OpenCV的cv2.Rodrigues()把旋转向量转成矩阵时，精度损失导致车道线投影偏差了3个像素。后来我改用双精度浮点，问题就解决了。嗯，这里要注意，嵌入式平台上float和double的差别有时候会坑人。

2. 平移向量：把原点挪过去

旋转搞定了朝向，平移解决位置。平移向量就是一个3x1的向量，表示两个坐标系原点之间的偏移。

比如，相机中心相对于车辆后轴中心的位置，就是平移向量。这个值通常用尺子量，或者通过标定得到。

我个人习惯把平移向量和旋转矩阵合在一起，写成4x4的齐次变换矩阵。为什么？因为这样可以把旋转和平移统一成一个矩阵乘法，代码写起来干净利落。

# 齐次变换矩阵示例
import numpy as np

R = np.eye(3)          # 旋转矩阵
t = np.array([1, 2, 3]) # 平移向量

T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = t

# 现在，一个点 [x, y, z, 1]^T 可以直接左乘 T 完成变换

3. 齐次坐标：为什么多一个1？

你可能会问，为什么非要多加一个维度，变成齐次坐标？

原因很简单：为了统一处理旋转、平移、缩放和投影。在欧氏几何里，平移不是线性变换，没法用3x3矩阵表示。但加上一个维度后，平移就变成了线性变换。

我记得有一次调试车道线逆投影，死活算不对。后来发现是齐次坐标的w分量忘了归一化。说白了，齐次坐标的最后一个分量是1，变换后要除以w才能得到真正的3D坐标。这个细节，新手很容易忽略。

4. 欧拉角与旋转顺序：一个坑接一个坑

欧拉角，就是绕三个轴转的角度。听起来简单，但坑特别多。

首先，旋转顺序不同，结果完全不同。常见的顺序有：

• ZYX：先绕Z轴转（偏航），再绕Y轴转（俯仰），最后绕X轴转（翻滚）——这是自动驾驶里最常用的顺序
• XYZ：先绕X轴转，再绕Y轴转，最后绕Z轴转——某些机器人库默认这个
• ZYZ：绕Z轴转两次，中间夹一次Y轴——这个在机械臂里常见

我曾经犯过一个错误：用ROS的tf库时，默认旋转顺序是ZYX，但我用MATLAB算出来的欧拉角是XYZ顺序。结果两个矩阵对不上，车道线在弯道处直接扭曲了。排查了整整一天才找到原因。

5. 知识体系总览

下面这张图，是我自己总结的刚体变换知识结构。你看一眼，心里就有谱了。

这张图把刚体变换拆成了四个核心模块。你从旋转矩阵和平移向量入手，然后通过齐次坐标把它们合在一起，最后用欧拉角来直观理解旋转。应用到车道线检测时，就是把这些变换串起来，形成一个完整的投影链路。

6. 实战中的避坑指南

最后，分享几个我踩过的坑：

• 旋转顺序不一致：不同库默认顺序不同，一定要显式指定。我习惯在代码注释里写清楚「这里用的是ZYX顺序」。
• 坐标系方向搞反：相机坐标系通常是Z轴朝前，但有些数据集用Z轴朝上。标定前先确认坐标系定义。
• 齐次坐标忘记归一化：变换后w分量可能不是1，要手动除以w。这个bug我至少犯过三次。
• 欧拉角转矩阵时精度问题：用双精度，别用单精度。尤其在嵌入式平台上，float的精度不够。

嗯，刚体变换这部分，说白了就是坐标系之间的「翻译官」。你把这个翻译官搞明白了，车道线检测的整个流程就通了一半。

可可精品资料库