第三章：刚体变换与李群李代数

好，我们进入第三章。这一章，说白了就是外参标定的数学地基。

你想想看，外参标定到底在干什么？

其实就是求一个变换——把激光雷达的点云，或者IMU的加速度，映射到相机坐标系下。这个变换，在数学上就叫刚体变换。

我刚开始做标定的时候，总觉得这东西不就是个矩阵乘法吗？后来踩了坑才发现，旋转矩阵的约束、李代数的求导，这些细节才是真正决定标定精度的地方。

3.1 旋转矩阵：不只是3x3的数字

旋转矩阵，记作 R，是一个 3x3 的矩阵。但它不是随便填9个数就能用的。

它有两条铁律：

• 正交性：R^TR = I，也就是矩阵的逆等于它的转置。
• 行列式为+1：这保证了它是纯旋转，没有镜像。

我在项目中遇到过一个问题：有人直接用欧拉角转成旋转矩阵，结果标定出来的外参，点云和图像总是对不上。查了半天，发现是欧拉角的万向锁问题，导致旋转矩阵不连续。

3.2 平移向量：简单但容易翻车

平移向量 t 就是一个 3x1 的向量，表示坐标原点的偏移。

嗯，这里要注意：平移向量是在目标坐标系下表示的。什么意思？

比如你要把激光雷达的点变换到相机坐标系，平移向量表示的是：激光雷达原点在相机坐标系下的位置。

我曾经在标定一个多线激光雷达和相机的组合时，把平移向量的方向搞反了。结果点云全部投影到了图像的背面。排查了整整两天，最后发现就是 t 的符号问题。

3.3 齐次坐标：为什么需要多写一个1？

你可能会问：旋转矩阵加平移向量，不就是 [R|t] 吗？为什么还要搞个齐次坐标？

原因很简单：为了把旋转和平移统一成一个矩阵乘法。

如果不加齐次坐标，变换一个点需要两步：

p' = R * p + t

但如果你把点 p 写成齐次形式 [x, y, z, 1]^T，变换矩阵写成 4x4 的：

T = [R  t]
    [0  1]

那么一次矩阵乘法就搞定了：

p' = T * p

我个人习惯在代码里始终用齐次坐标。因为当你需要连续变换多次时——比如从激光雷达到IMU，再到相机——齐次坐标可以让你把多个变换矩阵直接乘起来，非常清爽。

3.4 SE(3) 与 so(3)：李群李代数的直觉

好，接下来这部分，可能是本章最抽象的内容。但我尽量用直觉讲清楚。

SE(3) 是特殊欧氏群，它包含了所有刚体变换（旋转+平移）。

so(3) 是旋转矩阵对应的李代数，它本质上是三维的旋转向量。

为什么要引入李代数？

因为旋转矩阵 R 有正交约束，你不能直接对它做加法优化。比如你标定时要优化外参，如果用 R 作为优化变量，每次迭代后 R 就不再是旋转矩阵了——你需要重新投影到 SO(3) 上，很麻烦。

而李代数 so(3) 是一个向量空间，你可以直接对它做加法、求导、优化。优化完再映射回 SE(3) 就行。

我记得第一次用 g2o 做图优化标定时，就是因为没搞懂李代数的求导，导致雅可比矩阵写错了，优化一直不收敛。后来换成 Sophus 库的 SE3 类型，它自动处理了李代数到李群的映射，问题就解决了。

3.4.1 指数映射与对数映射

从 so(3) 到 SO(3) 的映射，叫指数映射。反过来叫对数映射。

公式我就不写了，你记住一句话：李代数是对旋转的局部线性化。

说白了，就是你在优化时，每次迭代都在当前旋转附近做一个小的扰动，这个扰动用李代数表示。然后通过指数映射，把扰动加到旋转矩阵上。

3.5 本章知识体系

下面这张图，是我自己总结的刚体变换与李群李代数的知识结构。你可以把它当作一个思维导图来看。

3.6 本章小结

这一章的内容，是外参标定的数学基础。你不需要把李群李代数的所有公式都背下来，但一定要理解：

• 旋转矩阵有约束，不能直接优化
• 齐次坐标让变换变得简洁
• 李代数是对旋转的局部线性化，方便求导和优化

下一章，我们会把这些数学工具用到实际的相机模型和畸变校正中。到时候你会发现，这些抽象的概念，其实都是用来解决具体问题的。