3、刚体变换与李群李代数：旋转矩阵、平移向量、齐次坐标、SE(3)与so(3)基础
大家好，我是你们的老朋友。今天我们来聊聊外参标定里最核心的数学基础——刚体变换与李群李代数。
说实话，我刚入行那会儿，看到这些术语就头大。旋转矩阵、平移向量、齐次坐标……感觉像在看天书。但后来我发现，这些东西说白了就是描述「一个物体怎么动」的数学工具。你想想看，外参标定要解决的不就是「相机坐标系和世界坐标系之间怎么互相转换」吗？
好，咱们一步步来。
3.1 旋转矩阵：描述「转」的数学
先问个问题：一个物体在三维空间里转动，怎么用数学描述？
最简单的办法就是旋转矩阵。它是一个 3×3 的矩阵，记作 R。它的核心性质是：R 的转置等于它的逆，而且行列式为 +1。这意味着旋转不会改变物体的形状和大小，只是换个方向。
旋转矩阵的性质：
- R^T = R^{-1}（正交性）
- det(R) = 1（右手系）
- 保持向量长度和夹角不变
我在项目中遇到过一个问题：两个相机之间的旋转矩阵算出来行列式是 -1。嗯，这通常意味着坐标系是左手系，或者计算过程中有符号错误。遇到这种情况，我一般会检查一下叉乘的顺序，或者看看是不是用了不同的坐标系约定。
3.2 平移向量：描述「移」的数学
旋转搞定了，那平移呢？平移更简单，就是一个三维向量 t = [t_x, t_y, t_z]^T。它表示物体沿着 x、y、z 轴分别移动了多少。
但这里有个坑：平移向量和旋转矩阵组合使用时，顺序很重要。先旋转再平移，和先平移再旋转，结果完全不同。我刚开始做标定时就犯过这个错，结果算出来的外参完全不对。
我的习惯：在代码里，我通常先做旋转，再做平移。这样物理意义更直观——先把物体转到目标方向，再挪到目标位置。
3.3 齐次坐标：把旋转和平移统一起来
旋转矩阵和平移向量分开用，其实挺麻烦的。每次变换都要先算旋转，再加平移。能不能把它们合并成一个操作？
能。这就是齐次坐标的妙处。
齐次坐标把一个三维点 [x, y, z]^T 扩展成四维 [x, y, z, 1]^T。然后，旋转和平移就可以合并成一个 4×4 的变换矩阵：
| R t |
| 0 1 |这个矩阵就是刚体变换矩阵，属于 SE(3) 群。用这个矩阵，一次矩阵乘法就能完成旋转+平移，非常方便。
说白了，齐次坐标就是给三维空间加了一个「缩放因子」，让平移也能用矩阵乘法表示。你想想看，如果没有齐次坐标，平移就得单独写一行代码，多麻烦。
3.4 SE(3) 与 so(3)：李群与李代数
好，接下来是重头戏——李群和李代数。别被名字吓到，其实它们解决的是一个很实际的问题：怎么对旋转和变换求导。
你想想看，在优化外参的时候，我们需要不断调整旋转矩阵和平移向量，让重投影误差最小。但旋转矩阵有正交约束，不能随便加减。你直接加一个小量，结果可能就不再是旋转矩阵了。
李群和李代数就是来解决这个问题的。
- SE(3)：特殊欧氏群，就是所有刚体变换矩阵的集合。它是一个李群。
- so(3)：旋转矩阵对应的李代数，本质上是三维向量的反对称矩阵。它没有约束，可以自由加减。
它们之间的关系是：李代数通过指数映射变成李群，李群通过对数映射变成李代数。
核心公式：
- 指数映射：exp(so(3)) → SO(3)
- 对数映射：log(SO(3)) → so(3)
- 对于 SE(3)：exp(se(3)) → SE(3)
我记得第一次用李代数做优化时，感觉就像打开了新世界的大门。以前用欧拉角优化，动不动就遇到万向锁，用四元数又得小心单位化。而李代数直接在切空间上优化，既没有约束，又不会出现奇异性。
避坑指南：我曾经在项目中直接用旋转矩阵做优化，结果迭代几次后矩阵就不再正交了。后来改用李代数表示旋转，问题迎刃而解。所以我的建议是：凡是涉及优化的地方，一律用李代数。
3.5 知识体系总览
为了让你更直观地理解这些概念之间的关系，我画了一张图：
这张图把整个知识体系串起来了。从最上层的刚体变换，到中间的旋转矩阵和平移向量，再到齐次坐标表示的 SE(3) 变换矩阵，最后到底层的李群与李代数。每一步都是前一步的抽象和推广。
3.6 实际应用中的选择
在实际的外参标定中，我一般这样选择：
| 场景 | 推荐表示方式 | 原因 |
|---|---|---|
| 标定初始化 | 旋转矩阵 + 平移向量 | 直观，容易理解 |
| 非线性优化 | 李代数 so(3) / se(3) | 无约束，求导方便 |
| 坐标变换计算 | 齐次坐标 SE(3) 矩阵 | 一次乘法完成变换 |
| 插值/滤波 | 四元数 + 平移向量 | 避免万向锁，插值平滑 |
嗯，这里要注意：没有一种表示方式是万能的。我个人的习惯是，在代码里同时保留多种表示，根据不同的操作选择最合适的。比如优化时用李代数，可视化时转成旋转矩阵，插值时用四元数。
一个小技巧：如果你用的是 C++，推荐用 Eigen 库的 Isometry3d 类型，它内部用齐次坐标存储，但提供了对旋转矩阵、平移向量、四元数的无缝转换接口。Python 的话，scipy.spatial.transform 里的 Rotation 类也很好用。
好了，这一章的内容就到这里。刚体变换和李群李代数是外参标定的数学基石，理解了它们，后面的标定算法就会顺畅很多。
可可精品资料库