3. 数学基础回顾（下）：李群与李代数（SO3, SE3），扰动模型与求导，这在优化中有什么用？
好，咱们接着聊数学基础。上一节讲了三维空间刚体运动的基本表示，这一节要深入一点，聊聊李群和李代数。
说实话，我刚入行那会儿，看到「李群」「李代数」这几个字就头大。心想我一个搞工程的，干嘛要学这么纯的数学？后来在项目里被优化问题折磨得死去活来，才明白这东西有多重要。
你想想看，SLAM 的核心就是状态估计。我们要估计机器人的位姿，还要估计地图点的位置。这本质上是个优化问题。而优化嘛，就得求导。但位姿这东西，它不在普通的欧式空间里，它在一个流形上。在流形上怎么求导？这就是李群和李代数要解决的问题。
3.1 为什么需要李群与李代数？
我们先看一个简单的问题。假设你有一个旋转矩阵 R，它属于 SO(3)。你想对它求导，比如求 R 对时间的导数。但 R 本身有约束：R^T R = I，且 det(R) = 1。你直接对矩阵里的 9 个元素求导，求出来的结果很可能不满足这个约束。这就麻烦了。
我早期做手眼标定的时候就踩过这个坑。直接用矩阵元素做优化，结果迭代出来的矩阵根本不是旋转矩阵，还得再投影回去，精度和效率都很差。
李群和李代数提供了一种优雅的方式：
- 李群（SO(3), SE(3)）是连续的群，它描述了旋转和变换本身。
- 李代数（so(3), se(3)）是李群在单位元处的切空间。说白了，它是一个向量空间，没有那些复杂的约束。
这样一来，我们可以在李代数上做优化（因为它是向量空间，可以随便加减求导），然后通过指数映射回到李群上。这就绕开了约束问题。
核心思想： 优化在李代数上进行，结果映射回李群。
3.2 SO(3) 与 so(3)
咱们先看旋转。SO(3) 是三维旋转群，它的李代数是 so(3)。
so(3) 里的元素是三维向量，记作 φ = (φ1, φ2, φ3)^T。这个向量对应一个反对称矩阵：
φ^ = [ 0 -φ3 φ2 ]
[ φ3 0 -φ1 ]
[-φ2 φ1 0 ]指数映射把 so(3) 映射到 SO(3)：
R = exp(φ^)这个公式的物理意义是什么？φ 的方向是旋转轴，它的模长是旋转角度。这就是罗德里格斯公式。
反过来，对数映射把 SO(3) 映射回 so(3)：
φ^ = ln(R)嗯，这里要注意，对数映射不是唯一的，因为旋转角度可以加上 2π 的整数倍。但在实际应用中，我们通常取模长在 [0, π] 范围内的那个解。
3.3 SE(3) 与 se(3)
再看变换。SE(3) 是三维欧式变换群，它的李代数是 se(3)。
se(3) 里的元素是六维向量，记作 ξ = (ρ, φ)^T。其中 ρ 是平移部分，φ 是旋转部分。
se(3) 对应的矩阵形式是：
ξ^ = [ φ^ ρ ]
[ 0^T 0 ]指数映射把 se(3) 映射到 SE(3)：
T = exp(ξ^) = [ R t ]
[ 0^T 1 ]这里的 R 和 t 就是变换矩阵的旋转和平移部分。
个人经验： 在实际代码中，我们很少直接调用 exp 和 ln 的完整矩阵运算。Eigen、Sophus、g2o、Ceres 这些库都封装好了。你只需要知道它们背后的含义就行。
3.4 扰动模型与求导
好，重头戏来了。在优化中，我们经常需要求一个函数关于位姿的导数。比如，给定一个空间点 p，经过变换 T 后得到 p' = T p。我们想求 p' 对 T 的导数。
有两种求导方式：
- 直接求导： 对 T 矩阵本身求导。但 T 有约束，求导结果不直观。
- 扰动模型： 给 T 左乘或右乘一个微小的扰动，然后求导。这个扰动是在李代数上的。
我个人习惯用扰动模型，因为它更简洁，而且避免了复杂的约束处理。
以 SO(3) 为例。假设我们有一个旋转 R，点 p 旋转后得到 p' = R p。我们想求 p' 对 R 的导数。
给 R 左乘一个微小旋转 ΔR = exp(φ^)，其中 φ 是一个小量。那么：
∂(R p) / ∂φ = - (R p)^这个结果非常简洁。它告诉我们，旋转后的点对旋转扰动的导数，就是该点的反对称矩阵。
对于 SE(3)，扰动模型类似。给 T 左乘一个微小变换 ΔT = exp(ξ^)，其中 ξ = (δρ, δφ)^T。那么：
∂(T p) / ∂ξ = [ I - (R p + t)^ ]
[ 0^T 0^T ]这个矩阵就是雅可比矩阵，在优化中会频繁用到。
避坑指南： 我曾经在推导 BA（Bundle Adjustment）的雅可比时，搞混了左扰动和右扰动。左扰动和右扰动的结果差一个符号。一定要根据你的优化库的约定来。比如 g2o 和 Ceres 通常用左扰动。
3.5 在优化中有什么用？
说了这么多，到底有什么用？我举几个实际例子。
1. 点云配准（ICP）
ICP 的核心是迭代最近点。每次迭代，我们要估计一个变换 T，使得源点云和目标点云的对齐误差最小。这个误差函数对 T 的导数，就是用李代数扰动模型来算的。
2. 图优化（Graph SLAM）
在 SLAM 中，我们构建一个图，节点是位姿，边是观测约束。优化这个图，就是调整所有节点的位姿，使得观测误差最小。每个节点的位姿更新，都是在李代数上完成的。
3. 视觉惯性里程计（VIO）
VIO 里要优化 IMU 预积分和视觉重投影误差。IMU 预积分本身就是在李代数上做的，因为旋转的积分需要指数映射。
说白了，只要你的优化变量涉及旋转或变换，你就离不开李群和李代数。它是连接连续优化和离散状态的桥梁。
总结一下：
- 李群（SO(3), SE(3)）描述旋转和变换本身。
- 李代数（so(3), se(3)）是向量空间，方便求导和优化。
- 指数/对数映射连接两者。
- 扰动模型是求导的标准方法，简洁且无约束。
嗯，这一节的内容确实有点抽象。但相信我，等你真正开始写优化代码的时候，就会觉得这些推导是顺理成章的。下一节我们会用代码把这些公式实现一遍，到时候你就知道它们有多好用了。
公众号:蓝海资料掘金营，微信deep3321