3. 刚体变换与李群李代数
各位同学，欢迎来到第三章。
这一章，我们聊聊自动驾驶感知里最基础、也最容易翻车的东西——刚体变换。说白了，就是车怎么动、传感器怎么转、点云怎么对齐。
我刚开始做标定时，觉得不就是个旋转平移嘛，有啥难的？结果第一次把激光雷达和相机对齐，点云投影到图像上全是重影，找了三天bug，最后发现是欧拉角顺序搞反了。嗯，从那以后，我再也不敢小看这章内容了。
核心一句话：刚体变换 = 旋转 + 平移。但怎么表示旋转，决定了你的代码是优雅还是灾难。
3.1 旋转矩阵：最直观，但最啰嗦
旋转矩阵 R 是一个 3x3 的正交矩阵，满足 R^T R = I，det(R) = 1。它描述了一个坐标系相对于另一个坐标系的朝向。
举个例子：
# 绕Z轴旋转30度的旋转矩阵
import numpy as np
theta = np.radians(30)
R_z = np.array([
[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0],
[0, 0, 1]
])
print(R_z)旋转矩阵的好处是直观，可以直接乘向量。但坏处也很明显——9个参数，只有3个自由度，冗余严重。而且你想想看，如果标定过程中累积了数值误差，R 可能不再是正交矩阵，点云就会变形。
我的习惯：在代码里，我一般不会直接用旋转矩阵做优化变量，而是用四元数或轴角，最后再转成矩阵去乘点云。这样数值稳定性好很多。
3.2 欧拉角：人类友好，但小心万向锁
欧拉角用三个角度（roll, pitch, yaw）描述旋转。在自动驾驶里，通常的顺序是 Z-Y-X（yaw-pitch-roll）。
但这里有个大坑——万向锁。当 pitch 接近 ±90° 时，roll 和 yaw 会耦合，丢失一个自由度。我在项目中遇到过，用欧拉角做外参优化，结果车在坡道上标定出来的结果直接炸了，就是因为 pitch 接近 90°。
避坑指南：我曾经在实车标定中，用欧拉角做插值，结果车辆姿态在俯仰角接近90度时，滚转和偏航突然跳变。后来全部改成四元数插值，问题解决。所以，千万别用欧拉角做插值或优化，只适合给人看。
3.3 四元数：真香，但需要适应
四元数 q = w + xi + yj + zk，满足 ||q|| = 1。它没有万向锁，插值平滑，数值稳定。
我个人的经验是：所有涉及旋转的优化，都用四元数。比如标定外参时，我优化的是四元数的虚部（实部通过归一化约束），而不是直接优化旋转矩阵。
# 四元数转旋转矩阵（标准实现）
def quat_to_rot(q):
w, x, y, z = q
R = np.array([
[1 - 2*y*y - 2*z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y],
[2*x*y + 2*w*z, 1 - 2*x*x - 2*z*z, 2*y*z - 2*w*x],
[2*x*z - 2*w*y, 2*y*z + 2*w*x, 1 - 2*x*x - 2*y*y]
])
return R小技巧：四元数乘法不满足交换律，顺序很重要。我一般用 Hamilton 约定（左乘），和 ROS、Eigen 保持一致。别用 JPL 约定，否则你会被各种库的兼容性搞疯。
3.4 SE(3) 与 so(3)：李群李代数的核心
好了，前面都是热身。现在进入正题——李群与李代数。
为什么需要这个？因为我们要做优化。标定外参本质上是一个优化问题：找到最优的 R 和 t，使得重投影误差最小。但旋转矩阵有正交约束，直接优化很难。李代数就是来解决这个问题的。
- SE(3)：特殊欧氏群，表示刚体变换。一个 4x4 的齐次矩阵，左上角是 R，右上角是 t，最后一行是 [0,0,0,1]。
- so(3)：李代数，对应旋转。是一个 3x3 的反对称矩阵，只有3个参数。
- se(3)：李代数，对应刚体变换。是一个 4x4 的矩阵，包含旋转和平移的微小增量。
说白了，李代数就是旋转/变换的「导数空间」。我们在优化时，在 so(3) 或 se(3) 上做增量更新，然后通过指数映射回到 SE(3)。这样就没有约束了，随便优化。
# 指数映射：so(3) -> SO(3) (Rodrigues公式)
def so3_exp(phi):
theta = np.linalg.norm(phi)
if theta < 1e-10:
return np.eye(3)
a = phi / theta
K = np.array([
[0, -a[2], a[1]],
[a[2], 0, -a[0]],
[-a[1], a[0], 0]
])
R = np.eye(3) + np.sin(theta) * K + (1 - np.cos(theta)) * (K @ K)
return R核心思想：优化时在 so(3) 上做加法（无约束），然后映射回 SO(3)（有约束）。这样既保证了旋转矩阵的正交性，又可以用常规的梯度下降法。
3.5 知识体系总览
下面这张图，是我自己总结的刚体变换与李群李代数的知识结构。你把它记在脑子里，后面所有标定算法都不会迷路。
3.6 实战中的选择建议
| 场景 | 推荐表示 | 原因 |
|---|---|---|
| 给人看/调试 | 欧拉角 | 直观，一眼能看出车头朝哪 |
| 点云变换 | 旋转矩阵 | 直接乘，效率高 |
| 优化/标定 | 四元数 + 李代数 | 无约束优化，数值稳定 |
| 插值/滤波 | 四元数 | 球面线性插值，平滑无跳变 |
最后提醒一句：不管用什么表示，一定要统一坐标系约定。我见过太多人因为「左乘右乘搞反」、「Z轴向上还是Y轴向上」这种问题，浪费好几天。建议团队内部统一用 ROS 的坐标系规范（右手系，Z向上，X向前）。
好了，这一章就到这里。刚体变换是标定的地基，地基不稳，上层算法再花哨也是白搭。下一章我们会把这些数学工具真正用到外参标定的实战中去。
本章要点回顾：
- 旋转矩阵：直观但冗余，适合做变换，不适合做优化
- 欧拉角：人类友好，但万向锁是硬伤，别用来插值和优化
- 四元数：真香，标定和插值的首选
- 李群李代数：优化利器，在 so(3) 上做增量，映射回 SO(3)
- 坐标系约定一定要统一，否则全是坑
可可精品资料库