PnP 问题：数学描述与 DLT 算法推导

在计算机视觉和位姿估计领域，PnP（Perspective-n-Point）问题 是一个经典的几何问题。它的核心目标是：

已知空间中 n n n 个 3D 点在世界坐标系下的坐标，以及它们在相机图像平面上对应的 2D 像素投影坐标，求解相机的 6 自由度（6-DoF）位姿（即旋转矩阵 R \mathbf{R} R 和平移向量 t \mathbf{t} t）。

PnP 广泛应用于相机标定、SLAM（同步定位与建图）、AR/VR（增强/虚拟现实）以及机器人视觉导航等场景。

以下是 PnP 问题的详细数学描述与基于 DLT（Direct Linear Transform，直接线性变换）方法的数学推导。

一、数学描述与基本模型

要解决 PnP 问题，首先需要建立 3D 点到 2D 像素的针孔相机投影模型。

1. 定义变量

3D 空间点 ：设世界坐标系下的第 i i i 个 3D 点为

P w i = $X w i , Y w i , Z w i$ T \mathbf{P}_{wi} = $X_{wi}, Y_{wi}, Z_{wi}$ ^T Pwi= $Xwi,Ywi,Zwi$ T

其齐次坐标为 P ~ w i = $X w i , Y w i , Z w i , 1$ T \tilde{\mathbf{P}}_{wi} = $X_{wi}, Y_{wi}, Z_{wi}, 1$ ^T P~wi= $Xwi,Ywi,Zwi,1$ T。

2D 图像点 ：对应的图像像素坐标为 p i = $u i , v i$ T \mathbf{p}_i = $u_i, v_i$ ^T pi= $ui,vi$ T，其齐次坐标为 p ~ i = $u i , v i , 1$ T \tilde{\mathbf{p}}_i = $u_i, v_i, 1$ ^T p~i= $ui,vi,1$ T。
相机内参矩阵 K \mathbf{K} K ：包含了相机的焦距 ( f x , f y ) (f_x, f_y) (fx,fy) 和主点坐标 ( c x , c y ) (c_x, c_y) (cx,cy)，这是已知的标定参数。

K = $f x 0 c x 0 f y c y 0 0 1$ \mathbf{K} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} K= fx000fy0cxcy1

相机外参（待求的位姿） ：旋转矩阵 R ∈ S O ( 3 ) \mathbf{R} \in SO(3) R∈SO(3)（ 3 × 3 3 \times 3 3×3 的正交矩阵）和平移向量 t ∈ R 3 \mathbf{t} \in \mathbb{R}^3 t∈R3（ 3 × 1 3 \times 1 3×1 的向量）。通常组合成一个 3 × 4 3 \times 4 3×4 的增广矩阵 $R ∣ t$ $\\mathbf{R} \\mid \\mathbf{t}$ $R∣t$ 。

2. 投影方程

根据针孔相机模型，3D 空间点到 2D 像素点的转换关系可以通过以下矩阵乘法表示：

s i $u i v i 1$ = K $R t$ $X w i Y w i Z w i 1$ s_i \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = \mathbf{K} \begin{bmatrix} \mathbf{R} & \mathbf{t} \end{bmatrix} \begin{bmatrix} X_{wi} \\ Y_{wi} \\ Z_{wi} \\ 1 \end{bmatrix} si uivi1 =K $Rt$ XwiYwiZwi1

其中， s i s_i si 是该点在相机坐标系下的深度（一个标量）。

二、数学推导：直接线性变换 (DLT)

求解 PnP 的方法有很多（如 P3P、EPnP、非线性优化等）。为了给出严谨的推导，这里使用最基础且通用的 DLT (Direct Linear Transform) 算法，它将非线性的几何问题转化为线性方程组求解。

步骤 1：归一化坐标转换

由于内参矩阵 K \mathbf{K} K 已知，我们可以将其移到等式左边，将像素坐标转换到归一化图像平面，消除内参的影响：

x i = $x i y i 1$ = K − 1 $u i v i 1$ \mathbf{x}_i = \begin{bmatrix} x_i \\ y_i \\ 1 \end{bmatrix} = \mathbf{K}^{-1} \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} xi= xiyi1 =K−1 uivi1

此时，投影方程简化为：

s i $x i y i 1$ = $R t$ $X w i Y w i Z w i 1$ s_i \begin{bmatrix} x_i \\ y_i \\ 1 \end{bmatrix} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \end{bmatrix} \begin{bmatrix} X_{wi} \\ Y_{wi} \\ Z_{wi} \\ 1 \end{bmatrix} si xiyi1 = $Rt$ XwiYwiZwi1

步骤 2：构建线性方程

为了方便推导，我们将待求的 3 × 4 3 \times 4 3×4 外参矩阵设为 M = $R ∣ t$ \mathbf{M} = $\\mathbf{R} \\mid \\mathbf{t}$ M= $R∣t$ 。记 M \mathbf{M} M 的三行分别为 m 1 , m 2 , m 3 \mathbf{m}_1, \mathbf{m}_2, \mathbf{m}_3 m1,m2,m3（均为 1 × 4 1 \times 4 1×4 的行向量）：

M = $m 1 m 2 m 3$ \mathbf{M} = \begin{bmatrix} \mathbf{m}_1 \\ \mathbf{m}_2 \\ \mathbf{m}_3 \end{bmatrix} M= m1m2m3

则投影方程可以展开为：

s i $x i y i 1$ = $m 1 P \~ w i m 2 P \~ w i m 3 P \~ w i$ s_i \begin{bmatrix} x_i \\ y_i \\ 1 \end{bmatrix} = \begin{bmatrix} \mathbf{m}1 \tilde{\mathbf{P}}{wi} \\ \mathbf{m}2 \tilde{\mathbf{P}}{wi} \\ \mathbf{m}3 \tilde{\mathbf{P}}{wi} \end{bmatrix} si xiyi1 = m1P~wim2P~wim3P~wi

由此我们可以得到三个等式：

s i x i = m 1 P ~ w i s i y i = m 2 P ~ w i s i = m 3 P ~ w i \begin{aligned} s_i x_i &= \mathbf{m}1 \tilde{\mathbf{P}}{wi} \\ s_i y_i &= \mathbf{m}2 \tilde{\mathbf{P}}{wi} \\ s_i &= \mathbf{m}3 \tilde{\mathbf{P}}{wi} \end{aligned} sixisiyisi=m1P~wi=m2P~wi=m3P~wi

将第三个等式代入前两个等式，消去未知的深度尺度 s i s_i si：

( m 3 P ~ w i ) x i = m 1 P ~ w i (\mathbf{m}3 \tilde{\mathbf{P}}{wi}) x_i = \mathbf{m}1 \tilde{\mathbf{P}}{wi} (m3P~wi)xi=m1P~wi

( m 3 P ~ w i ) y i = m 2 P ~ w i (\mathbf{m}3 \tilde{\mathbf{P}}{wi}) y_i = \mathbf{m}2 \tilde{\mathbf{P}}{wi} (m3P~wi)yi=m2P~wi

移项并整理，将未知的矩阵参数 m \mathbf{m} m 提取出来：

P ~ w i T m 1 T − x i P ~ w i T m 3 T = 0 \tilde{\mathbf{P}}_{wi}^T \mathbf{m}1^T - x_i \tilde{\mathbf{P}}{wi}^T \mathbf{m}_3^T = 0 P~wiTm1T−xiP~wiTm3T=0

P ~ w i T m 2 T − y i P ~ w i T m 3 T = 0 \tilde{\mathbf{P}}_{wi}^T \mathbf{m}2^T - y_i \tilde{\mathbf{P}}{wi}^T \mathbf{m}_3^T = 0 P~wiTm2T−yiP~wiTm3T=0

步骤 3：矩阵形式与 SVD 求解

将 M \mathbf{M} M 的所有元素展开成一个 12 × 1 12 \times 1 12×1 的列向量 h = $m 1 , m 2 , m 3$ T \mathbf{h} = $\\mathbf{m}_1, \\mathbf{m}_2, \\mathbf{m}_3$ ^T h= $m1,m2,m3$ T。对于第 i i i 个点对，我们可以构建一个 2 × 12 2 \times 12 2×12 的线性方程：

$P \~ w i T 0 T − x i P \~ w i T 0 T P \~ w i T − y i P \~ w i T$ h = 0 \begin{bmatrix} \tilde{\mathbf{P}}{wi}^T & \mathbf{0}^T & -x_i \tilde{\mathbf{P}}{wi}^T \\ \mathbf{0}^T & \tilde{\mathbf{P}}{wi}^T & -y_i \tilde{\mathbf{P}}{wi}^T \end{bmatrix} \mathbf{h} = \mathbf{0} $P\~wiT0T0TP\~wiT−xiP\~wiT−yiP\~wiT$ h=0

其中 0 T \mathbf{0}^T 0T 是 1 × 4 1 \times 4 1×4 的零向量。

由于 h \mathbf{h} h 有 12 个未知数，每个 3D-2D 匹配点对能提供 2 个方程。因此，至少需要 6 个不共面的点 就能构建 12 × 12 12 \times 12 12×12 的矩阵 A \mathbf{A} A，使得：

A h = 0 \mathbf{A} \mathbf{h} = \mathbf{0} Ah=0

为了避免平凡解 h = 0 \mathbf{h} = \mathbf{0} h=0，我们通常增加一个约束 ∥ h ∥ = 1 \|\mathbf{h}\| = 1 ∥h∥=1。这是一个典型的最小二乘问题，可以通过对矩阵 A \mathbf{A} A 进行奇异值分解 (SVD) 来求解：

A = U Σ V T \mathbf{A} = \mathbf{U} \mathbf{\Sigma} \mathbf{V}^T A=UΣVT

h \mathbf{h} h 的最优解即为矩阵 V \mathbf{V} V 的最后一列（对应于 A \mathbf{A} A 最小奇异值的右奇异向量）。

补充说明：为什么这是一个"最小二乘问题"？

很多人初学时会困惑：上面明明是齐次方程 A h = 0 \mathbf{A}\mathbf{h} = \mathbf{0} Ah=0，为什么突然就变成"最小二乘"了？关键在于理想情况与现实情况的差别。

① 理想情况（无噪声）

如果 3D 点和 2D 像素观测都完全精确，那么对于真实的位姿向量 h ∗ \mathbf{h}^* h∗，方程

A h ∗ = 0 \mathbf{A}\mathbf{h}^* = \mathbf{0} Ah∗=0

是严格成立 的。此时 A \mathbf{A} A 的秩为 11（而不是 12）， h ∗ \mathbf{h}^* h∗ 就是 A \mathbf{A} A 的零空间（null space）中的向量，可以直接通过解齐次线性方程组得到。

② 现实情况（有噪声）

实际中，像素坐标 ( u i , v i ) (u_i, v_i) (ui,vi) 的测量总有噪声，3D 点坐标 P w i \mathbf{P}_{wi} Pwi 也可能不精确。这导致：

A h ≠ 0 对任何 h 都成立 \mathbf{A}\mathbf{h} \neq \mathbf{0} \quad \text{对任何 } \mathbf{h} \text{ 都成立} Ah=0对任何 h 都成立

也就是说， A \mathbf{A} A 变成了满秩矩阵 （秩为 12），它的零空间里只有 0 \mathbf{0} 0 这一个向量。

③ 于是问题被迫"松弛"

既然找不到一个 h \mathbf{h} h 让 A h = 0 \mathbf{A}\mathbf{h} = \mathbf{0} Ah=0 严格成立，我们就退而求其次：

找一个 h \mathbf{h} h，让 A h \mathbf{A}\mathbf{h} Ah 尽可能接近 0 \mathbf{0} 0。

"尽可能接近"用数学语言表达就是最小化 ∥ A h ∥ 2 \|\mathbf{A}\mathbf{h}\|^2 ∥Ah∥2：

h ∗ = arg ⁡ min ⁡ h ∥ A h ∥ 2 \mathbf{h}^* = \arg\min_{\mathbf{h}} \|\mathbf{A}\mathbf{h}\|^2 h∗=arghmin∥Ah∥2

这就是最小二乘的定义------最小化"误差向量的平方和（二范数的平方）"。

④ 为什么必须加约束 ∥ h ∥ = 1 \|\mathbf{h}\| = 1 ∥h∥=1？

注意上面这个优化问题有一个致命的平凡解：

h = 0 ⟹ ∥ A h ∥ 2 = 0 \mathbf{h} = \mathbf{0} \implies \|\mathbf{A}\mathbf{h}\|^2 = 0 h=0⟹∥Ah∥2=0

也就是直接令所有参数为零，误差就是零，最优！但这没有任何物理意义（外参矩阵不能是零矩阵）。

更深层的原因是：投影方程 s i p ~ i = M P ~ w i s_i \tilde{\mathbf{p}}i = \mathbf{M}\tilde{\mathbf{P}}{wi} sip~i=MP~wi 中，如果把 M \mathbf{M} M 整体乘以任意非零常数 k k k， s i s_i si 也跟着变成 k s i k s_i ksi，等式照样成立。h \mathbf{h} h 的尺度是不确定的------这是齐次方程的固有性质。

所以我们人为固定尺度 ，最常用的就是令 ∥ h ∥ = 1 \|\mathbf{h}\| = 1 ∥h∥=1（单位向量）。这样既排除了 0 \mathbf{0} 0 这个平凡解，又锁定了一个唯一的尺度。

⑤ 完整的最小二乘问题形式

把上面所有内容综合起来，DLT 实际求解的是这个带约束的最小二乘问题：

h ∗ = arg ⁡ min ⁡ h ∥ A h ∥ 2 s.t. ∥ h ∥ = 1 \mathbf{h}^* = \arg\min_{\mathbf{h}} \|\mathbf{A}\mathbf{h}\|^2 \quad \text{s.t.} \quad \|\mathbf{h}\| = 1 h∗=arghmin∥Ah∥2s.t.∥h∥=1

⑥ 为什么 SVD 能解？

对 A \mathbf{A} A 做 SVD： A = U Σ V T \mathbf{A} = \mathbf{U}\mathbf{\Sigma}\mathbf{V}^T A=UΣVT。设 V \mathbf{V} V 的列向量为 v 1 , v 2 , ... , v 12 \mathbf{v}_1, \mathbf{v}2, \ldots, \mathbf{v}{12} v1,v2,...,v12（对应奇异值从大到小排列），任何单位向量 h \mathbf{h} h 都可以写成它们的线性组合：

h = ∑ j = 1 12 α j v j , ∑ j α j 2 = 1 \mathbf{h} = \sum_{j=1}^{12} \alpha_j \mathbf{v}_j, \quad \sum_j \alpha_j^2 = 1 h=j=1∑12αjvj,j∑αj2=1

代入目标函数（利用 U \mathbf{U} U 是正交阵，不改变范数）：

∥ A h ∥ 2 = ∥ Σ V T h ∥ 2 = ∑ j = 1 12 σ j 2 α j 2 \|\mathbf{A}\mathbf{h}\|^2 = \|\mathbf{\Sigma}\mathbf{V}^T\mathbf{h}\|^2 = \sum_{j=1}^{12} \sigma_j^2 \alpha_j^2 ∥Ah∥2=∥ΣVTh∥2=j=1∑12σj2αj2

要在 ∑ α j 2 = 1 \sum \alpha_j^2 = 1 ∑αj2=1 的约束下让这个加权和最小，显然应该把全部权重放在最小的 σ j \sigma_j σj 上 ------也就是 σ 12 \sigma_{12} σ12，对应 α 12 = 1 \alpha_{12} = 1 α12=1，其余为 0。

所以最优解就是：

h ∗ = v 12 （ V 的最后一列） \mathbf{h}^* = \mathbf{v}_{12} \quad（\mathbf{V} \text{ 的最后一列}） h∗=v12（V 的最后一列）

⑦ 小结

概念	含义
为什么是最小二乘？	噪声使 A h = 0 \mathbf{A}\mathbf{h}=\mathbf{0} Ah=0 无法严格成立，转而最小化残差范数 ∣ A h ∣ 2 \|\mathbf{A}\mathbf{h}\|^2 ∣Ah∣2
为什么要约束 ∣ h ∣ = 1 \|\mathbf{h}\|=1 ∣h∣=1？	① 排除平凡解 h = 0 \mathbf{h}=\mathbf{0} h=0；② 固定齐次方程的尺度自由度
为什么用 SVD？	带二次约束的二次优化问题有闭式解，即 A \mathbf{A} A 最小奇异值对应的右奇异向量

一句话概括：有噪声 → 残差不为零 → 最小化残差范数（最小二乘）；齐次方程有尺度自由度 → 加单位长度约束 → SVD 闭式求解。

步骤 4：正交化恢复旋转矩阵

解出 h \mathbf{h} h 后，可以将其重新排列回 3 × 4 3 \times 4 3×4 的矩阵 M = $R \^ ∣ t \^$ \mathbf{M} = $\\hat{\\mathbf{R}} \\mid \\hat{\\mathbf{t}}$ M= $R\^∣t\^$ 。

然而，DLT 方法是在没有任何约束的情况下求解出来的 12 个参数，这意味着解出的 R ^ \hat{\mathbf{R}} R^ 并不一定满足旋转矩阵的数学性质（即正交矩阵， S O ( 3 ) SO(3) SO(3)，行列式为 1）。

为了寻找最接近的合法旋转矩阵 R \mathbf{R} R，我们对提取出的 R ^ \hat{\mathbf{R}} R^ 再次进行 SVD：

R ^ = U R Σ R V R T \hat{\mathbf{R}} = \mathbf{U}_R \mathbf{\Sigma}_R \mathbf{V}_R^T R^=URΣRVRT

由于理想的正交矩阵其奇异值应全为 1，我们将其强制修改为单位阵，从而得到符合约束的旋转矩阵：

R = U R V R T \mathbf{R} = \mathbf{U}_R \mathbf{V}_R^T R=URVRT

注：如果解出的 R \mathbf{R} R 行列式为 − 1 -1 −1，说明包含了一个镜像变换，需要将 V R \mathbf{V}_R VR 的最后一列乘 − 1 -1 −1 修正。

最后，根据尺度恢复平移向量 t \mathbf{t} t（通过 R ^ \hat{\mathbf{R}} R^ 到 R \mathbf{R} R 的缩放比例）。

三、进阶：非线性优化（Bundle Adjustment）

DLT 等线性代数方法（包括 P3P、EPnP）虽然计算快，但在实际应用中对噪声非常敏感，且正交化操作会引入误差。因此，线性方法通常只作为初始值。

在实际工程中，最终都需要依靠非线性优化 来最小化重投影误差 (Reprojection Error)。我们构建以下目标函数（最小二乘优化问题）：

R ∗ , t ∗ = arg ⁡ min ⁡ R , t ∑ i = 1 n ∥ p i − π ( K , R , t , P w i ) ∥ 2 \mathbf{R}^*, \mathbf{t}^* = \arg \min_{\mathbf{R}, \mathbf{t}} \sum_{i=1}^n \left\| \mathbf{p}i - \pi(\mathbf{K}, \mathbf{R}, \mathbf{t}, \mathbf{P}{wi}) \right\|^2 R∗,t∗=argR,tmini=1∑n∥pi−π(K,R,t,Pwi)∥2

其中 π ( ⋅ ) \pi(\cdot) π(⋅) 是将 3D 点投影到 2D 像素平面的非线性函数。

利用李代数 s e ( 3 ) \mathfrak{se}(3) se(3) 表示位姿，计算雅可比矩阵，然后使用高斯-牛顿法 (Gauss-Newton) 或列文伯格-马夸尔特算法 (Levenberg-Marquardt, LM) 进行迭代优化，即可得到精度最高的相机位姿。