【DLT实战】从零推导PnP：手撕线性方程组与SVD分解求解相机位姿

1. PnP问题与DLT方法的核心思想想象你站在一个陌生城市手里只有一张照片和几个地标建筑的GPS坐标。如何通过照片中建筑物的位置反推出自己所在的位置和拍摄角度这正是PnPPerspective-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)]我们需要求解的是Af0其中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分解后AUΣV^T最小化||Af||等价于最小化||ΣV^Tf||。当f取V的最后一列时这个范数最小。4. 从DLT解恢复相机位姿4.1 尺度因子的处理通过SVD得到的解f是没有考虑尺度的因为Af0对任意尺度的f都成立。我们需要从f中恢复出有物理意义的旋转矩阵R和平移向量T。首先将f重组成3x4的投影矩阵P。P可以表示为K[R|T]。我们需要从P中分解出K、R和T。4.2 旋转矩阵的正交化理论上旋转矩阵应该是正交的但DLT直接求解得到的矩阵可能不严格满足正交性。我们可以通过SVD进行正交化# 假设P的前3x3部分是MKR U, S, Vt np.linalg.svd(M) R_hat U Vt这里有个符号不确定的问题R ±U V^T。需要通过深度为正的约束来确定正确符号。4.3 恢复内参和尺度通过QR分解可以将MKR分解为上三角矩阵内参K和正交矩阵旋转RK, R np.linalg.qr(M)平移向量T可以通过TK^{-1}P的第4列得到。尺度因子β可以通过旋转矩阵的归一化约束确定beta 1 / np.mean(S) # S是前面SVD的奇异值5. 实际应用中的技巧与陷阱5.1 数据归一化的重要性在实际应用中我发现如果不做数据归一化DLT的精度会很差。这是因为像素坐标和3D坐标的数值范围可能相差很大。我的经验法则是对2D点将坐标转换到以图像中心为原点最大范围为[-1,1]对3D点减去均值并缩放使点到原点的平均距离为√35.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点在相机前方Z0确保旋转矩阵的行列式为1必要时对R和T同时取反7.2 退化配置当所有3D点共面时DLT会出现问题。这时应该使用专门的平面PnP算法或者添加非共面的3D点。7.3 数值稳定性在构建矩阵A时我习惯将方程除以图像尺寸使数值在相近范围内。这能显著提高SVD的数值稳定性。