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

寨重 · 4 天前

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, \quadv^{\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;
}

复制代码

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

在手写 Levenberg-Marquardt 求解器的基础上，我们进一步借助 Ceres Solver 这一工业级非线性优化库，实现一个更加简洁、鲁棒且高效的 PnP 求解器。Ceres 的核心优势在于其强大的自动微分（Automatic Differentiation） 能力——用户只需提供残差计算函数，雅可比矩阵将由框架自动、精确地生成，极大降低了实现复杂度并避免了手动推导的潜在错误。具体代码实现如下：
[code]#include #include #include #include #include #include #include using namespace std;using namespace Eigen;// ================// 相机内参（固定）// ================struct CameraIntrinsics { double fx = 500.0, fy = 500.0; double cx = 320.0, cy = 240.0;};// ================// 修复版 Rodrigues 公式（支持自动微分 + 小角度泰勒展开）// ================template Eigen::Matrix rodrigues(const Eigen::Matrix& rvec) { T theta2 = rvec.squaredNorm(); const T kThreshold = T(1e-8); // 阈值：θ² < 1e-8 → θ < 1e-4 rad ≈ 0.0057° Eigen::Matrix 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 n = rvec / theta; T c = ceres::cos(theta); T s = ceres::sin(theta); Eigen::Matrix n_skew; n_skew 相机坐标系 Eigen::Matrix R = rodrigues(rvec_eigen); Eigen::Matrix P_cam = R * X_.template cast() + 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 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

橘芜 · 前天 08:27

这个有用。

梳踟希 · 前天 12:49

谢谢分享，辛苦了

鞭氅 · 昨天 04:31

感谢分享

扔飒 · 昨天 23:20

感谢发布原创作品，程序园因你更精彩

账号		自动登录	找回密码
密码			立即注册

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

相关帖子

回复

签约作者

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

相关帖子

相关推荐

回复

签约作者