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。请注意，我们知道的是A; B; C 在世界坐标系中的坐标，需要求得是在相机坐标系中的坐标。

注意这些方程中的已知量和未知量。由于我们知道 2D 点的图像位置，三个余弦角cos ⟨a; b⟩ ; cos ⟨b; c⟩ ; cos ⟨a; c⟩ 是已知的。同时， u = BC²/AB²; w = AC²/AB² 可以通过A; B; C 在世界坐标系下的坐标算出，变换到相机坐标系下之后，并不改变这个比值。该式中的 x; y 是未知的，随着相机移动会发生变化。因此，该方程组是关于 x; y 的一个二元二次方程（多项式方程）。
该方程最多可能得到四个解，但我们可以用验证点来计算最可能的解，得到 A; B; C 在相机坐标系下,的 3D 坐标。然后，根据 后一帧中相机坐标系下3D点-与前一帧中世界坐标系下3D 的点对匹配算法，计算相机的运动 R; t。

3) 3D-3D估计 ICP问题

假设我们有一组配对好的 3D 点（比如我们对两个 RGB-D 图像进行了匹配）。

P = {p₁ … p_n}; P ′ = {p′ ₁ … p′ _n}

现在，想要找一个欧氏变换 R; t，使得：
对于任意i，有
p_i = R*p′ _i + t
这个问题可以用迭代最近点（Iterative Closest Point, ICP）求解。读者应该注意到，3D-3D 位姿估计问题中，并没有出现相机模型，也就是说，仅考虑两组 3D 点之间的变换时，和相机并没有关系。因此，在激光 SLAM 中也会碰到 ICP，不过由于激光数据特征不够丰富，我们无从知道两个点集之间的匹配关系，只能认为距离最近的两个点为同一个，所以这个方法称为迭代最近点。而在视觉中，特征点为我们提供了较好的匹配关系，所以整个问题就变得更简单了。在 RGB-D SLAM 中，可以用这种方式估计相机位姿。

其实本质问题变成了，求最小二乘法优化问题。