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:

 

 

得到角度为：

同理可以得到：cos，cos

2 DLS（直接线性变换）

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

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

                                                         

消去s得到：

                                 

令等式右边为0得到：

                                               

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