第三章 坐标变换基础：刚体变换、旋转矩阵、欧拉角、四元数、齐次坐标
做BEV感知，说白了就是在跟坐标系打交道。
我记得刚入行那会儿，有个师兄跟我说过一句话：「搞自动驾驶，一半的时间在调模型，另一半的时间在搞坐标变换。」当时我不信，直到自己踩了坑才明白——坐标变换搞不清楚，你连点云和图像都对齐不了，更别提什么鸟瞰图了。
这一章，咱们就把坐标变换的底裤扒干净。我会从最基础的刚体变换讲起，然后聊旋转矩阵、欧拉角、四元数，最后用齐次坐标把所有东西串起来。嗯，内容不少，但都是硬通货。
核心要点：坐标变换是BEV感知的数学基础。不理解它，你写的代码就是空中楼阁。
3.1 刚体变换：物体在空间中的「搬家」
刚体变换，说白了就是物体在三维空间里移动和转动，但形状和大小不变。你想想看，一辆车在路上开，它本身不会变形吧？这就是刚体变换。
刚体变换由两部分组成：
- 旋转（Rotation）——物体绕某个轴转了个角度
- 平移（Translation）——物体从A点挪到了B点
我在项目中遇到过一个问题：激光雷达和相机的外参标定，本质上就是在求一个刚体变换矩阵。如果这个矩阵算错了，点云投影到图像上就会「飘」，偏差可能达到几十个像素。嗯，这种问题排查起来特别头疼。
个人经验：我建议你在做标定之前，先用手边的工具（比如RVIZ或者MeshLab）可视化一下两个传感器的坐标系。亲眼看到它们之间的相对位置关系，比光看数字要直观得多。
3.2 旋转矩阵：最正统的旋转表示
旋转矩阵，是描述旋转最标准的方式。它是一个3x3的正交矩阵，行列式为+1。
为什么用矩阵？因为矩阵可以跟向量直接相乘，方便啊！
// 绕Z轴旋转θ角度的旋转矩阵
R_z(θ) = [
[cosθ, -sinθ, 0],
[sinθ, cosθ, 0],
[0, 0, 1]
]
// 点P绕Z轴旋转后的坐标
P' = R_z(θ) * P旋转矩阵有三个重要性质：
- 正交性：R^T * R = I，也就是说逆矩阵等于转置矩阵
- 行列式为+1：保证了旋转不会产生镜像
- 可组合：多个旋转可以连乘，R_total = R3 * R2 * R1
注意：旋转矩阵的乘法不满足交换律！也就是说R1 * R2 ≠ R2 * R1。我曾经在写代码时搞混了顺序，结果点云投影出来全乱套了。排查了整整一个下午才发现是矩阵乘法顺序的问题。
3.3 欧拉角：直观但容易踩坑
欧拉角用三个角度来表示旋转：绕X轴转（roll）、绕Y轴转（pitch）、绕Z轴转（yaw）。
为什么说它直观？因为你可以直接说「车头向右转了30度」，大家都能听懂。
但欧拉角有个致命的问题——万向锁（Gimbal Lock）。
什么是万向锁？当pitch角达到±90度时，roll和yaw的旋转轴会重合，导致丢失一个自由度。你想想看，本来三个自由度，突然变成两个了，这谁受得了？
我在做IMU数据处理时就遇到过这个问题。IMU输出的欧拉角，一旦车辆爬陡坡（pitch接近90度），姿态解算就崩了。后来我果断换成了四元数，问题才解决。
避坑指南：如果你在做连续旋转或者大角度旋转，尽量别用欧拉角。用四元数或者旋转矩阵，虽然看起来复杂一点，但至少不会突然「卡死」。
3.4 四元数：旋转的「瑞士军刀」
四元数，听起来很高大上，其实就是一个四维向量：q = w + xi + yj + zk。
其中w是实部，x、y、z是虚部。约束条件是w² + x² + y² + z² = 1。
为什么用四元数？三个理由：
- 无万向锁——随便你怎么转，不会丢自由度
- 插值平滑——做姿态平滑时，四元数的球面线性插值（SLERP）效果极好
- 计算高效——比旋转矩阵少几个乘法，在嵌入式设备上优势明显
// 四元数表示绕单位向量(u_x, u_y, u_z)旋转θ角度
q = [cos(θ/2), u_x*sin(θ/2), u_y*sin(θ/2), u_z*sin(θ/2)]
// 用四元数旋转一个三维向量v
// 先把v写成纯四元数：v_q = [0, v_x, v_y, v_z]
// 然后：v'_q = q * v_q * q_conjugate我的习惯：在代码里，我一般用Eigen库或者Sophus库来处理四元数。手写四元数乘法容易出错，尤其是共轭和逆的关系，搞混了就全完了。用现成的库，省心又安全。
3.5 齐次坐标：把旋转和平移统一起来
好了，前面我们讲了旋转（用旋转矩阵或四元数）和平移（用向量）。但问题是，旋转和平移是两种不同的运算，能不能把它们统一起来？
答案就是——齐次坐标。
齐次坐标的核心思想很简单：给三维向量加一个维度，变成四维向量。
- 普通三维点：(x, y, z) → 齐次坐标：(x, y, z, 1)
- 普通三维向量：(x, y, z) → 齐次坐标：(x, y, z, 0)
为什么要区分点和向量？因为点可以平移，向量不能平移。你想想看，一个方向向量，平移之后还是它自己，对吧？
齐次变换矩阵T是一个4x4的矩阵：
T = [
[R, t],
[0, 1]
]
// 其中R是3x3旋转矩阵，t是3x1平移向量
// 用T变换一个点P：
P' = T * P这样做的好处太明显了：
- 一次矩阵乘法，同时完成旋转和平移
- 多个变换可以连乘：T_total = T3 * T2 * T1
- 逆变换也很简单：T_inv = [R^T, -R^T * t; 0, 1]
实战经验：在BEV感知中，我们经常需要把激光雷达坐标系下的点云变换到车辆坐标系，再变换到世界坐标系。这一连串的变换，用齐次矩阵连乘，一行代码就搞定了。我曾经见过有人用循环加if-else来手动做变换，代码写了上百行，还容易出错。用齐次矩阵，10行代码解决战斗。
3.6 知识体系总览
下面这张图，是我自己总结的坐标变换知识体系。你可以把它当作一个「地图」，随时回来查阅。
3.7 总结与建议
好了，这一章的内容就到这里。我们来捋一捋：
- 刚体变换是基础，旋转加平移
- 旋转矩阵最正统，但计算量稍大
- 欧拉角最直观，但小心万向锁
- 四元数最实用，推荐在代码中使用
- 齐次坐标把一切统一起来，是工程实现的首选
我的建议：刚开始学的时候，别急着背公式。先在纸上画一画坐标系，用手算几个简单的旋转例子。等理解了物理意义，代码自然就会写了。
嗯，坐标变换这东西，说难不难，说简单也不简单。关键是多练、多用。等你真正在项目里用它解决过问题，就再也不会忘了。