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)** 进行迭代优化，即可得到精度最高的相机位姿。 *** ** * ** ***