相机位姿求解——P3P问题总结汇报(SLAM、SFM).pptxVIP

SLAM、SFM、3D重建P3P问题总结汇报汇报人姓名汇报日期

CONTENTS010203相机位姿求解概述P3P的求解过程P3P代码讲解

相机位姿求解

——P3P问题/01

引入位姿求解是计算机视觉中经常遇到的，Perspective-n-Points,PnP(P3P)提供了一种解决方案，它是一种由3D-2D的位姿求解方式，即需要已知匹配的3D点和图像2D点。目前遇到的场景主要有两个，其一是求解相机相对于某2维图像/3维物体的位姿，具体的如AR应用，人脸跟踪等；其二就是SLAM算法中估计相机位姿时通常需要PnP给出相机初始位姿。

场景1这里要说明的是在场景1中，我们通常输入的是物体在世界坐标系下的3D点以及这些3D点在图像上投影的2D点，因此求得的是相机（相机坐标系）相对于真实物体（世界坐标系）的位姿，如图所示：1

场景2在场景2中，通常输入的是上一帧中的3D点（在上一帧的相机坐标系下表示的点）和这些3D点在当前帧中的投影得到的2D点，所以它求得的是当前帧相对于上一帧的位姿变换，如图所示：2

P3P的求解过程/02

两种情况本质上是相同的，都是基于已知3D点和对应的图像2D点求解相机运动的过程。下面详细探讨P3P的求解过程。

我们首先需要知道的是P3P并不是直接根据3D-2D点求出相机位姿矩阵，而是先求出对应的2D点在当前相机坐标系下的3D坐标，然后根据世界坐标系下的3D坐标和当前相机坐标系下的3D坐标求解相机位姿的。

P3P的求解是从余弦定理开始的，设相机坐标中心为点P，A、B、C为不共线的三个3D点，D为验证3D点，根据余弦定理有如下公式：接下来其实是对上述3个式子消元化简的过程，同时除以并且使得：

则可得：然后再次进行替换，另：

则可得：将第一个式子代入第2，3式，可以化简得到：

接下来的过程就是如何通过上述两个式子求解A，B，C在当前相机坐标系下的坐标。首先需要明确的是哪些量是已知量，输入的是3D-2D的坐标，也即都是已知的。因为首先AB，BC，AC的距离都是可以根据输入的3D点求得，而输入的2D点可以求解三个余弦值（如何求解，像素坐标根据相机内参矩阵和畸变参数可以求得在归一化图像平面上的3D坐标，此时z=1，故余弦值可求）。此时未知数仅x，y两个，所以理论上两个未知数两个方程，是可求的。（从x，y求PA，PB，PC也可求）

代码讲解/03

代码对应：看看上述过程是如何代码实现的①

代码对应：看看上述过程是如何代码实现的②

代码对应：看看上述过程是如何代码实现的③

代码对应：看看上述过程是如何代码实现的④

代码对应：看看上述过程是如何代码实现的⑤这里面主要是使用吴消元法求解PA，PB，PC的距离

代码对应：看看上述过程是如何代码实现的⑥这里面主要是使用吴消元法求解PA，PB，PC的距离

代码对应：看看上述过程是如何代码实现的⑦这里面主要是使用吴消元法求解PA，PB，PC的距离

代码对应：看看上述过程是如何代码实现的⑧这里面主要是使用吴消元法求解PA，PB，PC的距离

代码对应：看看上述过程是如何代码实现的⑨这里面主要是使用吴消元法求解PA，PB，PC的距离

代码对应：看看上述过程是如何代码实现的⑩这里面主要是使用吴消元法求解PA，PB，PC的距离

代码对应：看看上述过程是如何代码实现的十一看看是如何从4组解中选择合适的解的：

附：吴消元法求解系数

参考：/blog/p3p-perspective-point-overview/#Appendix资料分享

Thanks.