自动驾驶（八）---------基于视觉的SLAM

基于视觉的SLAM

按照传统做法分成以下几块：

1. 前端：特征提取，配准。 2. 中间：计算变换矩阵初值。3. 后端：多帧误差最优化。4. 回环优化

1. 前端：特征提取，配准。

       图片特征提取匹配有很多种：

       1. SIFT：大名鼎鼎，尺度不变特征变换，但是速度太慢。

       2. SUFT：在SIFT的基础上优化，基于Hessian矩阵和一阶Haar小波，但还是慢。

       3. FAST：只检测像素的灰度值，没有方向和尺度信息。

       4. ORB：改进了FAST，使用非最大值抑制，使用图像金字塔保证尺度不变性，灰度质心法保证旋转不变性。

       要满足实时性，一般采用ORB的方法，提取完特征后就是配准，opencv提供一个暴力配准的方法：BFMatcher 特征向量两两一一比较，得到匹配关系。

       这样得到的匹配关系很乱，一般采用RANSAC的方法剔除异常匹配：

        1.通过匹配点计算两个图像之间单应矩阵（findHomography），然后利用重投影误差来判定某一个匹配是不是正确的匹配。

        2.剔除异常匹配，用剩下的匹配计算新的单应矩阵，重新判断所有匹配的正确性，一至迭代到稳定的结果。

2. 中间：计算变换矩阵初值。

        使用第一步计算的匹配关系计算两帧之间的坐标变换，这里与ICP不同的，匹配关系都叠加上了摄像机成像部分，并没有三维坐标的匹配关系，不过没关系，opencv已经帮我们想好了。

        假设我们提前已经标定好相机，详情见opencv张正友标定。一对匹配的像点之间存在这某种约束关系，这种约束被称为两视图的对极约束。

                            

       1. 两个相机中心的连线CC'称为基线.

       2. X e e' x x' 五点共面。

       3. 存在从点x到对极线l′的变换（基础矩阵）

       4. 通过匹配点的关联，我们能知道x x'的对应关系，由此得出基础矩阵。

       设以第一个相机作为坐标系三维空间的点P=[X,Y,Z]，其在两个相机的像点分别为p1,p2。由于第一个相机的中心作为世界坐标系的原点，也就是说第一个相机没有旋转和平移，通过小孔相机模型可得：

                                                                 

        K是相机的内参，带入P得：                

        两边同时左乘K−1K−1，得到               

                  

        两边同时左乘向量tt的反对称矩阵，消除t       

        两边再同时左乘                               

                 

        再将x1,x2 换掉                                       

        上式是对极约束的另一种表示，该式子中仅包含像点，相机的旋转和平移，中间的矩阵就是基础矩阵F。

                                                                      

       提取中间的矩阵可到                           

       E被称为本质矩阵，其和基础矩阵相差相机的内参K。

       也就是说，仅通过匹配的点对（最少7对）可以计算出两视图的基础矩阵F，然后再从F中分解得到相机的位姿势。

3. 后端：多帧误差最优化。

       通过上面的计算，可以得到相机两两之间的位置关系，但是由于误差累计，肯定会导致出现较大的偏差。

      为此我们可以通过全局优化的方式，进行继续优化。

      对于多帧而言：

                                                              

       i = [0,n] 为每两帧之间的匹配点，j = [0,m] 为对应不同的帧

       为了求所有对应的R 和T，我们求出一个全局最优解：

       为此需要用梯度下降的方法，这要求我们对自变量R和T求偏导，但是R是一个3自由的矩阵，为此需要把R变换到李群代数中：