如何用PnP算法高效求解最小二乘问题？
摘要：深入剖析了计算机视觉中PnP问题的数学模型与求解策略，详细推导了基于旋转向量参数化的重投影误差雅可比矩阵，并通过手写Levenberg-Marquardt算法与Ceres自动微分框架的对比实现，展示了从理论推导到工程落地的完整流程。
1. 引言
通过本系列前几篇文章（最小二乘问题详解：目录）的学习，我们对最小二乘问题有了较为系统的认识：它是一种广泛应用于科学与工程领域的参数估计与优化方法。在计算机视觉中，最小二乘思想贯穿于许多核心算法，尤其在运动恢复结构（Structure from Motion, SFM）这一经典框架中体现得尤为明显。
SFM 的完整流程通常包含五个关键子问题：
相机标定（Camera Calibration）
PnP 问题（Perspective-n-Point）
三角化（Triangulation）
对极几何估计（Fundamental / Essential Matrix Estimation）
束平差（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 等主流库），默认采用左扰动，原因有二：
参数语义清晰：旋转向量 \(\boldsymbol{r}\) 通常被解释为“从世界到相机的旋转”，左扰动与此一致；
雅可比形式简洁：左扰动导出的雅可比矩阵具有更对称、更易记忆的结构。
本文遵循这一惯例。