最小二乘问题详解11：基于李代数的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)\) 中的元素是反对称矩阵。但更方便的是，我们可以用一个三维向量 \(\boldsymbol{\phi} = [\phi_1, \phi_2, \phi_3]^\top \in \mathbb{R}^3\) 来唯一表示它：

\[\boldsymbol{\phi} \quad \longleftrightarrow \quad [\boldsymbol{\phi}]_\times = \begin{bmatrix}0 & -\phi_3 & \phi_2 \\\phi_3 & 0 & -\phi_1 \\-\phi_2 & \phi_1 & 0\end{bmatrix}\in \mathfrak{so}(3)\]
这个向量 \(\boldsymbol{\phi}\) 的方向代表旋转轴，模长 \(\|\boldsymbol{\phi}\|\) 代表旋转角度（弧度）。其实，\(\boldsymbol{\phi}\) 就是《最小二乘问题详解10：PnP问题求解》中使用的旋转向量。
2.3 指数映射

如何将李代数中的向量“提升”回李群中的旋转矩阵？通过指数映射（Exponential Map）：

\[\mathbf{R} = \exp([\boldsymbol{\phi}]_\times) = \mathrm{Exp}(\boldsymbol{\phi})\]
这个公式正是我们熟悉的 Rodrigues 旋转公式。它建立了从平坦的 \(\mathbb{R}^3\)（李代数坐标）到弯曲的 \(SO(3)\)（李群）的光滑映射。
反之，通过对数映射（Logarithmic Map），我们可以将任意旋转矩阵 \(\mathbf{R} \in SO(3)\) “拉回”到其对应的李代数向量 \(\boldsymbol{\phi} = \mathrm{Log}(\mathbf{R})\)。
2.4 重要性

李群-李代数框架的核心优势在于：我们可以在李代数 \(\mathfrak{so}(3)\)（一个向量空间）上定义优化变量和扰动，然后通过指数映射将其作用于李群 \(SO(3)\) 上，从而保证每一步更新都严格停留在合法的旋转集合内。
具体到优化中，我们不再更新旋转向量本身，而是优化一个微小的李代数增量 \(\delta \boldsymbol{\phi}\)，并通过左乘（或右乘）的方式更新当前旋转：

\[\mathbf{R}_{\text{new}} = \underbrace{\exp([\delta \boldsymbol{\phi}]_\times)}_{\text{微小旋转}} \cdot \mathbf{R}_{\text{current}}\]
这种方式天然满足群的乘法结构，避免了非法旋转的产生，并为雅可比矩阵的推导提供了自洽的理论基础。
2.5 特殊欧氏群 \(SE(3)\) 与李代数 \(\mathfrak{se}(3)\)

在 PnP 问题中，我们不仅需要估计相机的朝向（旋转），还需要估计其位置（平移）。因此，更方便的做法不是单独使用 \(SO(3)\) ——而是使用一个能同时描述刚体位姿（6 自由度）的数学对象。这个对象就是特殊欧氏群（Special Euclidean Group），记作 \(SE(3)\)。它由所有合法的三维刚体变换组成，通常用一个 \(4 \times 4\) 的齐次矩阵表示：

\[\mathbf{T} = \begin{bmatrix}\mathbf{R} & \mathbf{t} \\\mathbf{0}^\top & 1\end{bmatrix}\in SE(3),\quad \text{其中 } \mathbf{R} \in SO(3),\ \mathbf{t} \in \mathbb{R}^3.\]
\(SE(3)\) 同样是一个李群：它具备群结构（变换可复合、可逆），并且是一个光滑流形。但和 \(SO(3)\) 一样，它不是一个向量空间——你不能简单地将两个位姿矩阵相加。
为了在 \(SE(3)\) 上进行优化，我们需要它的李代数 \(\mathfrak{se}(3)\)。\(\mathfrak{se}(3)\) 是 \(SE(3)\) 在单位元（即 \(\mathbf{R} = \mathbf{I}, \mathbf{t} = \mathbf{0}\)）处的切空间，也是一个向量空间。\(\mathfrak{se}(3)\) 中的元素可以用一个 6 维向量 \(\boldsymbol{\xi} \in \mathbb{R}^6\) 来表示：

\[\boldsymbol{\xi} = \begin{bmatrix}\boldsymbol{\phi} \\ \boldsymbol{\rho}\end{bmatrix}\in \mathbb{R}^6,\quad \text{其中 } \boldsymbol{\phi} \in \mathbb{R}^3 \text{ 是旋转向量},\ \boldsymbol{\rho} \in \mathbb{R}^3 \text{ 是平移向量}.\]
对应的反对称形式（hat 算子）为：

\[[\boldsymbol{\xi}]_\times = \begin{bmatrix}[\boldsymbol{\phi}]_\times & \boldsymbol{\rho} \\\mathbf{0}^\top & 0\end{bmatrix}\in \mathfrak{se}(3)\]
通过指数映射，我们可以将李代数中的微小扰动 \(\boldsymbol{\xi}\) 映射回 \(SE(3)\) 中的一个刚体变换：

\[\mathbf{T} = \exp([\boldsymbol{\xi}]_\times) = \mathrm{Exp}(\boldsymbol{\xi}) \in SE(3)\]
这个映射有闭式解（可通过 Rodrigues 公式和积分推导），常用于从李代数构造变换矩阵。
2.6 左扰动模型与位姿更新

在优化中，我们不直接更新位姿 \(\mathbf{T}\)，而是优化一个微小的李代数增量 \(\delta \boldsymbol{\xi} = [\delta \boldsymbol{\phi}^\top, \delta \boldsymbol{\rho}^\top]^\top\)，并通过左乘的方式更新当前估计：

\[\mathbf{T}_{\text{new}} = \underbrace{\exp([\delta \boldsymbol{\xi}]_\times)}_{\text{微小刚体运动}} \cdot \mathbf{T}_{\text{current}}.\]
这种“左扰动”具有明确的几何意义：微小运动是相对于当前相机坐标系（即光心）定义的。这正是我们在《最小二乘问题详解10：PnP问题求解》中采用的扰动模型；更重要的是，对空间点 \(\mathbf{X}\)（齐次坐标 \(\tilde{\mathbf{X}} = [\mathbf{X}^\top, 1]^\top\)）的作用可线性近似为：

\[\delta \mathbf{P} = -[\mathbf{P}]_\times \, \delta \boldsymbol{\phi} + \delta \boldsymbol{\rho}\]
其中，\(\mathbf{P} = \mathbf{R} \mathbf{X} + \mathbf{t}\)。这与《最小二乘问题详解10：PnP问题求解》手动推导的雅可比矩阵完全一致。
因此，基于 \(SE(3)\) 李代数的优化，并非推翻前作，而是为其提供了一个自洽、严谨且可扩展的理论框架：

• 所有更新自动保持 \(\mathbf{R} \in SO(3)\)；
  
• 旋转和平移被统一处理；
  
• 扰动模型与成像几何天然对齐。
  

3. 求解

有了李群与李代数的基本工具，我们现在可以重新构建 PnP 问题的非线性最小二乘求解器。值得强调的是，我们的目标函数和残差形式与《最小二乘问题详解10：PnP问题求解》完全相同——区别仅在于如何对位姿参数进行扰动和更新。
3.1 问题建模回顾

给定 \(n\) 对 3D-2D 点对应 \(\{ \mathbf{X}_i \in \mathbb{R}^3, \mathbf{x}_i \in \mathbb{R}^2 \}\)，以及已知相机内参 \(\mathbf{K}\)，我们希望求解相机位姿 \((\mathbf{R}, \mathbf{t})\)，使得重投影误差最小：

\[\min_{\mathbf{R}, \mathbf{t}} \sum_{i=1}^n \left\| \mathbf{x}_i - \pi\left( \mathbf{K}, \mathbf{R} \mathbf{X}_i + \mathbf{t} \right) \right\|^2,\]
其中 \(\pi(\cdot)\) 为透视投影函数。
在基于李代数的优化框架中，我们不将全局参数 \(\boldsymbol{\phi}\) 和 \(\mathbf{t}\) 直接作为优化变量，而是采用局部增量更新策略：在每次迭代中，维护当前位姿 \(\mathbf{T} = [\mathbf{R}, \mathbf{t}] \in SE(3)\)，并求解一个 6 维李代数增量 \(\delta \boldsymbol{\xi} = [\delta \boldsymbol{\phi}^\top, \delta \boldsymbol{\rho}^\top]^\top\)，通过左乘指数映射更新位姿。这种策略既能保证旋转的合法性，又能自然地处理刚体运动的几何结构。
3.2 扰动模型与雅可比矩阵

在非线性最小二乘优化中，我们通过迭代求解一个局部线性化的子问题来更新参数。由于相机位姿属于李群 \(SE(3)\)，我们不能在欧氏空间中直接加减参数，而应在当前位姿的切空间（即李代数 \(\mathfrak{se}(3)\)）中引入微小扰动。
3.2.1 对增量求导

设当前位姿为 \(\mathbf{T} = [\mathbf{R}, \mathbf{t}]\)，对应的相机坐标系下 3D 点为：

\[\mathbf{P}_i = \mathbf{R} \mathbf{X}_i + \mathbf{t}\]
我们引入一个微小的 SE(3) 左扰动 \(\delta \boldsymbol{\xi} = [\delta \boldsymbol{\phi}^\top, \delta \boldsymbol{\rho}^\top]^\top \in \mathbb{R}^6\)，其中：

• \(\delta \boldsymbol{\phi} \in \mathbb{R}^3\) 表示绕当前相机光心的微小旋转；
  
• \(\delta \boldsymbol{\rho} \in \mathbb{R}^3\) 表示微小平移。
  

该扰动对应的刚体变换为 \(\exp([\delta \boldsymbol{\xi}]_\times) \in SE(3)\)，新位姿为：

\[\mathbf{T}' = \exp([\delta \boldsymbol{\xi}]_\times) \, \mathbf{T}\]
相应地，点 \(\mathbf{X}_i\) 在新相机坐标系中的位置为：

\[\tilde{\mathbf{P}}_i' = \exp([\delta \boldsymbol{\xi}]_\times) \, \tilde{\mathbf{P}}_i\tag{1}\]
其中，\(\tilde{\mathbf{P}}_i = \begin{bmatrix} \mathbf{P}_i \\ 1 \end{bmatrix}\) 。这样，重投影残差是 \(\delta \boldsymbol{\xi}\) 的函数。我们在 \(\delta \boldsymbol{\xi} = \mathbf{0}\) 处进行一阶泰勒展开：

\[\mathbf{r}_i(\delta \boldsymbol{\xi}) \approx \mathbf{r}_i(\mathbf{0}) + \underbrace{\frac{\partial \mathbf{r}_i}{\partial \delta \boldsymbol{\xi}}}_{\displaystyle \mathbf{J}_i} \delta \boldsymbol{\xi}.\]
因此，雅可比矩阵必须定义为残差对李代数增量 \(\delta \boldsymbol{\xi}\) 的偏导，这是李群优化的核心。
3.2.2 扰动模型的建立

利用指数映射的一阶近似 \(\exp([\delta \boldsymbol{\xi}]_\times) \approx \mathbf{I} + [\delta \boldsymbol{\xi}]_\times\)，代入到式(1)，并将反对称矩阵展开：

\[[\delta \boldsymbol{\xi}]_\times =\begin{bmatrix}[\delta \boldsymbol{\phi}]_\times & \delta \boldsymbol{\rho} \\\mathbf{0}^\top & 0\end{bmatrix}\]
可得：

\[\mathbf{P}_i' \approx \mathbf{P}_i + [\delta \boldsymbol{\phi}]_\times \mathbf{P}_i + \delta \boldsymbol{\rho}.\]
由叉积反对称性质 \([\delta \boldsymbol{\phi}]_\times \mathbf{P}_i = -[\mathbf{P}_i]_\times \delta \boldsymbol{\phi}\)，最终得到 SE(3) 左扰动模型：

\[\boxed{\mathbf{P}_i' \approx \mathbf{P}_i - [\mathbf{P}_i]_\times \, \delta \boldsymbol{\phi} + \delta \boldsymbol{\rho}}\tag{2}\]

扰动直接作用于 \(\mathbf{P}_i = \mathbf{R} \mathbf{X}_i + \mathbf{t}\)，即相对于光心的位置向量。这正是“绕光心旋转”的数学体现，也是《最小二乘问题详解10：PnP问题求解》中所采用的物理模型。

3.2.3 雅可比矩阵的链式推导

设相机内参为 \(\mathbf{K} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}\)，图像坐标为：

\[u = f_x \frac{P_x}{P_z} + c_x, \quad v = f_y \frac{P_y}{P_z} + c_y\]
其中 \(\mathbf{P}_i = [P_x, P_y, P_z]^\top\)。根据链式法则，雅可比矩阵为：

\[\mathbf{J}_i = \frac{\partial (u, v)}{\partial \delta \boldsymbol{\xi}}= \frac{\partial (u, v)}{\partial \mathbf{P}_i} \cdot \frac{\partial \mathbf{P}_i}{\partial \delta \boldsymbol{\xi}}\]
首先求 \(\displaystyle \frac{\partial (u, v)}{\partial \mathbf{P}_i}\) 。标量 \(u\) 和标量 \(v\) 分别对向量 \(\mathbf{P}_i\) 求导，得到相应的梯度：

\[\frac{\partial u}{\partial \mathbf{P}_i} = \left[ \frac{f_x}{P_z},\ 0,\ -\frac{f_x P_x}{P_z^2} \right], \quad\frac{\partial v}{\partial \mathbf{P}_i} = \left[ 0,\ \frac{f_y}{P_z},\ -\frac{f_y P_y}{P_z^2} \right]\]
组合可得：

\[\frac{\partial \pi}{\partial \mathbf{P}_i} =\begin{bmatrix}\displaystyle \frac{f_x}{P_z} & 0 & \displaystyle -\frac{f_x P_x}{P_z^2} \\0 & \displaystyle \frac{f_y}{P_z} & \displaystyle -\frac{f_y P_y}{P_z^2}\end{bmatrix}\in \mathbb{R}^{2 \times 3}\]
然后求 \(\displaystyle \frac{\partial \mathbf{P}_i}{\partial \delta \boldsymbol{\xi}}\) 。由扰动模型式(2)，\(\mathbf{P}_i\) 对增量 \(\delta \boldsymbol{\xi} = [\delta \boldsymbol{\phi}^\top, \delta \boldsymbol{\rho}^\top]^\top\) 的导数可通过逐分量计算：

• 对 \(\delta \boldsymbol{\phi}\) 的导数：
  由 \(\delta \mathbf{P}_i = -[\mathbf{P}_i]_\times \delta \boldsymbol{\phi}\)，可知：
  
  \[\frac{\partial \mathbf{P}_i}{\partial \delta \boldsymbol{\phi}} = -[\mathbf{P}_i]_\times =\begin{bmatrix}0 & p_z & -p_y \\-p_z & 0 & p_x \\p_y & -p_x & 0\end{bmatrix}\]
  
• 对 \(\delta \boldsymbol{\rho}\) 的导数：
  由 \(\delta \mathbf{P}_i = \delta \boldsymbol{\rho}\)，显然：
  
  \[\frac{\partial \mathbf{P}_i}{\partial \delta \boldsymbol{\rho}} = \mathbf{I}_{3\times3}\]
  

注意：我们约定 \(\delta \boldsymbol{\xi} = [\delta \boldsymbol{\phi}^\top, \delta \boldsymbol{\rho}^\top]^\top\)（旋转在前，平移在后），因此：

\[\boxed{\frac{\partial \mathbf{P}_i}{\partial \delta \boldsymbol{\xi}} =\begin{bmatrix}-[\mathbf{P}_i]_\times & \mathbf{I}_{3\times3}\end{bmatrix}\in \mathbb{R}^{3 \times 6}}\tag{3}\]
最后，计算完整雅可比矩阵。将上述两部分相乘，即得：

\[\mathbf{J}_i = \begin{bmatrix}\displaystyle \frac{f_x}{P_z} & 0 & \displaystyle -\frac{f_x P_x}{P_z^2} \\0 & \displaystyle \frac{f_y}{P_z} & \displaystyle -\frac{f_y P_y}{P_z^2}\end{bmatrix}\cdot\begin{bmatrix}-[\mathbf{P}_i]_\times & \mathbf{I}\end{bmatrix}\]
显式展开后为：

\[\mathbf{J}_i = \left[\begin{array}{ccc|ccc}-\dfrac{f_x P_x P_y}{P_z^2} &f_x \left(1 + \dfrac{P_x^2}{P_z^2}\right) &-\dfrac{f_x P_y}{P_z} &\dfrac{f_x}{P_z} &0 &-\dfrac{f_x P_x}{P_z^2} \\\dfrac{f_y P_y^2}{P_z^2} &-\dfrac{f_y P_x P_y}{P_z^2} &\dfrac{f_y P_x}{P_z} &0 &\dfrac{f_y}{P_z} &-\dfrac{f_y P_y}{P_z^2}\end{array}\right]\]
此结果与《最小二乘问题详解10：PnP问题求解》中推导的雅可比矩阵完全一致，验证了 SE(3) 李代数框架工程实现的等价性。
当然可以！以下是重写后的 3.3 节“优化流程”，已全面适配 SE(3) 李代数“旋转在前、平移在后”的标准顺序，并修正了原稿中不一致的更新表达式（如错误地使用 \(\delta \boldsymbol{\rho}\) 更新旋转），同时强化了李群更新的几何意义与工程实现细节。
3.3 优化流程

尽管最终的雅可比矩阵形式完全一致，但参数的更新机制存在本质区别：我们不再在欧氏空间中直接加减旋转向量和平移向量，而是通过李代数增量驱动李群上的左乘更新，从而严格保持位姿的合法性。
完整的迭代优化流程如下：

• 初始化
  使用 EPnP、UPnP 或 RANSAC-PnP 等方法获得初始位姿 \((\mathbf{R}_0, \mathbf{t}_0)\)，并构造对应的齐次变换矩阵：
  
  \[\mathbf{T}_0 = \begin{bmatrix}\mathbf{R}_0 & \mathbf{t}_0 \\\mathbf{0}^\top & 1\end{bmatrix}\in SE(3)\]
  
• 迭代直到收敛（例如残差变化小于阈值或达到最大迭代次数）：
  a. 前向投影：对每个 3D 点 \(\mathbf{X}_i\)，计算其在当前相机坐标系下的位置
  
  \[\mathbf{P}_i = \mathbf{R} \mathbf{X}_i + \mathbf{t}\]
  b. 构建残差：计算重投影误差
  
  \[\mathbf{r}_i = \mathbf{x}_i - \pi\big(\mathbf{K} \mathbf{P}_i\big) \in \mathbb{R}^2\]
  c. 计算雅可比：根据第 3.2.3 节，为每个点计算 \(2 \times 6\) 的雅可比矩阵 \(\mathbf{J}_i\)，其中前 3 列对应旋转扰动 \(\delta \boldsymbol{\phi}\)，后 3 列对应平移扰动 \(\delta \boldsymbol{\rho}\)。
  d. 求解增量：将所有残差和雅可比堆叠为全局向量 \(\mathbf{r}\) 和矩阵 \(\mathbf{J}\)，并采用 Levenberg-Marquardt (LM) 等方法求解：
  
  \[\big(\mathbf{J}^\top \mathbf{J} + \lambda \mathbf{I}\big) \, \delta \boldsymbol{\xi} = \mathbf{J}^\top \mathbf{r}\]
  其中 \(\delta \boldsymbol{\xi} = [\delta \boldsymbol{\phi}^\top, \delta \boldsymbol{\rho}^\top]^\top \in \mathbb{R}^6\) 是待求的李代数增量。
  e. 执行李群更新（关键步骤）：
  利用指数映射将增量作用于当前位姿，采用左扰动模型进行更新：
  
  \[\mathbf{T} \leftarrow \exp\big([\delta \boldsymbol{\xi}]_\times\big) \cdot \mathbf{T} = \begin{bmatrix}\exp([\delta \boldsymbol{\phi}]_\times) & \delta \boldsymbol{\rho} \\\mathbf{0}^\top & 1\end{bmatrix}\begin{bmatrix}\mathbf{R} & \mathbf{t} \\\mathbf{0}^\top & 1\end{bmatrix}.\]
  展开后等价于：
  
  \[athbf{R} \leftarrow \exp([\delta \boldsymbol{\phi}]_\times) \, \mathbf{R}, \quad\mathbf{t} \leftarrow \exp([\delta \boldsymbol{\phi}]_\times) \, \mathbf{t} + \delta \boldsymbol{\rho}.\]
  
• 输出结果
  返回最终的旋转矩阵 \(\mathbf{R}\) 和平移向量 \(\mathbf{t}\) 作为优化后的相机位姿。
  

由于无需存储或更新 \(\boldsymbol{\phi}\)。位姿始终以 \((\mathbf{R}, \mathbf{t})\) 形式保存，仅在每次迭代中通过李代数增量 \(\delta \boldsymbol{\xi}\) 进行左乘更新。这避免了反复进行 \(\mathrm{Log}/\mathrm{Exp}\) 转换，提高了数值稳定性与效率。
4. 实例

在前文的理论推导中，我们详细阐述了如何基于 \(SE(3)\) 李代数框架构建 PnP 问题的非线性最小二乘求解器。然而，从理论到实践并非易事：手动实现 Levenberg-Marquardt 优化器、精确计算雅可比矩阵、并处理李群上的复杂更新逻辑，不仅代码量庞大，而且极易引入数值错误。为了将精力聚焦于核心思想的验证而非底层工程细节，这里直接 Ceres Solver 库来实现本案例。
4.1 代码实现与理论对照

具体代码实现如下：
[code]#include #include #include #include #include #include #include constexpr double PI = 3.14159265358979323846;inline Eigen::Matrix3d Skew(const Eigen::Vector3d& v) {  Eigen::Matrix3d m;  m