SLAM学习——视觉里程计中位姿估计，PnP、ICP问题的本质描述

SLAM中的前端也称为视觉里程计，本质在于利用计算计算机视觉图像中的特征点的距离，进而估算相机的旋转R和位移t，即计算相机的姿态变化。

根据采集的相机的特点不同，可以分成一下几类：

• 2D-2D 单目摄像机，也称为对极几何问题
• 3D-2D 已知特征点的3D世界坐标和图片中的2D坐标，通常是RGBD相机/双面相机拍摄的图片，该问题可以使用PnP来求解
• 3D-3D 已知

下面分类介绍：

1）2D-2D对极几何问题

2）3D-2D PnP问题

PNP(Perspective N Points)问题首先于1981年由Fishler和Bolles提出，类似的问题可以追溯到1841年摄影测量学领域。所谓PNP问题，就是指如下的物体定位问题:假定摄像机为小孔模型且已标定好，摄取一幅在物体坐标系下坐标已知的N个空间点的像，且这N个图像点的坐标已知,确定这N个空间点在摄像机坐标系下的坐标。
在SLAM中，对于匹配的特征点来说，前一帧中的坐标存在于我们已经构建好的地图中，相当于世界坐标系坐标。后一帧中对特征点重新拍摄了，获取了2D坐标，PnP需要计算特征点在后一帧中的3D坐标，

这个算法的输入是一组点的3D世界坐标和2D像素坐标,输出是相机的旋转矩阵®和平移矩阵(t)，比较经典的是P3P算法，即利用3对特征点来求相机位姿。

P3P 需要利用给定的三个点的几何关系。它的输入数据为三对 3D-2D 匹配点。记 3D点为 A; B; C， 2D 点为 a; b; c，其中小写字母代表的点为大写字母在相机成像平面上的投影，如图 所示。此外， P3P 还需要使用一对验证点，以从可能的解出选出正确的那一个（类似于对极几何情形）。记验证点对为 D − d，相机光心为 O。