最小二乘问题详解10：PnP问题求解

1. 引言

通过本系列前几篇文章（最小二乘问题详解：目录）的学习，我们对最小二乘问题有了较为系统的认识：它是一种广泛应用于科学与工程领域的参数估计与优化方法。在计算机视觉中，最小二乘思想贯穿于许多核心算法，尤其在运动恢复结构（Structure from Motion, SFM）这一经典框架中体现得尤为明显。

SFM 的完整流程通常包含五个关键子问题：

1. 相机标定（Camera Calibration）
2. PnP 问题（Perspective-n-Point）
3. 三角化（Triangulation）
4. 对极几何估计（Fundamental / Essential Matrix Estimation）
5. 束平差（Bundle Adjustment, BA）

其中，PnP 问题是最基础、也最直观的一个环节。它负责在已知部分 3D 结构和对应 2D 观测的前提下，求解相机的位姿。虽然形式简单，但其求解质量直接影响后续三角化与全局优化的稳定性，是连接 2D 图像与 3D 场景的重要桥梁。

2. 问题模型

2.1 原理概述

PnP（Perspective-n-Point）问题的目标是：给定 \(n\) 个已知的世界坐标系下的 3D 点 \(\mathbf{X}_i = [X_i, Y_i, Z_i]^\top \in \mathbb{R}^3\) 及其在图像中对应的 2D 像素坐标 \(\mathbf{x}_i = [u_i, v_i]^\top \in \mathbb{R}^2\)（\(i = 1, \dots, n\)），求解相机相对于世界坐标系的位姿------即相机的位置和朝向。

这个问题建立在针孔相机成像模型 之上。假设相机内参 \(\mathbf{K}\) 已通过标定获得（例如 \(\mathbf{K} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}\)），那么一个空间点 \(\mathbf{X}_i\) 经过刚体变换（旋转 + 平移）后，再经透视投影，最终落在图像平面上。这一过程可写为：

\[s_i \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = \mathbf{K} \begin{bmatrix} \mathbf{R} \mid \mathbf{t} \end{bmatrix} \begin{bmatrix} \mathbf{X}_i \\ 1 \end{bmatrix} \tag{1} \]

我们逐项解释这个公式：

• \(\mathbf{R} \in \mathbb{R}^{3 \times 3}\) 是一个旋转矩阵，描述相机坐标系相对于世界坐标系的朝向；
• \(\mathbf{t} \in \mathbb{R}^3\) 是一个平移向量，描述世界坐标系原点在相机坐标系下的位置（注意：这是从世界到相机的变换，所以点先旋转再平移：\(\mathbf{X}_i^c = \mathbf{R} \mathbf{X}_i + \mathbf{t}\)）；
• \([\mathbf{R} \mid \mathbf{t}] \in \mathbb{R}^{3 \times 4}\) 表示将 \(\mathbf{R}\) 和 \(\mathbf{t}\) 拼接成一个 \(3 \times 4\) 的矩阵，用于对齐次坐标 \([\mathbf{X}_i^\top, 1]^\top\) 进行线性变换；
• \(s_i\) 是一个未知的正实数尺度因子 。它之所以出现，是因为等式左边是齐次坐标 （homogeneous coordinates）。在齐次表示中，\([u, v, 1]^\top\) 和 \([s u, s v, s]^\top\) 代表同一个像素点。因此，\(s_i\) 不是我们要估计的参数，也不是已知量，而是一个由深度决定的中间变量 （实际上 \(s_i = Z_i^c\)，即该点在相机坐标系下的 Z 坐标）。我们在优化时并不直接求 \(s_i\)，而是通过"重投影"来消除它。

为了进行数值优化，我们需要将 (1) 式转换为显式的像素坐标表达式。设 \(\mathbf{X}_i^c = [X_i^c, Y_i^c, Z_i^c]^\top = \mathbf{R} \mathbf{X}_i + \mathbf{t}\)，则理想投影点为：

\[\begin{aligned} u_i^{\text{proj}} &= \frac{f_x X_i^c}{Z_i^c} + c_x \\ v_i^{\text{proj}} &= \frac{f_y Y_i^c}{Z_i^c} + c_y \\ \end{aligned} \tag{2} \]

2.2 参数化

相机位姿包含 6 个自由度（6-DOF）：3 个用于描述位置（平移），3 个用于描述朝向（旋转）。平移部分可直接用三维向量

\[\mathbf{t} = [t_x,\, t_y,\, t_z]^\top \in \mathbb{R}^3 \]

表示，形式简单且无约束。

难点在于如何用 3 个独立参数 来表示旋转。常见的选择包括欧拉角、四元数、旋转矩阵或旋转向量：

• 欧拉角 （如 yaw-pitch-roll）虽然直观，但在俯仰角（pitch）接近 ±90° 时会发生万向锁（Gimbal Lock），导致两个旋转轴重合、自由度丢失。此时雅可比矩阵奇异，非线性优化极易失败。
• 旋转矩阵 \(\mathbf{R} \in \mathbb{R}^{3 \times 3}\) 虽然直接，但包含 9 个元素，却只有 3 个自由度，存在大量冗余和正交性约束（\(\mathbf{R}^\top \mathbf{R} = \mathbf{I}, \det(\mathbf{R}) = 1\)），不适合作为无约束优化变量。
• 单位四元数 虽无奇异性，但需维持单位模长约束（\(\|\mathbf{q}\| = 1\)），在最小二乘优化中需额外处理归一化或引入约束，增加实现复杂度。

因此，我们采用 旋转向量 （Rotation Vector）作为旋转的参数化方式。旋转向量是一个三维向量 \(\boldsymbol{r} = [r_x,\, r_y,\, r_z]^\top \in \mathbb{R}^3\)，其方向表示旋转轴 ，模长 \(\theta = \|\boldsymbol{r}\|\) 表示绕该轴的旋转角度 （单位：弧度）。例如，若物体绕单位向量 \(\mathbf{n}\) 旋转 \(\theta\) 弧度，则旋转向量为 \(\boldsymbol{r} = \theta \mathbf{n}\)。

给定旋转向量 \(\boldsymbol{r}\)，可通过 Rodrigues 公式 唯一地计算出对应的旋转矩阵 \(\mathbf{R}(\boldsymbol{r})\)：

\[\mathbf{R}(\boldsymbol{r}) = \begin{cases} \displaystyle \cos\theta \cdot \mathbf{I} + (1 - \cos\theta) \cdot \mathbf{n}\mathbf{n}^\top + \sin\theta \cdot [\mathbf{n}]_\times, & \theta \neq 0 \\ \mathbf{I}, & \theta = 0 \end{cases} \]

其中 \(\theta = \|\boldsymbol{r}\|\)，\(\mathbf{n} = \boldsymbol{r}/\theta\)（当 \(\theta > 0\)），\([\mathbf{n}]_\times\) 是叉积反对称矩阵：

\[[\mathbf{n}]_\times = \begin{bmatrix} 0 & -n_z & n_y \\ n_z & 0 & -n_x \\ -n_y & n_x & 0 \end{bmatrix}. \]

这种表示法仅有 3 个无约束实数参数，无奇异性，且与 OpenCV 等主流视觉库的接口一致，非常适合用于非线性最小二乘优化。

综上，我们将 PnP 问题的全部待估参数组织为一个 6 维向量：

\[\boldsymbol{\theta} = \begin{bmatrix} \boldsymbol{r} \\ \mathbf{t} \end{bmatrix}= \begin{bmatrix} r_x \\ r_y \\ r_z \\ t_x \\ t_y \\ t_z \end{bmatrix} \in \mathbb{R}^6. \]

后续的最小二乘求解将围绕这个参数向量 \(\boldsymbol{\theta}\) 展开。

3. 求解

回顾《最小二乘问题详解4：非线性最小二乘》中非线性最小二乘问题的定义：给定观测数据 \(\{(\mathbf{x}_i, \mathbf{y}i)\}{i=1}^n\)，我们希望找到参数 \(\boldsymbol{\theta}\)，使得非线性模型 \(f(\mathbf{x}_i; \boldsymbol{\theta})\) 尽可能接近观测值 \(\mathbf{y}_i\)。目标函数为：

\[\min_{\boldsymbol{\theta}} S(\boldsymbol{\theta}) = \sum_{i=1}^n \| \mathbf{r}_i(\boldsymbol{\theta}) \|^2 \]

其中：

\[\mathbf{r}_i(\boldsymbol{\theta}) = \mathbf{y}_i - f(\mathbf{x}_i; \boldsymbol{\theta}) \]

在 PnP 问题中，这一框架可直接对应如下：

• 输入变量 \(\mathbf{x}_i\)：世界坐标系下的 3D 点 \(\mathbf{X}_i = [X_i, Y_i, Z_i]^\top\)；

• 观测输出 \(\mathbf{y}_i\)：图像中的 2D 像素坐标 \(\mathbf{x}_i = [u_i, v_i]^\top\)；

• 待估参数 \(\boldsymbol{\theta}\)：6 维相机位姿向量 \(\boldsymbol{\theta} = [\boldsymbol{r}^\top, \mathbf{t}^\top]^\top \in \mathbb{R}^6\)；

• 非线性模型 \(f(\mathbf{X}_i; \boldsymbol{\theta})\)：即 重投影函数 ，它将 3D 点通过当前位姿变换并投影到图像平面，输出预测的像素坐标：

  \[f(\mathbf{X}_i; \boldsymbol{\theta}) = \begin{bmatrix} u_i^{\text{proj}} \\ v_i^{\text{proj}} \end{bmatrix} = \begin{bmatrix} \displaystyle \frac{f_x (\mathbf{R}(\boldsymbol{r}) \mathbf{X}_i + \mathbf{t})_x}{(\mathbf{R}(\boldsymbol{r}) \mathbf{X}_i + \mathbf{t})_z} + c_x \\ \displaystyle \frac{f_y (\mathbf{R}(\boldsymbol{r}) \mathbf{X}_i + \mathbf{t})_y}{(\mathbf{R}(\boldsymbol{r}) \mathbf{X}_i + \mathbf{t})_z} + c_y \end{bmatrix} \]

因此，第 \(i\) 个点的残差向量为：

\[\mathbf{r}_i(\boldsymbol{\theta}) = \begin{bmatrix} u_i - u_i^{\text{proj}} \\ v_i - v_i^{\text{proj}} \end{bmatrix} \in \mathbb{R}^2 \]

由于模型 \(f\) 包含旋转（通过 Rodrigues 公式）和透视除法（除以 \(Z_i^c\)），它是一个高度非线性 的函数，无法直接求解闭式解。我们必须借助迭代优化方法（如 Gauss-Newton 或 Levenberg-Marquardt），而这些方法的核心是计算雅可比矩阵。

根据一般定义，整体雅可比矩阵 \(J(\boldsymbol{\theta}) \in \mathbb{R}^{2n \times 6}\) 由每个点的雅可比块垂直堆叠而成：

\[J(\boldsymbol{\theta}) = \begin{bmatrix} J_1(\boldsymbol{\theta}) \\ J_2(\boldsymbol{\theta}) \\ \vdots \\ J_n(\boldsymbol{\theta}) \end{bmatrix} \]

其中：

\[J_i(\boldsymbol{\theta}) = \frac{\partial f(\mathbf{X}_i; \boldsymbol{\theta})}{\partial \boldsymbol{\theta}^\top} \in \mathbb{R}^{2 \times 6}. \]

也可以对残差 \(\mathbf{r}_i = \mathbf{x}_i - f(\mathbf{X}_i; \boldsymbol{\theta})\) 求偏导：\(J_i(\boldsymbol{\theta}) = - \frac{\partial f(\mathbf{X}_i; \boldsymbol{\theta})}{\partial \boldsymbol{\theta}^\top}\)，两者求偏导的结果方向相反。这里延续《最小二乘问题详解4：非线性最小二乘》中的做法，使用对模型函数 \(f(\mathbf{X}_i; \boldsymbol{\theta})\) 求偏导。

将重投影函数代入，有：

\[J_i = \frac{\partial}{\partial \boldsymbol{\theta}^\top} \begin{bmatrix} u^{\text{proj}} \\ v^{\text{proj}} \end{bmatrix}= \left[ \frac{\partial (u^{\text{proj}}, v^{\text{proj}})}{\partial \boldsymbol{r}^\top} \quad \frac{\partial (u^{\text{proj}}, v^{\text{proj}})}{\partial \mathbf{t}^\top} \right] \in \mathbb{R}^{2 \times 6} \]

3.1 平移项求导

我们首先计算重投影函数对平移向量 \(\mathbf{t} = [t_x, t_y, t_z]^\top\) 的偏导数。这部分相对简单，因为平移与 3D 点的变换是线性关系。

设当前位姿下，第 \(i\) 个 3D 点在相机坐标系中的坐标为：

\[\mathbf{P} = \begin{bmatrix} P_x \\ P_y \\ P_z \end{bmatrix} = \mathbf{R}(\boldsymbol{r}) \mathbf{X}_i + \mathbf{t} \]

根据针孔相机模型（式 (2)），其重投影到图像平面的像素坐标为：

\[u^{\text{proj}} = \frac{f_x P_x}{P_z} + c_x, \quad v^{\text{proj}} = \frac{f_y P_y}{P_z} + c_y. \]

由于 \(\mathbf{P}\) 直接依赖于 \(\mathbf{t}\)，且 \(\frac{\partial \mathbf{P}}{\partial \mathbf{t}} = \mathbf{I}_{3\times3}\)（即 \(P_x\) 仅对 \(t_x\) 求导为 1，其余为 0，以此类推），我们可以直接对 \(u^{\text{proj}}\) 和 \(v^{\text{proj}}\) 关于 \(t_x, t_y, t_z\) 求偏导。

关于向量对向量求导，参见5.1节补充说明。

将 \(u^{\text{proj}} = f_x P_x / P_z + c_x\) 视为关于 \(P_x\) 和 \(P_z\) 的函数，再利用链式法则（注意 \(P_x\) 和 \(P_z\) 都是 \(\mathbf{t}\) 的函数）：

• \(\displaystyle \frac{\partial u^{\text{proj}}}{\partial t_x} = \frac{\partial u^{\text{proj}}}{\partial P_x} \cdot \frac{\partial P_x}{\partial t_x} + \frac{\partial u^{\text{proj}}}{\partial P_z} \cdot \frac{\partial P_z}{\partial t_x} = \left( \frac{f_x}{P_z} \right) \cdot 1 + \left( -\frac{f_x P_x}{P_z^2} \right) \cdot 0 = \frac{f_x}{P_z}\)
• \(\displaystyle \frac{\partial u^{\text{proj}}}{\partial t_y} = \frac{\partial u^{\text{proj}}}{\partial P_x} \cdot 0 + \frac{\partial u^{\text{proj}}}{\partial P_z} \cdot 0 = 0\)
• \(\displaystyle \frac{\partial u^{\text{proj}}}{\partial t_z} = \frac{\partial u^{\text{proj}}}{\partial P_x} \cdot 0 + \frac{\partial u^{\text{proj}}}{\partial P_z} \cdot 1 = -\frac{f_x P_x}{P_z^2}\)

同理，对于 \(v^{\text{proj}} = f_y P_y / P_z + c_y\)：

• \(\displaystyle \frac{\partial v^{\text{proj}}}{\partial t_x} = 0\)
• \(\displaystyle \frac{\partial v^{\text{proj}}}{\partial t_y} = \frac{f_y}{P_z}\)
• \(\displaystyle \frac{\partial v^{\text{proj}}}{\partial t_z} = -\frac{f_y P_y}{P_z^2}\)

将上述结果按行排列（第一行为 \(u^{\text{proj}}\) 对 \(\mathbf{t}\) 的梯度，第二行为 \(v^{\text{proj}}\) 的梯度），得到重投影函数对平移部分的雅可比矩阵：

\[\boxed{ \frac{\partial (u^{\text{proj}}, v^{\text{proj}})}{\partial \mathbf{t}^\top}= \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} } \tag{A} \]

该矩阵清晰地反映了：

• 平移在 \(x/y\) 方向的改变会直接 影响图像上的 \(u/v\) 坐标（正比于焦距，反比于深度 \(P_z\)）；
• 平移在 \(z\) 方向的改变会通过改变深度 间接影响所有像素坐标，且影响程度与当前点的 \(x/y\) 位置成正比。

3.2 旋转项求导

接下来计算重投影函数对旋转向量 \(\boldsymbol{r} = [r_x, r_y, r_z]^\top\)的偏导数：

\[\frac{\partial (u^{\text{proj}}, v^{\text{proj}})}{\partial \boldsymbol{r}^\top} \]

但问题在于：旋转向量和旋转矩阵之间是非线性关系 （通过 Rodrigues 公式），直接对 \(\boldsymbol{r}\) 求导会非常复杂（涉及三角函数、分段定义、除零风险等）。所以在工程上，一般使用 扰动法 来代替直接求导。所谓扰动法，就是通过分析扰动响应（即"如果我稍微转一下，图像点怎么动"）来得到雅可比矩阵，也就是局部线性化旋转对重投影的影响。

关于扰动法，参见5.2节补充说明。

假设在当前旋转 \(\mathbf{R}\) 上施加一个微小的旋转向量扰动 \(\delta \boldsymbol{r} = [\delta r_x, \delta r_y, \delta r_z]^\top\)，则新的旋转近似为：

\[\mathbf{R}' \approx (\mathbf{I} + [\delta \boldsymbol{r}]_\times) \mathbf{R} \]

其中 \([\delta \boldsymbol{r}]_\times\) 是叉积反对称矩阵。于是，扰动后的 3D 点变为：

\[\mathbf{P}' = \mathbf{R}' \mathbf{X} + \mathbf{t} \approx (\mathbf{I} + [\delta \boldsymbol{r}]\times) (\mathbf{R} \mathbf{X}) + \mathbf{t} = \mathbf{P} + [\delta \boldsymbol{r}]\times \mathbf{P} \]

利用叉积性质：\([\delta \boldsymbol{r}]\times \mathbf{P} = -[\mathbf{P}]\times \delta \boldsymbol{r}\)，所以：

\[\delta \mathbf{P} = \mathbf{P}' - \mathbf{P} \approx -[\mathbf{P}]_\times \delta \boldsymbol{r} \]

这说明：旋转向量的微小变化 \(\delta \boldsymbol{r}\) 引起的 3D 点变化为 \(\delta \mathbf{P} = -[\mathbf{P}]_\times \delta \boldsymbol{r}\)。

然后，对重投影函数求全微分。对于一个可微函数 \(u^{\text{proj}}(\boldsymbol{r})\)，其在 \(\boldsymbol{r}\) 处的微小变化满足：

\[\delta u^{\text{proj}} = \frac{\partial u^{\text{proj}}}{\partial r_x} \delta r_x + \frac{\partial u^{\text{proj}}}{\partial r_y} \delta r_y + \frac{\partial u^{\text{proj}}}{\partial r_z} \delta r_z = \frac{\partial u^{\text{proj}}}{\partial \boldsymbol{r}^\top} \, \delta \boldsymbol{r}. \]

所以 \(\delta r_x, \delta r_y, \delta r_z\) 的系数就是偏导数 \(\frac{\partial u^{\text{proj}}}{\partial r_x}, \frac{\partial u^{\text{proj}}}{\partial r_y}, \frac{\partial u^{\text{proj}}}{\partial r_z}\)。

另一方面，由于 \(u^{\text{proj}} = f_x P_x / P_z + c_x\)，其全微分为：

\[\delta u^{\text{proj}} = \frac{\partial u^{\text{proj}}}{\partial \mathbf{P}} \cdot \delta \mathbf{P} = \left[ \frac{f_x}{P_z},\ 0,\ -\frac{f_x P_x}{P_z^2} \right] \cdot (-[\mathbf{P}]_\times \delta \boldsymbol{r}) \]

而 \([\mathbf{P}]_\times = \begin{bmatrix}0 & -P_z & P_y \\P_z & 0 & -P_x \\-P_y & P_x & 0\end{bmatrix}\)，所以：

\[-[\mathbf{P}]_\times \delta \boldsymbol{r} = \begin{bmatrix} P_z \delta r_y - P_y \delta r_z \\ -P_z \delta r_x + P_x \delta r_z \\ P_y \delta r_x - P_x \delta r_y \end{bmatrix} \]

代入得：

\[\delta u^{\text{proj}} = \frac{f_x}{P_z} (P_z \delta r_y - P_y \delta r_z) + \left(-\frac{f_x P_x}{P_z^2}\right) (P_y \delta r_x - P_x \delta r_y) \]

整理各项系数：

• \(\delta r_x\) 的系数：\(-\frac{f_x P_x P_y}{P_z^2}\)
• \(\delta r_y\) 的系数：\(f_x + \frac{f_x P_x^2}{P_z^2}\)
• \(\delta r_z\) 的系数：\(-\frac{f_x P_y}{P_z}\)

同理对 \(v^{\text{proj}} = f_y P_y / P_z + c_y\)，其梯度为 \([0, f_y/P_z, -f_y P_y / P_z^2]\)，可得：

• \(\delta r_x\) 的系数：\(\frac{f_y P_y^2}{P_z^2}\)
• \(\delta r_y\) 的系数：\(-\frac{f_y P_x P_y}{P_z^2}\)
• \(\delta r_z\) 的系数：\(\frac{f_y P_x}{P_z}\)

因此，对旋转向量的雅可比块为：

\[\boxed{ \frac{\partial (u^{\text{proj}}, v^{\text{proj}})}{\partial \boldsymbol{r}^\top}= \begin{bmatrix} \displaystyle -\frac{f_x P_x P_y}{P_z^2} & \displaystyle f_x \left(1 + \frac{P_x^2}{P_z^2}\right) & \displaystyle -\frac{f_x P_y}{P_z} \\ \displaystyle \frac{f_y P_y^2}{P_z^2} & \displaystyle -\frac{f_y P_x P_y}{P_z^2} & \displaystyle \frac{f_y P_x}{P_z} \end{bmatrix} } \tag{B} \]

3.3 完整的雅可比块

将 (A) 和 (B) 拼接，得到第 \(i\) 个点的完整 \(2 \times 6\) 雅可比矩阵：

\[\boxed{ 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] } \]

其中：

\[\mathbf{P} = [P_x, P_y, P_z]^\top = \mathbf{R}(\boldsymbol{r}) \mathbf{X}_i + \mathbf{t} \]

4. 实例

4.1 手写实现

为了更深入地理解 PnP 问题的数学本质与优化过程，笔者基于 Eigen 库从零实现了一个完整的求解器。该实现采用前文手动推导的解析雅可比矩阵，并结合 Levenberg-Marquardt（LM）算法 对重投影误差进行非线性最小二乘优化，从而求解相机的 6 自由度位姿。

#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <random>
#include <vector>

using namespace std;
using namespace Eigen;

using Vector6d = Matrix<double, 6, 1>;

// ================
// 工具函数：旋转向量 -> 旋转矩阵 (Rodrigues)
// ================
Matrix3d rodrigues(const Vector3d& rvec) {
  double theta = rvec.norm();
  if (theta < 1e-8) {
    return Matrix3d::Identity();
  }
  Vector3d n = rvec / theta;  // 单位旋转轴
  double c = cos(theta), s = sin(theta);
  Matrix3d n_skew;
  n_skew << 0, -n(2), n(1), n(2), 0, -n(0), -n(1), n(0), 0;
  return c * Matrix3d::Identity() + (1 - c) * n * n.transpose() + s * n_skew;
}

// ================
// 相机内参（固定）
// ================
struct CameraIntrinsics {
  double fx = 500.0, fy = 500.0;
  double cx = 320.0, cy = 240.0;
};

// ================
// 重投影函数：给定 3D 点 X, 位姿 [rvec, t], 返回 [u, v]
// ================
Vector2d project(const Vector3d& X, const Vector3d& rvec, const Vector3d& t,
                 const CameraIntrinsics& K) {
  Matrix3d R = rodrigues(rvec);
  Vector3d P = R * X + t;  // 相机坐标系下的点
  // 注意：若 P.z <= 0，投影无意义，但此处假设数据合理
  double u = K.fx * P(0) / P(2) + K.cx;
  double v = K.fy * P(1) / P(2) + K.cy;
  return Vector2d(u, v);
}

// ================
// 计算完整雅可比 J_i ∈ ℝ^{2×6}
// ================
Matrix<double, 2, 6> computeJacobianBlock(const Vector3d& X,
                                          const Vector3d& rvec,
                                          const Vector3d& t,
                                          const CameraIntrinsics& K) {
  Matrix3d R = rodrigues(rvec);
  Vector3d P = R * X + t;  // P = [Px, Py, Pz]^T

  double Px = P(0), Py = P(1), Pz = P(2);
  double fx = K.fx, fy = K.fy;

  if (fabs(Pz) < 1e-6) {
    // 避免除零，返回零雅可比（该点贡献为0）
    return Matrix<double, 2, 6>::Zero();
  }

  Matrix<double, 2, 6> J;

  // --- 旋转部分 (左3列) ---
  J(0, 0) = -fx * Px * Py / (Pz * Pz);       // ∂u/∂rx
  J(0, 1) = fx * (1 + Px * Px / (Pz * Pz));  // ∂u/∂ry
  J(0, 2) = -fx * Py / Pz;                   // ∂u/∂rz

  J(1, 0) = fy * Py * Py / (Pz * Pz);   // ∂v/∂rx
  J(1, 1) = -fy * Px * Py / (Pz * Pz);  // ∂v/∂ry
  J(1, 2) = fy * Px / Pz;               // ∂v/∂rz

  // --- 平移部分 (右3列) ---
  J(0, 3) = fx / Pz;               // ∂u/∂tx
  J(0, 4) = 0.0;                   // ∂u/∂ty
  J(0, 5) = -fx * Px / (Pz * Pz);  // ∂u/∂tz

  J(1, 3) = 0.0;                   // ∂v/∂tx
  J(1, 4) = fy / Pz;               // ∂v/∂ty
  J(1, 5) = -fy * Py / (Pz * Pz);  // ∂v/∂tz

  return J;
}

// ================
// 主函数
// ================
int main() {
  // ------------------------
  // 1. 设置真实相机位姿
  // ------------------------
  Vector3d true_rvec(0.1, -0.2, 0.3);  // 真实旋转向量
  Vector3d true_t(0.5, -0.3, 2.0);     // 真实平移
  CameraIntrinsics K;

  cout << "真实旋转向量: " << true_rvec.transpose() << endl;
  cout << "真实平移: " << true_t.transpose() << endl;

  // ------------------------
  // 2. 生成 3D-2D 对应点（带噪声）
  // ------------------------
  int N = 20;
  vector<Vector3d> Xs(N);
  vector<Vector2d> zs(N);

  // 固定随机种子以保证可复现性
  mt19937 gen(42);
  uniform_real_distribution<double> dist(-1.0, 1.0);
  normal_distribution<double> noise(0.0, 1.0);  // 像素噪声 ~1px

  for (int i = 0; i < N; ++i) {
    Xs[i] = Vector3d(dist(gen), dist(gen), dist(gen) + 3.0);
    Vector2d z_proj = project(Xs[i], true_rvec, true_t, K);
    zs[i] = z_proj + Vector2d(noise(gen), noise(gen));
  }

  // ------------------------
  // 3. 初始化估计值（从零开始）
  // ------------------------
  Vector6d theta;
  theta.head<3>().setZero();  // rvec = [0,0,0]
  theta.tail<3>().setZero();  // t = [0,0,0]

  cout << "\n初始估计位姿:" << endl;
  cout << "rvec: " << theta.head<3>().transpose() << endl;
  cout << "t:    " << theta.tail<3>().transpose() << endl;

  // ------------------------
  // 4. Levenberg-Marquardt 迭代（修复版）
  // ------------------------
  int max_iter = 50;
  double lambda = 1e-3;  // 初始阻尼因子
  double nu = 2.0;       // 阻尼调整因子
  double tol = 1e-8;

  for (int iter = 0; iter < max_iter; ++iter) {
    VectorXd residuals(2 * N);
    MatrixXd J(2 * N, 6);
    double total_error = 0.0;

    for (int i = 0; i < N; ++i) {
      Vector3d rvec_est = theta.head<3>();
      Vector3d t_est = theta.tail<3>();
      Vector2d z_pred = project(Xs[i], rvec_est, t_est, K);
      Vector2d res = zs[i] - z_pred;
      residuals.segment<2>(2ULL * i) = res;
      total_error += res.squaredNorm();

      Matrix<double, 2, 6> Ji = computeJacobianBlock(Xs[i], rvec_est, t_est, K);
      J.block<2, 6>(2ULL * i, 0) = Ji;
    }

    cout << "迭代 " << iter << ": 总重投影误差 = " << total_error << endl;

    Matrix<double, 6, 6> H = J.transpose() * J;
    Vector6d g = J.transpose() * residuals;

    if (g.norm() < tol) {
      cout << "梯度足够小，收敛！" << endl;
      break;
    }

    // 构建阻尼 Hessian
    Vector6d diag_H = H.diagonal();
    for (int i = 0; i < 6; ++i) {
      if (fabs(diag_H(i)) < 1e-12) diag_H(i) = 1e-12;
    }

    Matrix<double, 6, 6> H_damped = H;
    for (int i = 0; i < 6; ++i) {
      H_damped(i, i) += lambda * diag_H(i);
    }

    Vector6d delta = H_damped.ldlt().solve(g);
    if (delta.norm() < tol) {
      cout << "更新步长太小，收敛！" << endl;
      break;
    }

    // 试更新
    Vector6d theta_new = theta + delta;
    double new_error = 0.0;
    for (int i = 0; i < N; ++i) {
      Vector2d z_pred =
          project(Xs[i], theta_new.head<3>(), theta_new.tail<3>(), K);
      Vector2d res = zs[i] - z_pred;
      new_error += res.squaredNorm();
    }

    // 使用标准预测减少量计算 rho
    double actual_reduction = total_error - new_error;
    double predicted_reduction = -delta.dot(g);  // 标准一阶模型预测减少量

    double rho = 0.0;
    if (predicted_reduction > 1e-12) {
      rho = actual_reduction / predicted_reduction;
    } else if (actual_reduction > 0) {
      rho = 1.0;  // 实际减少了，接受
    }

    if (rho > 0) {
      theta = theta_new;
      lambda = lambda / std::max(nu, 1.0);
      lambda = std::max(lambda, 1e-15);  // 防止 lambda 过小
      cout << "  -> 接受更新 (λ=" << lambda << ", ρ=" << rho << ")" << endl;

      if (std::abs(actual_reduction) < tol * total_error) {
        cout << "误差下降不足，收敛！" << endl;
        break;
      }
    } else {
      lambda = lambda * nu;
      lambda = std::min(lambda, 1e10);  // 防止 lambda 无限增长
      cout << "  -> 拒绝更新 (λ=" << lambda << ", ρ=" << rho << ")" << endl;
    }
  }

  // ------------------------
  // 5. 输出结果
  // ------------------------
  cout << "\n--- 优化完成 ---" << endl;
  cout << "估计旋转向量: " << theta.head<3>().transpose() << endl;
  cout << "估计平移:      " << theta.tail<3>().transpose() << endl;

  return 0;
}

运行结果如下所示：

真实旋转向量:  0.1 -0.2  0.3
真实平移:  0.5 -0.3    2

初始估计位姿:
rvec: 0 0 0
t:    0 0 0
迭代 0: 总重投影误差 = 214620
  -> 接受更新 (λ=0.0005, ρ=1)
迭代 1: 总重投影误差 = 14795.1
  -> 接受更新 (λ=0.00025, ρ=1)
迭代 2: 总重投影误差 = 3217.53
  -> 接受更新 (λ=0.000125, ρ=1)
迭代 3: 总重投影误差 = 1097.51
  -> 接受更新 (λ=6.25e-05, ρ=1)
迭代 4: 总重投影误差 = 231.248
  -> 接受更新 (λ=3.125e-05, ρ=1)
迭代 5: 总重投影误差 = 122.308
  -> 接受更新 (λ=1.5625e-05, ρ=1)
迭代 6: 总重投影误差 = 83.4454
  -> 接受更新 (λ=7.8125e-06, ρ=1)
迭代 7: 总重投影误差 = 63.9914
  -> 接受更新 (λ=3.90625e-06, ρ=1)
迭代 8: 总重投影误差 = 53.2186
  -> 接受更新 (λ=1.95313e-06, ρ=1)
迭代 9: 总重投影误差 = 47.0382
  -> 接受更新 (λ=9.76563e-07, ρ=1)
迭代 10: 总重投影误差 = 43.4281
  -> 接受更新 (λ=4.88281e-07, ρ=1)
迭代 11: 总重投影误差 = 41.2913
  -> 接受更新 (λ=2.44141e-07, ρ=1)
迭代 12: 总重投影误差 = 40.0111
  -> 接受更新 (λ=1.2207e-07, ρ=1)
迭代 13: 总重投影误差 = 39.2346
  -> 接受更新 (λ=6.10352e-08, ρ=1)
迭代 14: 总重投影误差 = 38.7577
  -> 接受更新 (λ=3.05176e-08, ρ=1)
迭代 15: 总重投影误差 = 38.4606
  -> 接受更新 (λ=1.52588e-08, ρ=1)
迭代 16: 总重投影误差 = 38.2728
  -> 接受更新 (λ=7.62939e-09, ρ=1)
迭代 17: 总重投影误差 = 38.1521
  -> 接受更新 (λ=3.8147e-09, ρ=1)
迭代 18: 总重投影误差 = 38.0732
  -> 接受更新 (λ=1.90735e-09, ρ=1)
迭代 19: 总重投影误差 = 38.0207
  -> 接受更新 (λ=9.53674e-10, ρ=1)
迭代 20: 总重投影误差 = 37.9851
  -> 接受更新 (λ=4.76837e-10, ρ=1)
迭代 21: 总重投影误差 = 37.9606
  -> 接受更新 (λ=2.38419e-10, ρ=1)
迭代 22: 总重投影误差 = 37.9435
  -> 接受更新 (λ=1.19209e-10, ρ=1)
迭代 23: 总重投影误差 = 37.9313
  -> 接受更新 (λ=5.96046e-11, ρ=1)
迭代 24: 总重投影误差 = 37.9224
  -> 接受更新 (λ=2.98023e-11, ρ=1)
迭代 25: 总重投影误差 = 37.916
  -> 接受更新 (λ=1.49012e-11, ρ=1)
迭代 26: 总重投影误差 = 37.9113
  -> 接受更新 (λ=7.45058e-12, ρ=1)
迭代 27: 总重投影误差 = 37.9077
  -> 接受更新 (λ=3.72529e-12, ρ=1)
迭代 28: 总重投影误差 = 37.9051
  -> 接受更新 (λ=1.86265e-12, ρ=1)
迭代 29: 总重投影误差 = 37.9031
  -> 接受更新 (λ=9.31323e-13, ρ=1)
迭代 30: 总重投影误差 = 37.9016
  -> 接受更新 (λ=4.65661e-13, ρ=1)
迭代 31: 总重投影误差 = 37.9004
  -> 接受更新 (λ=2.32831e-13, ρ=1)
迭代 32: 总重投影误差 = 37.8996
  -> 接受更新 (λ=1.16415e-13, ρ=1)
迭代 33: 总重投影误差 = 37.8989
  -> 接受更新 (λ=5.82077e-14, ρ=1)
迭代 34: 总重投影误差 = 37.8984
  -> 接受更新 (λ=2.91038e-14, ρ=1)
迭代 35: 总重投影误差 = 37.898
  -> 接受更新 (λ=1.45519e-14, ρ=1)
迭代 36: 总重投影误差 = 37.8977
  -> 接受更新 (λ=7.27596e-15, ρ=1)
迭代 37: 总重投影误差 = 37.8975
  -> 接受更新 (λ=3.63798e-15, ρ=1)
迭代 38: 总重投影误差 = 37.8973
  -> 接受更新 (λ=1.81899e-15, ρ=1)
迭代 39: 总重投影误差 = 37.8972
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 40: 总重投影误差 = 37.8971
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 41: 总重投影误差 = 37.897
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 42: 总重投影误差 = 37.8969
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 43: 总重投影误差 = 37.8969
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 44: 总重投影误差 = 37.8969
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 45: 总重投影误差 = 37.8968
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 46: 总重投影误差 = 37.8968
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 47: 总重投影误差 = 37.8968
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 48: 总重投影误差 = 37.8968
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 49: 总重投影误差 = 37.8968
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 50: 总重投影误差 = 37.8968
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 51: 总重投影误差 = 37.8968
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 52: 总重投影误差 = 37.8967
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 53: 总重投影误差 = 37.8967
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 54: 总重投影误差 = 37.8967
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 55: 总重投影误差 = 37.8967
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 56: 总重投影误差 = 37.8967
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 57: 总重投影误差 = 37.8967
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 58: 总重投影误差 = 37.8967
  -> 接受更新 (λ=1e-15, ρ=1)
迭代 59: 总重投影误差 = 37.8967
更新步长太小，收敛！

--- 优化完成 ---
估计旋转向量: 0.0922212 -0.204636  0.296474
估计平移:       0.515189 -0.312825   1.99306

通过这一手写实现，不仅能验证理论推导的正确性，也有助于掌握视觉几何中参数化、投影模型与迭代优化之间的内在联系。

4.2 Ceres实现

在手写 Levenberg-Marquardt 求解器的基础上，我们进一步借助 Ceres Solver 这一工业级非线性优化库，实现一个更加简洁、鲁棒且高效的 PnP 求解器。Ceres 的核心优势在于其强大的自动微分（Automatic Differentiation） 能力------用户只需提供残差计算函数，雅可比矩阵将由框架自动、精确地生成，极大降低了实现复杂度并避免了手动推导的潜在错误。具体代码实现如下：

#include <ceres/autodiff_cost_function.h>
#include <ceres/ceres.h>

#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <random>
#include <vector>

using namespace std;
using namespace Eigen;

// ================
// 相机内参（固定）
// ================
struct CameraIntrinsics {
  double fx = 500.0, fy = 500.0;
  double cx = 320.0, cy = 240.0;
};

// ================
// 修复版 Rodrigues 公式（支持自动微分 + 小角度泰勒展开）
// ================
template <typename T>
Eigen::Matrix<T, 3, 3> rodrigues(const Eigen::Matrix<T, 3, 1>& rvec) {
  T theta2 = rvec.squaredNorm();
  const T kThreshold = T(1e-8);  // 阈值：θ² < 1e-8 → θ < 1e-4 rad ≈ 0.0057°
  Eigen::Matrix<T, 3, 3> R;

  if (theta2 < kThreshold) {
    // 二阶泰勒展开: R ≈ I + [r]_x + 0.5 * [r]_x^2
    T rx = rvec(0), ry = rvec(1), rz = rvec(2);
    T rx2 = rx * rx, ry2 = ry * ry, rz2 = rz * rz;

    R(0, 0) = T(1) - T(0.5) * (ry2 + rz2);
    R(0, 1) = -rz + T(0.5) * rx * ry;
    R(0, 2) = ry + T(0.5) * rx * rz;

    R(1, 0) = rz + T(0.5) * rx * ry;
    R(1, 1) = T(1) - T(0.5) * (rx2 + rz2);
    R(1, 2) = -rx + T(0.5) * ry * rz;

    R(2, 0) = -ry + T(0.5) * rx * rz;
    R(2, 1) = rx + T(0.5) * ry * rz;
    R(2, 2) = T(1) - T(0.5) * (rx2 + ry2);
  } else {
    T theta = ceres::sqrt(theta2);
    Eigen::Matrix<T, 3, 1> n = rvec / theta;
    T c = ceres::cos(theta);
    T s = ceres::sin(theta);

    Eigen::Matrix<T, 3, 3> n_skew;
    n_skew << T(0), -n(2), n(1), n(2), T(0), -n(0), -n(1), n(0), T(0);

    R = c * Eigen::Matrix<T, 3, 3>::Identity() +
        (T(1) - c) * (n * n.transpose()) + s * n_skew;
  }
  return R;
}

// ================
// PnP 残差项（每个 3D-2D 对应一个 2D 残差）
// ================
struct PnPCostFunctor {
  PnPCostFunctor(const Vector3d& X, const Vector2d& x_observed,
                 const CameraIntrinsics& K)
      : X_(X), x_obs_(x_observed), K_(K) {}

  template <typename T>
  bool operator()(const T* const rvec, const T* const t, T* residual) const {
    Eigen::Matrix<T, 3, 1> rvec_eigen(rvec[0], rvec[1], rvec[2]);
    Eigen::Matrix<T, 3, 1> t_eigen(t[0], t[1], t[2]);

    // 世界点 -> 相机坐标系
    Eigen::Matrix<T, 3, 3> R = rodrigues(rvec_eigen);
    Eigen::Matrix<T, 3, 1> P_cam = R * X_.template cast<T>() + t_eigen;

    // 数值保护：避免深度非正
    const T min_depth = T(1e-6);
    if (P_cam(2) < min_depth) {
      P_cam(2) = min_depth;
    }

    // 重投影
    T u_proj = T(K_.fx) * P_cam(0) / P_cam(2) + T(K_.cx);
    T v_proj = T(K_.fy) * P_cam(1) / P_cam(2) + T(K_.cy);

    // 残差 = 观测 - 预测
    residual[0] = T(x_obs_(0)) - u_proj;
    residual[1] = T(x_obs_(1)) - v_proj;

    return true;
  }

 private:
  const Vector3d X_;
  const Vector2d x_obs_;
  const CameraIntrinsics K_;
};

// ================
// 辅助函数：用于最后验证（非优化用）
// ================
Vector2d project_point(const Vector3d& X, const Vector3d& rvec,
                       const Vector3d& t, const CameraIntrinsics& K) {
  Matrix3d R;
  double theta = rvec.norm();
  if (theta < 1e-8) {
    R = Matrix3d::Identity();
  } else {
    Vector3d n = rvec / theta;
    double c = cos(theta), s = sin(theta);
    Matrix3d n_skew;
    n_skew << 0, -n(2), n(1), n(2), 0, -n(0), -n(1), n(0), 0;
    R = c * Matrix3d::Identity() + (1 - c) * n * n.transpose() + s * n_skew;
  }
  Vector3d P = R * X + t;
  double u = K.fx * P(0) / (P(2) > 1e-6 ? P(2) : 1e-6) + K.cx;
  double v = K.fy * P(1) / (P(2) > 1e-6 ? P(2) : 1e-6) + K.cy;
  return Vector2d(u, v);
}

int main() {
  // ------------------------
  // 1. 真实位姿
  // ------------------------
  Vector3d true_rvec(0.1, -0.2, 0.3);
  Vector3d true_t(0.5, -0.3, 2.0);
  CameraIntrinsics K;

  cout << "真实旋转向量: " << true_rvec.transpose() << endl;
  cout << "真实平移: " << true_t.transpose() << endl;

  // ------------------------
  // 2. 生成带噪声的 3D-2D 数据（固定种子）
  // ------------------------
  int N = 20;
  vector<Vector3d> Xs(N);
  vector<Vector2d> zs(N);

  mt19937 gen(42);  // 固定种子
  uniform_real_distribution<double> dist(-1.0, 1.0);
  normal_distribution<double> noise(0.0, 1.0);  // ~1px 噪声

  for (int i = 0; i < N; ++i) {
    Xs[i] = Vector3d(dist(gen), dist(gen), dist(gen) + 3.0);
    Vector2d z_proj = project_point(Xs[i], true_rvec, true_t, K);
    zs[i] = z_proj + Vector2d(noise(gen), noise(gen));
  }

  // ------------------------
  // 3. 初始化参数（从零开始）
  // ------------------------
  double rvec[3] = {0.0, 0.0, 0.0};
  double t[3] = {0.0, 0.0, 0.0};

  cout << "\n初始估计位姿:" << endl;
  cout << "rvec: [" << rvec[0] << ", " << rvec[1] << ", " << rvec[2] << "]"
       << endl;
  cout << "t:    [" << t[0] << ", " << t[1] << ", " << t[2] << "]" << endl;

  // ------------------------
  // 4. 构建 Ceres 问题
  // ------------------------
  ceres::Problem problem;
  for (int i = 0; i < N; ++i) {
    ceres::CostFunction* cost_function =
        new ceres::AutoDiffCostFunction<PnPCostFunctor, 2, 3, 3>(
            new PnPCostFunctor(Xs[i], zs[i], K));
    problem.AddResidualBlock(cost_function, nullptr, rvec, t);
  }

  // ------------------------
  // 5. 配置求解器
  // ------------------------
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = true;
  options.max_num_iterations = 50;
  options.function_tolerance = 1e-12;
  options.gradient_tolerance = 1e-10;
  options.parameter_tolerance = 1e-8;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  cout << summary.BriefReport() << "\n";

  // ------------------------
  // 6. 输出结果 & 验证
  // ------------------------
  cout << "--- 优化完成 ---" << endl;
  cout << "估计旋转向量: [" << rvec[0] << ", " << rvec[1] << ", " << rvec[2]
       << "]" << endl;
  cout << "估计平移:      [" << t[0] << ", " << t[1] << ", " << t[2] << "]"
       << endl;

  double total_error = 0.0;
  for (int i = 0; i < N; ++i) {
    Vector2d z_pred = project_point(Xs[i], Vector3d(rvec[0], rvec[1], rvec[2]),
                                    Vector3d(t[0], t[1], t[2]), K);
    Vector2d res = zs[i] - z_pred;
    total_error += res.squaredNorm();
  }
  cout << "最终重投影误差平方和: " << total_error << endl;

  return 0;
}

编译并运行该程序，得到如下输出：

真实旋转向量:  0.1 -0.2  0.3
真实平移:  0.5 -0.3    2

初始估计位姿:
rvec: [0, 0, 0]
t:    [0, 0, 0]
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  1.073098e+05    0.00e+00    6.60e+05   0.00e+00   0.00e+00  1.00e+04        0    6.39e-05    1.50e-04
   1  9.758582e+03    9.76e+04    1.07e+05   0.00e+00   9.14e-01  2.32e+04        1    8.38e-05    4.94e-04
   2  2.106817e+02    9.55e+03    7.53e+03   7.90e-01   9.81e-01  6.95e+04        1    3.77e-05    8.53e-04
   3  1.900002e+01    1.92e+02    1.42e+02   1.61e-01   1.00e+00  2.08e+05        1    5.02e-05    1.13e-03
   4  1.886750e+01    1.33e-01    5.59e-01   4.32e-03   1.00e+00  6.25e+05        1    5.77e-05    1.27e-03
   5  1.886750e+01    1.93e-06    9.74e-04   1.93e-05   1.00e+00  1.88e+06        1    3.55e-05    1.52e-03
   6  1.886750e+01    3.51e-11    3.96e-06   8.96e-08   1.01e+00  5.63e+06        1    4.23e-05    1.77e-03
Ceres Solver Report: Iterations: 7, Initial cost: 1.073098e+05, Final cost: 1.886750e+01, Termination: CONVERGENCE
--- 优化完成 ---
估计旋转向量: [0.093651, -0.20464, 0.296343]
估计平移:      [0.514623, -0.309082, 1.99296]
最终重投影误差平方和: 37.735

从结果可见：

• 优化仅需 7 次迭代 即收敛；
• 估计的旋转向量与平移向量与真值高度接近（旋转误差 < 0.007 弧度 ≈ 0.4°，平移误差 < 0.015 单位）；
• 最终重投影误差平方和为 37.735 ，对应 约 0.97 像素的 RMSE（平均重投影误差），与数据生成时添加的 1 像素高斯噪声水平一致，表明求解达到了理论最优精度。

值得注意的是，这里的实现对Rodrigues 公式的作了一些改进。在使用旋转向量（axis-angle）参数化旋转时，标准实现常在角度过小时采用硬性截断（例如 if (θ < ε) return Identity();）。然而，这种做法在自动微分框架下会带来严重问题：当初始旋转向量为零（如本例中 [0, 0, 0]）时，Ceres 的自动微分系统会认为旋转矩阵与输入参数无关------因为函数在该点附近被强制设为常量，导致梯度恒为零。其直接后果是：旋转参数完全无法更新，即便平移可能部分收敛，整体位姿估计仍严重失效。

为解决这一问题，我们在 Rodrigues 映射中引入了小角度下的二阶泰勒展开近似：

\[\mathbf{R}(\mathbf{r}) \approx \mathbf{I} + [\mathbf{r}]\times + \frac{1}{2} [\mathbf{r}]\times^2 \]

该表达式在 \(\|\mathbf{r}\| \to 0\) 时不仅保持高精度，更重要的是保留了非零的一阶导数信息。这使得 Ceres 能够正确计算残差对旋转向量的雅可比矩阵，从而在优化初期就有效驱动旋转参数更新。不过这不是本文论述的重点，有机会再进一步详述。

5. 补充

5.1 向量对向量求导

在3.1节中，出现了一个向量对向量求导式子\(\frac{\partial \mathbf{P}}{\partial \mathbf{t}} = \mathbf{I}_{3\times3}\)，其结果为什么是一个矩阵？

先把问题简单化，看看标量对向量求导。假设有一个函数 f(\\mathbf{t}) = t_x + 2t_y ，其中 \(\mathbf{t} = [t_x, t_y, t_z]^\top\)。那么它的梯度（即 标量对向量的导数）是：

\[\frac{\partial f}{\partial \mathbf{t}} = \begin{bmatrix} \frac{\partial f}{\partial t_x} \\ \frac{\partial f}{\partial t_y} \\ \frac{\partial f}{\partial t_z} \end{bmatrix}= \begin{bmatrix} 1 \\ 2 \\ 0 \end{bmatrix} \in \mathbb{R}^3. \]

也就是标量对向量求导的结果是一个列向量。

接下来，看向量对向量求导。现在考虑一个向量值函数：

\[\mathbf{P}(\mathbf{t}) = \begin{bmatrix} P_x \\ P_y \\ P_z \end{bmatrix}= \begin{bmatrix} R_{11}X + R_{12}Y + R_{13}Z + t_x \\ R_{21}X + R_{22}Y + R_{23}Z + t_y \\ R_{31}X + R_{32}Y + R_{33}Z + t_z \end{bmatrix} = \mathbf{R}\mathbf{X} + \mathbf{t} \]

由于是对\(\mathbf{t}\)求偏导，因此将 \(\mathbf{R}\) 和 \(\mathbf{X}\) 当作常数：

• \(P_x = (\text{常数}) + t_x\)
• \(P_y = (\text{常数}) + t_y\)
• \(P_z = (\text{常数}) + t_z\)

现在我们问：\(\mathbf{P}\) 的每个分量对 \(\mathbf{t}\) 的每个分量的偏导是多少 ？结果就是向量对向量的导数 ，其数学定义就是 雅可比矩阵：

\[\frac{\partial \mathbf{P}}{\partial \mathbf{t}^\top}= \begin{bmatrix} \displaystyle \frac{\partial P_x}{\partial t_x} & \displaystyle \frac{\partial P_x}{\partial t_y} & \displaystyle \frac{\partial P_x}{\partial t_z} \\ \displaystyle \frac{\partial P_y}{\partial t_x} & \displaystyle \frac{\partial P_y}{\partial t_y} & \displaystyle \frac{\partial P_y}{\partial t_z} \\ \displaystyle \frac{\partial P_z}{\partial t_x} & \displaystyle \frac{\partial P_z}{\partial t_y} & \displaystyle \frac{\partial P_z}{\partial t_z} \end{bmatrix} \in \mathbb{R}^{3 \times 3}. \]

代入上面的表达式：

• \(\frac{\partial P_x}{\partial t_x} = 1\)，而 \(\frac{\partial P_x}{\partial t_y} = 0\)，\(\frac{\partial P_x}{\partial t_z} = 0\)
• \(\frac{\partial P_y}{\partial t_y} = 1\)，其余为 0
• \(\frac{\partial P_z}{\partial t_z} = 1\)，其余为 0

所以：

\[\frac{\partial \mathbf{P}}{\partial \mathbf{t}^\top}= \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} = \mathbf{I}_{3\times3}. \]

"\(\frac{\partial \mathbf{P}}{\partial \mathbf{t}} = \mathbf{I}\)" 实际上是简写，严格来说应该是 \(\frac{\partial \mathbf{P}}{\partial \mathbf{t}^\top} = \mathbf{I}\)，即 输出向量对输入向量的转置求导 ，结果是一个矩阵------这个矩阵其实就是雅可比矩阵的标准定义。

将上面两种情况总结成下表：

情况	输入	输出	导数形式	名称
标量 ← 向量	\(\mathbf{t} \in \mathbb{R}^n\)	\(f \in \mathbb{R}\)	\(\frac{\partial f}{\partial \mathbf{t}} \in \mathbb{R}^n\)（列向量）	梯度
向量 ← 向量	\(\mathbf{t} \in \mathbb{R}^n\)	\(\mathbf{P} \in \mathbb{R}^m\)	\(\frac{\partial \mathbf{P}}{\partial \mathbf{t}^\top} \in \mathbb{R}^{m \times n}\)	雅可比矩阵

另外，再进一步地说，这种定义使得链式法则可以像矩阵乘法一样工作。例如，如果我们有：

\[u^{\text{proj}} = g(\mathbf{P}), \quad \mathbf{P} = h(\mathbf{t}), \]

那么：

\[\frac{\partial u^{\text{proj}}}{\partial \mathbf{t}^\top} = \frac{\partial u^{\text{proj}}}{\partial \mathbf{P}^\top} \cdot \frac{\partial \mathbf{P}}{\partial \mathbf{t}^\top}. \]

很显然：

• \(\frac{\partial u^{\text{proj}}}{\partial \mathbf{P}^\top}\) 是 \(1 \times 3\) 行向量（标量对向量导）
• \(\frac{\partial \mathbf{P}}{\partial \mathbf{t}^\top}\) 是 \(3 \times 3\) 矩阵
• 结果是 \(1 \times 3\) 行向量，即 \(u^{\text{proj}}\) 对 \(\mathbf{t}\) 的梯度

再回到 PnP 问题中的 \(\mathbf{P} = \mathbf{R}\mathbf{X} + \mathbf{t}\)，这是一个线性函数 ，所以它的雅可比矩阵就是单位阵 \(\mathbf{I}\)。所以这并不是什么神秘操作，而是多元微积分中标准的推广，目的是让高维函数的导数能像线性代数一样运算。

5.2 扰动法详解

在第 3.2 节中，我们通过扰动法推导了重投影函数对旋转向量的雅可比矩阵。这里作一些简要的说明。

为什么使用扰动法

旋转向量 \(\boldsymbol{r}\) 与旋转矩阵 \(\mathbf{R}\) 之间的映射由 Rodrigues 公式给出：

\[\mathbf{R}(\boldsymbol{r}) = \cos\theta\,\mathbf{I} + (1 - \cos\theta)\,\mathbf{n}\mathbf{n}^\top + \sin\theta\,[\mathbf{n}]_\times, \quad \theta = \|\boldsymbol{r}\|,\ \mathbf{n} = \boldsymbol{r}/\theta \]

该函数在 \(\theta = 0\) 处存在分段定义，且包含三角函数、向量归一化等非线性操作。若直接对 \(\boldsymbol{r}\) 求导：

• 表达式极其复杂；
• 在 \(\theta \to 0\) 时易出现数值不稳定；
• 实现容易出错，调试困难。

而非线性优化（如 Gauss-Newton）本质上只关心局部线性近似 ------即"参数微调时，目标函数如何变化"。这正是扰动法 的用武之地。总而言之，我们不需要知道全局的导数公式，只需知道在当前位姿附近，微小旋转如何影响观测值 。扰动法通过构造一个局部线性模型来回答这个问题，从而绕开复杂的解析求导。

左扰动 vs 右扰动

当我们说"对旋转加一个小扰动"，必须明确：这个扰动是在哪个参考系下施加的？

• 左扰动 （Left Perturbation）：扰动作用于世界坐标系。相当于先绕世界坐标轴微转，再应用原相机旋转。数学上：

  \[\mathbf{R}' \approx (\mathbf{I} + [\delta\boldsymbol{r}]_\times) \mathbf{R} \]

• 右扰动 （Right Perturbation）：扰动作用于相机坐标系。相当于先应用原旋转，再绕相机自身坐标轴微转：

  \[\mathbf{R}' \approx \mathbf{R} (\mathbf{I} + [\delta\boldsymbol{r}]_\times) \]

两种方式都合法，但结果不同 。在计算机视觉和 SLAM 领域（包括 OpenCV、Ceres Solver、g2o 等主流库），默认采用左扰动，原因有二：

1. 参数语义清晰 ：旋转向量 \(\boldsymbol{r}\) 通常被解释为"从世界到相机的旋转"，左扰动与此一致；
2. 雅可比形式简洁：左扰动导出的雅可比矩阵具有更对称、更易记忆的结构。

本文遵循这一惯例。