上一篇讲了对极几何解决的是两幅图像之间怎么恢复相机运动的问题。这篇讲一个更实用的场景已知一组3D点和它们在图像上的2D投影怎么估计相机的位姿这个问题在实际工程中出现的频率比对极几何高得多。这就是PnPPerspective-n-Point问题。在视觉SLAM的跟踪阶段PnP是用得最多的位姿估计方法。因为SLAM运行一段时间之后地图里已经有了一批3D路标点。来了新帧之后只需要找到这些路标点在新帧上的投影就能用PnP算出当前帧的位姿。整个过程只需要一帧图像的信息不需要和前一帧做对比这就是PnP的优势。面试的时候PnP是必考内容各种解法的原理和区别都要搞清楚。PnP问题的定义给定n个3D空间点及其在图像上的2D投影点求解相机的位姿旋转R和平移t。3D点P₁, P₂, ..., Pₙ世界坐标系下 2D点p₁, p₂, ..., pₙ像素坐标 投影关系p K[R|t]PK是相机内参要求的就是R和t。这是一个经典的几何问题有多种解法。注意这里的n可以是任意大于等于3的数不同的n对应不同的解法。n越大约束越多理论上精度越高但计算量也越大。实际中n通常在10-100之间。PnP的几种解法DLT直接线性变换最朴素的解法。把投影方程展开每个3D-2D对应提供两个线性方程。至少需要6对点因为R有约束DLT先不考虑旋转矩阵的正交性。解出来之后再做SVD分解恢复正交旋转矩阵。实现简单但精度不高通常只作为其他方法的初始化。P3P只需要3对点就能求解。原理是利用3个3D点和相机光心构成的四面体的边长关系列出一个多项式方程最多有4组解。然后用额外的点来验证哪组解是对的。P3P计算很快适合做RANSAC的内层求解器。P3P的直觉是这样的假设你知道三个3D点A、B、C的位置也看到它们在图像上的投影a、b、c。从投影关系你能算出光心O到A、B、C三条射线之间的夹角用内参和像素坐标就能算。然后在三角形OAB中AB已知角AOB已知用余弦定理就能列出OA和OB的关系。三个三角形给出三个方程解出OA、OB、OC的长度位姿就恢复了。EPnP2009年提出的高效解法。把n个3D点用4个虚拟控制点来表示把问题从求解n个点的位姿简化为求解4个控制点的坐标。复杂度O(n)精度也不错。OpenCV里的EPnP实现是常用的选择。EPnP的核心优势是计算速度快几百个点的PnP也能在1毫秒内完成。# OpenCV中求解PnP retval, rvec, tvec cv2.solvePnP( objectPoints, # N×1×3 的3D点 imagePoints, # N×1×2 的2D点 cameraMatrix, # 相机内参K distCoeffs, # 畸变系数 flagscv2.SOLVEPNP_EPNP # 用EPnP方法 ) # rvec: 旋转向量轴角表示tvec: 平移向量迭代法非线性优化以上方法都是线性或近似的。迭代法直接把PnP建模为非线性最小二乘问题用Gauss-Newton或Levenberg-Marquardt迭代求解。精度通常最高但需要好的初始值。实际中通常先用线性方法如EPnP给出初始值再用迭代法精化。在SLAM中PnP怎么用视觉SLAM的跟踪阶段PnP是这样工作的第一步当前帧提取特征点。 第二步和地图中已有的3D路标点做匹配通过描述子匹配或者特征追踪。 第三步有了3D-2D对应关系用PnP求解当前帧的位姿。 第四步用RANSAC剔除误匹配得到可靠的位姿估计。ORB-SLAM2的跟踪线程就是这么做的。它的地图里存着大量ORB特征点及其3D坐标新帧来了之后通过描述子匹配找到对应的3D-2D关系然后用P3PRANSAC求解位姿最后用迭代法精化。整个过程通常在十几毫秒内就能完成满足实时要求。VINS-Mono的位姿估计稍有不同。它不是直接用PnP而是用滑动窗口内的IMU预积分结果作为先验然后用视觉观测来修正。但本质上也是在解一个3D-2D的位姿估计问题只是多了IMU的约束。面试中的高频追问P3P为什么只需要3个点 3个3D点和相机光心构成一个四面体。3个3D点之间的距离已知从地图中得到3个3D点到光心的距离未知但有约束通过投影关系和余弦定理。三个余弦定理方程可以解出三个距离进而恢复位姿。方程最多有4组正实数解用第4个点来消歧。PnP的精度受什么影响 三个因素3D点的精度地图质量、2D点的精度特征提取和匹配的精度、点的分布点越分散位姿估计越稳定。如果所有3D点都集中在一个很小的区域位姿估计的误差会很大。为什么SLAM跟踪阶段用PnP而不用对极几何 对极几何只需要2D-2D匹配恢复的是相对运动两帧之间的R和t。PnP用的是3D-2D匹配直接恢复绝对位姿相对于世界坐标系的R和t。SLAM跟踪阶段地图已经建好了有3D信息可用用PnP更直接、更准确。solvePnP和solvePnPRansac有什么区别 solvePnPRansac内部集成了RANSAC能自动剔除误匹配。solvePnP假设所有输入都是正确的匹配。实际中几乎总是用solvePnPRansac因为匹配中难免有错误。PnP和ICP有什么区别 PnP是3D-2D的配准3D点投影到2D图像ICP是3D-3D的配准两组3D点云对齐。PnP用在视觉SLAM里ICP用在激光SLAM里。两者的数学形式不同但目标一样找到最优的旋转和平移。旋转向量rvec是什么 旋转向量是旋转的轴角表示。向量的方向是旋转轴长度是旋转角度。用Rodrigues公式可以转换成3×3的旋转矩阵。旋转向量只有3个参数比旋转矩阵的9个参数更紧凑优化起来更方便。PnP和其他位姿估计方法的对比SLAM里估计位姿有几种方法适用场景不同2D-2D对极几何两帧之间没有3D信息时使用恢复的是相对运动。用于SLAM的初始化阶段。3D-3DICP两组3D点云之间的配准。用于激光SLAM或者RGB-D相机的位姿估计。3D-2DPnP已知3D点求相机位姿。用于视觉SLAM的跟踪阶段是最常用的方法。直接法不用特征点直接用像素灰度的光度误差来估计位姿。计算快但要求光照稳定。这四种方法覆盖了SLAM中所有可能的位姿估计场景。理解它们的区别和联系面试的时候就能对答如流展现出你对SLAM前端的全面理解。工程实践中的经验PnP在实际使用中要注意几点。输入点不能太少至少需要6-8对匹配才能给出稳定的结果。如果匹配点太少比如只有3-4对位姿估计会很不稳定。这时候应该降低特征提取的阈值或者扩大匹配搜索范围。3D-2D匹配的质量很关键。如果匹配了很多错误的3D点比如把A路标点匹配到了B路标点的投影上PnP的结果就会跑偏。所以匹配阶段要严格验证宁缺勿滥。RANSAC的参数要根据场景调。内点阈值reprojection error阈值通常设为1-3个像素。迭代次数根据预期的外点比例来定一般设为100-200次。还有一个容易忽略的点畸变校正。如果相机有明显的畸变比如鱼眼镜头在做PnP之前必须先把2D点校正到无畸变的坐标。否则投影模型不匹配PnP的结果会有系统性的偏差。OpenCV的undistortPoints函数可以做这个校正。最后一个建议PnP的结果最好做一次BA精化。PnP给出的位姿只是初步估计用BABundle Adjustment对所有变量位姿路标点做一次联合优化精度会更高。ORB-SLAM的局部BA就是在做这件事。PnP是视觉SLAM跟踪阶段的核心算法也是面试中最高频的考点之一。从P3P到EPnP从线性解法到非线性优化每种方法的原理和适用场景你都要心里有数。上一篇第241篇 视觉SLAM前端之对极几何——本质矩阵和基础矩阵下一篇我们聊直接法——不用特征点直接用像素灰度来做位姿估计的另一条路。直接法和特征点法是视觉SLAM的两大流派各有优劣面试的时候经常被拿来对比。如果这篇文章对你有帮助欢迎点赞支持一下你的鼓励是我持续更新的动力