高翔视觉SLAM十四讲学习笔记-第7讲视觉里程计（♥重点章节♥）

---

学习任务

1. 理解图像特征点的意义, 并掌握在单幅图像中提取出特征点，及多幅图像中匹配 特征点的方法。
2. 理解对极几何的原理，利用对极几何的约束，恢复出图像之间的摄像机的三维运 动。
3. 理解PNP 问题，及利用已知三维结构与图像的对应关系，求解摄像机的三维运 动。
4. 理解ICP 问题，及利用点云的匹配关系，求解摄像机的三维运动。
5. 理解如何通过三角化，获得二维图像上对应点的三维结构。

7.1 特征点法

• 路标
  
• 特征点
  

举个栗子ORB

BRIEF

7.2 2D-2D 对极几何

E共有五个自由度：也就是说至少有五对点就可以把E算出来，称为五点法但是比较麻烦，所以把它当成普通矩阵用八对点把E求出来。

八点法求矩阵E（Essential）

八对点构成的方程组8×9的矩阵，相当于AX=0的情况，解出A来就是要求的矩阵E

从E计算R,t:奇异值分解

使用H恢复R，t（八点法共面时）

小结

7.3 三角测量

在单目SLAM 中，仅通过单张图像无法获得像素的深度信息，我们需要通过三角测量（Triangulation）（或三角化）的方法来估计地图点的深度。

三角测量的定义

三角测量是指，通过在两处观察同一个点的夹角，确定该点的距离。在SLAM 中，我们主要用三角化来估计像素点的距离。

三角法求深度原理

7.4 3D-2D PnP（♥重要的姿态估计方法♥）

PnP定义

PnP（Perspective-n-Point）是求解3D 到2D 点对运动的方法。它描述了当我们知道n 个3D 空间点以及它们的投影位置时，如何估计相机所在的位姿。

如果两张图像中，其中一张特征点的3D 位置已知，那么最少只需三个点对（需要至少一个额外点验证结果）就可以估计相机运动。特征点的3D 位置可以由三角化，或者由RGB-D 相机的深度图确定。因此，在双目或RGB-D 的视觉里程计中，我们可以直接使用PnP 估计相机运动。而在单目视觉里程计中，必须先进行初始化，然后才能使用PnP。3D-2D 方法不需要使用对极约束，又可以在很少的匹配点中获得较好的运动估计，是最重要的一种姿态估计方法。

PnP问题求解方法

PnP 问题有很多种求解方法，例如用三对点估计位姿的P3P[45]，直接线性变换（DLT），EPnP（Efficient PnP）[46]，UPnP[47] 等等）。此外，还能用非线性优化的方式，构建最小二乘问题并迭代求解，也就是万金油式的Bundle Adjustment。我们先来看DLT，然后再讲Bundle Adjustment。

[45] X.-S. Gao, X.-R. Hou, J. Tang, and H.-F. Cheng, “Complete solution classification for the perspective-three-point problem,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 25, pp. 930–943, Aug 2003.
[46] V. Lepetit, F. Moreno-Noguer, and P. Fua, “Epnp: An accurate o(n) solution to the pnp problem,” International Journal of Computer Vision, vol. 81, no. 2, pp. 155–166, 2008.
[47] A. Penate-Sanchez, J. Andrade-Cetto, and F. Moreno-Noguer, “Exhaustive linearization for robust camera pose and focal length estimation,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 35, no. 10, pp. 2387–2400, 2013.

7.4.1 直接线性变换（DLT）

7.4.2 P3P

即：利用三对点求相机外参
它仅使用三对匹配点，对数据要求较少，推导请参考文献【49】

[49]iplimage, “P3p(blog).” “http://iplimage.com/blog/p3p-perspective-point overview/”, 2016.

P3P 需要利用给定的三个点的几何关系。它的输入数据为三对3D-2D 匹配点。记3D点为A，B，C，2D 点为a， b， c，其中小写字母代表的点为大写字母在相机成像平面上的投影。
P3P 还需要使用一对验证点，以从可能的解出选出正确的那一个（类似于对极几何情形）。记验证点对为D-d，相机光心为O。请注意，我们知道的是A，B，C 在世界坐标系中的坐标，而不是在相机坐标系中的坐标。一旦3D 点在相机坐标系下的坐标能够算出，我们就得到了3D-3D 的对应点，把PnP 问题转换为了ICP 问题。（因为图中OA的长度是未知的）

7.4.3 PnP 优化解法 Bundle Adjustment

前面说的线性方法，往往是先求相机位姿，再求空间点位置，而非线性优化则是把它们都看成优化变量，放在一起优化。这是一种非常通用的求解方式，我们可以用它对PnP 或ICP 给出的结果进行优化。

7.5 3D-3D ICP

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

ICP的求解有两种方法：利用线性代数的求解（主要是SVD），以及利用非线性优化方式的求解（类似于Bundle Adjustment）。

7.5.1 SVD 方法

带匹配的



不为零只是无关