自动驾驶（四十一）---------视觉SLAM（视觉里程计）

       视觉里程计主要介绍什么是特征点，如何提取和匹配特征点，以及如何根据配对的特征点估计相机运动。

1. ORB特征点

      FAST是一种角点检测方法：如果一个像素与它邻域的像素差别较大（过亮或过暗）, 那它更可能是角点。

      FAST：在图像中选取像素 p，假设它的亮度为 Ip；设置一个阈值 T(比如 Ip 的 20%)； 以像素 p 为中心, 选取半径为 3 的圆上的 16 个像素点； 假如选取的圆上，有连续的 N 个点的亮度大于 Ip + T 或小于 Ip − T，那么像素 p 可以被认为是特征点；循环以上步，对每一个像素执行相同的操作。

2. BRIEF描述子

      BRIEF：向量由许多个 0 和 1 组成，这里的 0 和 1 编 码了关键点附近两个像素（比如说 p 和 q）的大小关系：如果 p 比 q 大，则取 1，反之就 取 0。如果我们取了 128 个这样的 p, q，最后就得到 128 维由 0，1 组成的向量。

3. 特征匹配

      快速近似最 近邻（FLANN）算法更加适合于匹配点数量极多的情况。由于这些匹配算法理论已经成 熟，而且实现上也已集成到 OpenCV，所以我们这里就不再描述它的技术细节了。

4. 2D-2D: 对极几何

      假设我们从两张图像中，得到了一对配对好的特征点，需要恢复出在两帧 之间摄像机的运动。通过这些匹配好的点，存在一定的约束关系，最优化出两帧的运动信息：下面我们介绍对极几何约束：

                                                           

       连线 O1p1 和连线O2p2 在三维空间中会相交于点 P。这时候点 O1, O2, P 三个点可以 确定一个平面，称为极平面。O1O2 连线与像平面 I1, I2 的交点分别 为 e1, e2。e1, e2，称为极点，O1O2 被称为基线。称极平面与 两个像平面 I1, I2 之间的相交线 l1, l2 为极线，对极约束简洁地给出了两个匹配点的空间位置关系。

5. 3D-2D: PnP

      PnP是求解 3D 到 2D 点对运动的方法。它描述了当我们知道 n 个 3D 空间点以及它们的投影位置时，如何估计相机所在的位姿。在双目或 RGB-D 的视觉里程计中， 我们可以直接使用 PnP 估计相机运动，3D-2D 方法不需要使用对极约束，又可以在很少的匹配点中获得较好的运 动估计，是最重要的一种姿态估计方法。

      考虑某个空间点 P，它的齐次坐标为 P = (X, Y, Z, 1)T。在图像 I 中，投影到特征 点 x1 = (u1, v1, 1)T（以归一化平面齐次坐标表示）。

                                                                  

       用最后一行把 s 消去，得到两个约束：

      令：得到：和

      假设一 共有 N 个特征点，可以列出线性方程组： ，即可实现矩阵 T 的线性求解。

6. 3D-3D: ICP

       假设我们有3D点之间的匹配关系，求欧氏变换 R, t，在激光 SLAM 中碰到 ICP，由于激光数据无从知道点集之间的匹配关系，只能认为距离最近的两个点为同一个， 所以称为迭代最近点。而在视觉中，特征点为我们提供了较好的匹配关系，所以问题就变得更简单了。线性代数的求解（主要是 SVD），以及非线性优化方式的求解。

      1. SVD:  定义第 i 对点的误差项： 求使误差平方和达到极小的 R, t：

         a. 计算两组点的质心位置 p, p ′，然后计算每个点的去质心坐标：

         b. 根据以下优化问题计算旋转矩阵：

         c. 根据第二步的 R，计算 t：

       2. 非线性优化方法:  极小化误差平方和：

          使用李代数扰动模型即可：在非线性优化中只需不断迭代，我们就能找到极小值。