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

charlee44

发布于 2026-03-10 10:10:49

1030

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）问题的目标是：给定

个已知的世界坐标系下的 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}

因此，第

个点的残差向量为：

\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

由于模型

包含旋转（通过 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 点的变换是线性关系。

设当前位姿下，第

个 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

）；

平移在

方向的改变会通过改变深度间接影响所有像素坐标，且影响程度与当前点的

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) 拼接，得到第

个点的完整

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}\)	雅可比矩阵

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