SLAM前端：PnP（一）DLS、P3P

PnP是一类问题的统称，是指通过多对点的3D位置及2D投影座标，来估计相机位姿R、t。

场景一：视觉slam中在初始化后可以知道空间中一些点在世界座标系下的座标，在下一帧图像进行特征点匹配后，利用这些特征点的3D座标及2D像素座标，PnP可以计算出当前帧图像的R、t，即在世界座标系下的R、t。

场景二：输入匹配特征点在上一帧的2D像素座标、在上一帧相机座标系下的3D座标、在当前帧的2D像素座标，求两帧之间的位姿R、t。

PnP是一大类问题，根据问题不同解法有P3P、DLS、EPnP、UPnP等。

1 P3P

对于场景一问题可以用P3P问题求解，需要三个点即可完成。需要条件：三个点的世界座标、当前帧下的2D像素座标。求解思路是首先求解三个点在相机座标系下的座标，再通过世界座标系到像素座标系的转换公式计算相机在世界座标系下的R、t。

 

 

场景如上图所示，三个空间点ABC、图像投影点abc、相机光心O。计算过程如下：

首先根据余弦定理可得：

                                               

等式两边同时除以，并令得到：

                                                          

令得到：

                                                            

方程中已知量为，未知量为x,y，两个未知量将上述方程化简得到：

求解cos<a,b>:

 

 

得到角度为：

同理可以得到：cos<a,c>，cos<b,c>

2 DLS（直接线性变换）

设空间中某点P的世界座标为，在图像中的2D座标为，现在求解R、t。

首先定义曾广矩阵，由世界座标系转像素座标系有：

                                                         

消去s得到：

                                 

令等式右边为0得到：

                                               

每个特征点提供两个方程，求解需要6个点。