如何将基于李代数的PnP优化为？

1. 引言 在上一篇文章《最小二乘问题详解10：PnP问题求解》中，我们基于旋转向量（axis-angle）参数化，构建了一个完整的非线性最小二乘框架来求解 Perspective-n-Point（PnP）问题。通过手动推导重投影函数的雅可比矩阵，并结合 Levenberg-Marquardt 算法，我们成功实现了从 3D-2D 点对应关系中恢复相机位姿的功能。该方法直观、易于实现，且在初始值接近真值时表现良好。 然而，这种将旋转简单视为三维向量并在欧氏空间中直接加减的优化策略，在处理旋转这一具有特殊几何结构的对象时，存在若干内在局限： 首先，旋转的“加法”并不像普通向量那样工作。我们在优化中执行 \(\boldsymbol{r} \leftarrow \boldsymbol{r} + \Delta \boldsymbol{r}\)，但两个旋转向量相加的结果，通常不等于它们所代表的两次连续旋转的合成效果。这种不一致性在小扰动下影响不大，但当初始估计偏差较大或优化步长较大时，会导致更新后的旋转不再满足正交性约束（即不再是合法的旋转矩阵），从而使优化过程偏离正确的几何路径。 其次，旋转向量在零附近的行为不够“平滑”。虽然 Rodrigues 公式能将旋转向量转换为旋转矩阵，但在角度趋近于零时，其导数计算容易出现数值不稳定。我们在自动微分实现中不得不引入泰勒展开来“修补”梯度，这本质上是因为我们强行用一个平坦的坐标系去描述一个弯曲的空间——就像试图用一张平面地图精确表示整个地球表面一样，总会存在扭曲。 更重要的是，我们缺乏一种统一且自洽的方式来定义“微小旋转”。在《最小二乘问题详解10：PnP问题求解》中，我们采用“左扰动”来推导雅可比矩阵，这是一种有效的工程技巧，但它更像是一个经验规则：为什么是左乘而不是右乘？这种扰动方式能否自然地推广到包含平移的完整位姿（即 6 自由度运动）？如果没有一个坚实的理论基础，这些问题很难得到清晰回答。 2. 李群与李代数 为从根本上解决上述问题，我们需要一个能够尊重旋转内在几何结构的数学框架——它既能让我们像操作普通向量一样进行微分和优化，又能保证所有操作始终落在合法的旋转集合之内。这个强大的现代几何工具就是李群（Lie Group）与李代数（Lie Algebra）。如果你已经阅读过笔者此前的系列文章《详解SLAM中的李群和李代数（上）》与《详解SLAM中的李群和李代数（中）》，那么对以下概念应不陌生；如果不了解或者感觉有点模糊，强烈建议先回顾这两篇文章以建立扎实基础。 2.1 特殊正交群 \(SO(3)\) 所有合法的三维旋转矩阵构成一个集合： \[SO(3) = \left\{ \mathbf{R} \in \mathbb{R}^{3\times3} \mid \mathbf{R}^\top \mathbf{R} = \mathbf{I},\ \det(\mathbf{R}) = 1 \right\} \] 这个集合不仅是一个群（满足封闭性、结合律、单位元、逆元），更是一个光滑流形——它没有尖角或断裂，局部看起来像欧氏空间。我们称 \(SO(3)\) 为特殊正交群，它是描述三维旋转的李群。 关键在于：\(SO(3)\) 不是向量空间。你不能随意将两个旋转矩阵相加，结果很可能不再属于 \(SO(3)\)。这正是我们在《最小二乘问题详解10：PnP问题求解》中直接对旋转向量加减所引发问题的根源。 2.2 李代数 \(\mathfrak{so}(3)\) 既然不能在 \(SO(3)\) 上直接做加法，那能否在其“附近”找一个平坦的空间来操作？答案是肯定的。在单位元（即单位矩阵 \(\mathbf{I}\)）处，\(SO(3)\) 的切空间构成了一个向量空间，称为李代数，记作 \(\mathfrak{so}(3)\)。 \(\mathfrak{so}(3)\) 中的元素是反对称矩阵。