【DLT实战】从零推导PnP：手撕线性方程组与SVD分解求解相机位姿
1. PnP问题与DLT方法的核心思想
想象你站在一个陌生城市，手里只有一张照片和几个地标建筑的GPS坐标。如何通过照片中建筑物的位置反推出自己所在的位置和拍摄角度？这正是PnP（Perspective-n-Point）问题要解决的核心场景。在计算机视觉中，我们通过已知的3D空间点坐标和它们在2D图像上的投影位置，求解相机的位姿（位置和姿态）。
DLT（直接线性变换）是PnP问题最直观的解法之一。我第一次接触这个方法时，被它巧妙地将几何问题转化为线性代数问题的思路惊艳到了。它的核心思想可以概括为三步：
- 建立3D点到2D点的投影方程
- 将非线性方程转化为线性方程组
- 用SVD分解求解超定方程组
关键突破点在于如何将看似复杂的相机投影模型，拆解成我们可以用线性代数工具处理的形式。这就像把一道立体几何题转化为坐标系中的向量运算，突然就变得可计算了。
2. 从投影方程到线性方程组
2.1 相机投影模型的数学表达
相机的成像过程本质上是一个透视投影。用数学表示就是：
Z_c * [u, v, 1]^T = K [R|T] [X_w, Y_w, Z_w, 1]^T这个方程看起来有点吓人，但拆开看每个部分都有明确的物理意义：
Z_c是点在相机坐标系下的深度[u,v]是图像像素坐标K是相机内参矩阵[R|T]是外参（旋转和平移）[X_w,Y_w,Z_w]是世界坐标
我第一次推导时，最困惑的是为什么等式左边要乘以Z_c。后来才明白这是为了将齐次坐标转换为非齐次坐标，相当于把三维点投影到归一化成像平面。
2.2 展开投影方程
让我们把矩阵乘法彻底展开。假设内参矩阵K为：
[fx 0 u0] [ 0 fy v0] [ 0 0 1]将外参矩阵[R|T]记为3x4的矩阵，其元素为f11到f34。经过展开后，我们得到三个方程：
Z_c*u = fx*(f11*X_w + f12*Y_w + f13*Z_w + f14) + u0*(f31*X_w + f32*Y_w + f33*Z_w + f34) Z_c*v = fy*(f21*X_w + f22*Y_w + f23*Z_w + f24) + v0*(f31*X_w + f32*Y_w + f33*Z_w + f34) Z_c = f31*X_w + f32*Y_w + f33*Z_w + f342.3 消去Z_c得到线性方程
这里有个精妙的技巧：用第三个方程消去前两个方程中的Z_c。比如第一个方程可以写成：
fx*(f11*X_w + f12*Y_w + f13*Z_w + f14) + (u0-u)*(f31*X_w + f32*Y_w + f33*Z_w + f34) = 0这样我们就得到了关于f11到f34的线性方程。每对3D-2D点对应点可以给出两个这样的方程。
3. 构建超定方程组与SVD求解
3.1 构建系数矩阵
当有n对匹配点时，我们可以构建一个2n×12的矩阵A。这个矩阵的每一行对应一个方程，例如第i对点对应的两行是：
[fx*X_i, fx*Y_i, fx*Z_i, fx, 0,0,0,0, (u0-u)*X_i, (u0-u)*Y_i, (u0-u)*Z_i, (u0-u)] [0,0,0,0, fy*X_i, fy*Y_i, fy*Z_i, fy, (v0-v)*X_i, (v0-v)*Y_i, (v0-v)*Z_i, (v0-v)]我们需要求解的是Af=0，其中f是所有f11到f34排列成的12维向量。
3.2 最小二乘解与SVD
当n≥6时，这是一个超定方程组。我们需要找使||Af||最小的f，且||f||=1。这正是SVD大显身手的地方：
U, S, Vt = np.linalg.svd(A) f = Vt[-1] # 取最小奇异值对应的右奇异向量为什么取最后一个右奇异向量？因为SVD分解后，A=UΣV^T，最小化||Af||等价于最小化||ΣV^Tf||。当f取V的最后一列时，这个范数最小。
4. 从DLT解恢复相机位姿
4.1 尺度因子的处理
通过SVD得到的解f是没有考虑尺度的（因为Af=0对任意尺度的f都成立）。我们需要从f中恢复出有物理意义的旋转矩阵R和平移向量T。
首先将f重组成3x4的投影矩阵P。P可以表示为K[R|T]。我们需要从P中分解出K、R和T。
4.2 旋转矩阵的正交化
理论上旋转矩阵应该是正交的，但DLT直接求解得到的矩阵可能不严格满足正交性。我们可以通过SVD进行正交化：
# 假设P的前3x3部分是M=KR U, S, Vt = np.linalg.svd(M) R_hat = U @ Vt这里有个符号不确定的问题：R = ±U V^T。需要通过深度为正的约束来确定正确符号。
4.3 恢复内参和尺度
通过QR分解可以将M=KR分解为上三角矩阵（内参K）和正交矩阵（旋转R）：
K, R = np.linalg.qr(M)平移向量T可以通过T=K^{-1}P的第4列得到。尺度因子β可以通过旋转矩阵的归一化约束确定：
beta = 1 / np.mean(S) # S是前面SVD的奇异值5. 实际应用中的技巧与陷阱
5.1 数据归一化的重要性
在实际应用中，我发现如果不做数据归一化，DLT的精度会很差。这是因为像素坐标和3D坐标的数值范围可能相差很大。我的经验法则是：
- 对2D点：将坐标转换到以图像中心为原点，最大范围为[-1,1]
- 对3D点：减去均值并缩放，使点到原点的平均距离为√3
5.2 异常值处理
DLT对异常点非常敏感。我通常会：
- 先用RANSAC去除外点
- 对所有内点应用DLT
- 用非线性优化进一步优化结果
5.3 与非线性优化的结合
DLT的解可以作为Bundle Adjustment的良好初始值。在我的项目中，这种组合方式既保证了效率又获得了高精度：
# 伪代码示例 poses = dlt_solve(points3d, points2d) optimized_poses = bundle_adjustment(poses, points3d, points2d)6. 与其他PnP方法的对比
虽然DLT简单直观，但在不同场景下可能需要考虑其他方法：
- P3P：只需要3个点，但可能有多个解
- EPnP：计算效率高，适合实时应用
- UPnP：不需要知道相机内参
在我的性能测试中，当点数>20时，DLT的精度与EPnP相当，但计算量更大。不过DLT有个独特优势：当相机内参不准确时，它比其他方法更鲁棒。
7. 实现中的常见问题
7.1 符号确定问题
在恢复R和T时，符号模糊是个棘手问题。我的解决方案是：
- 检查3D点在相机前方（Z>0）
- 确保旋转矩阵的行列式为+1
- 必要时对R和T同时取反
7.2 退化配置
当所有3D点共面时，DLT会出现问题。这时应该使用专门的平面PnP算法，或者添加非共面的3D点。
7.3 数值稳定性
在构建矩阵A时，我习惯将方程除以图像尺寸，使数值在相近范围内。这能显著提高SVD的数值稳定性。