VSLAM基础（五）————三角化、PnP、ICP

在之前的章节中我们获得了同名像点从而恢复相片的姿态，接下来就该轮到vslam前端中最后一部分————计算三维点坐标（三角化），并通过三维坐标预测姿态（PnP）。

一、三角化

在单目slam中仅通过单张图像无法获得像素的深度信息，所以我们需要通过三角测量（Triangulation）的方法来估计地图的深度。一般情况是当我们得到两个视图的一组匹配点，我们希望能恢复出世界点在三维世界的坐标。

这里介绍一个线性三角形法：

 

在已知了两张图像的姿态后我们能建立以下等式

x=PX；

x′=P′X；

然后使用DLT算法整理可得AX = 0（一对点可得到4行方程）

这里相当于解一个线性最小二乘问题。方程的解为A的最小奇异值对应的单位奇异矢量。

而三角化存在一个矛盾：

图像平移越大，三角化的精度越高；平移越小，匹配效果越好；

二、PnP

PnP是求解3D到2D点对运动的方法。直接点说它是在已知3D空间点坐标与对应2D图像投影坐标的情况下，恢复图像姿态。在vslam中，在初始化获得了一系列3D点后，我们就可以使用PnP来估计后续姿态。相比于对极几何估计姿态，PnP方法需要更少的点对（3个以上），不需要对极约束，并且PnP方法可以避免尺度的问题。

这里介绍一个线性PnP算法：

一个点对（3D点X及其投影2D点x坐标）可以建立x=PX（在这里P是未知）；

P为11个自由度矩阵，故有6对点对使用DLT建立方程就可解得P；

当然PnP算法是一个大类，上面所讲只是一个概念的展示，想要深入的了解可以查找P3P，EPnP、UPnP。。。

三、ICP

ICP是求解3D到3D点的位姿估计问题。实际上是我们知道在两个相机坐标系下同一地图点的两个坐标，则可以建立一个欧式变换：

同PnP解法一样也存在线性解法（DLT、SVD）与非线性优化解法，这里就不再赘述了。