SVO深度解析(二)之跟踪部分

本篇博客总结了SVO的前端跟踪部分

转载请说明出处：

http://blog.csdn.net/zhubaohua_bupt/article/details/74910568

2 跟踪部分

跟踪部分主要干了两件事

<1>经过图像对齐，计算一个粗糙的位姿

<2>创建一个当前帧的局部地图，根据地图，对位姿进一步优化。

2.1图像对齐

图像对齐的目的是迭代计算帧间位姿。

用于图像对齐的地图点是上一帧所能看到的地图点，按先验知识来说，图像帧间变换比较小，

咱们有理由相信上一帧和当前帧所能看到场景大部分相同。

其过程以下，

1找到前帧（K-1）看到的地图点p1,p2,p3。

2投影至后帧（k）二维图像上，

3而后最小化灰度偏差函数（这是一个最优化过程，又称图像对齐），获得位姿，over。

 

2.2 经过局部地图对当前帧位姿优化

虽然上一帧能看到的地图点和当前帧已经重合大部分，但仍有一部分当前帧能看到的视野，

上一帧看不到（比图下图p1,p2,p3,p4）。咱们知道，当前帧和以前看到的视野重合率越高，

优化的位姿就越趋于准确，那么，除了上一帧，有没有其余帧和当前帧视野重合呢？

确定是有的，那把这些帧(以下图Ir1和Ir2)都扒出来

（一般是关键帧,由于这些帧既保证视野重复率不是过高，并且位姿相对与普通帧较准确），

用于优化当前帧位姿。这就是本步骤要干的事情。

这些帧称之为局部关键帧，其看到的地图点集合称之为局部地图。

 

2.2.1 局部地图的构建

2.2.1.1 五点的构建

SVO采用的5点法来构建局部地图，思想是构建关键帧的时候，顺便在关键帧图像上找到5个特征点，

分布以下

 

上图中，红色点表明提取的普通特征点，蓝色点表明5点法提取的特征点，

看到这里，5点怎么提取应该一目了然了吧，即提取左上、左下、右上、右下和中间的特征点做为5点。

其实如今Frame.cpp里setKeyPoints()，checkKeyPoints（）函数有个错误，稍微看一下就能看到。

 

2.2.1.2 局部地图的构建

当前帧以前的每个关键帧都有5点分布，那么选哪些关键帧做为局部优化关键帧呢？

也就是5点在筛选局部关键帧时有啥用呢？

SVO这样用：把每一个关键帧上的5点投影至当前帧，只要投影上任何一个，

就把对应关键帧以及关键帧与当前帧的距离记录下来，

而后，按距离排序，取前n个关键帧，做为局部关键帧，关键帧上的地图点为局部地图。

实如今Reprojector.cpp里，具体位置以下

2.2.2优化位姿

这里先说一下一个概念：

重投影偏差：地图点P在当前帧的投影点p1和P与当前帧匹配的像素点p2之间的几何像素距离。

优化位姿的前提是找到局部地图点与当前帧像素的匹配关系，

而后，利用匹配关系，经过缩小的地图点的重投影，来优化位姿。

2.2.2.1 地图点是什么，与每一帧的关系是什么？

地图点一般又叫作地标点（landmark），它是在关键帧中提取的特征点，被赋予深度后的三维点。

在RGBD和双目的SLAM里，关键帧中提取的特征点伴随着深度信息，通过筛选直接就能放入地图。

可是，在单目SLAM里，关键帧中刚提取的特征点，并无深度信息，须要通过一段时间的不断估计，

认为深度比较准确后，才把该特征点对应的三维点放入地图。

也就是说，地图点的来源是关键帧，

做用： 1 优化帧的位姿（缩小重投影偏差函数）

            2 创建稀疏地图

那么，地图点与普通帧和关键帧有没有其余联系呢？

实际上，在VSLAM工做一段时间后，不管是双目，RGBD仍是单目，

图像帧的位姿通常都是经过PNP来估计的，PNP的过程可表述为，

三维地图点到当前帧的投影，而后创建偏差函数（直接法：灰度，特征点法：距离）来优化位姿。

那么，地图点与帧（包括普通帧和关键帧）的关系就是，用地图点估计并优化该帧的位姿。

2.2.2.2 地图点和当前帧找匹配关系

对于局部地图里的每一个地图点P，都被当前帧（CF）以前的多个关键帧KFS看到过，

也就是说，对于一个地图点P,咱们能找到观测到它的全部关键帧集合。

 

代码里，存储在Point的obs_数据结构里面。

 

<1>和谁匹配？

匹配不但须要P的坐标，并且还须要P的描述，用来计算类似度（要否则怎么知道是否匹配上了呢）。

对于每个地图点P，都关联着许多关键帧（或者说，许多关键帧都看到过它），

也就是说，点P的描述能够来源于其中的任何关键帧。那么，选择哪一个关键帧上的描述和当前帧匹配呢？

SVO是这样作的：选择观测角度（地图点与不一样时刻相机的光心连线）与当前帧比较小的那个关键帧，

做为匹配描述。

这个作法也很好理解，毕竟夹角越小，两帧图像位姿差别越小，

地图点在两幅图像上（一个是关键帧，另外一个是当前帧）的描述越类似，越容易匹配成功嘛。

<2>怎么匹配？

匹配策略：网格法匹配。

为何要网格法匹配？

咱们知道，理论上，三个不在一条线上的点能肯定一个平面。

实际上，在SLAM；里，地图点都是有偏差的，若是这三个点离的很近，

那么肯定的平面精度没有离得远时肯定的平面准确，

 

在SLAM中，这个平面就是图像平面。若是咱们能保证地图点在当前帧图像平面上投影的比较均匀，

那么，用这些地图点估计的位姿也就比较准确。

怎么进行网格法匹配？

在当前帧上画一个虚拟的网格，统计每一个网格里投影上的地图点，而后把地图点按质量排序。

对于某个网格，按地图点质量从前日后匹配，只要匹配成功一个，

此网格就不接受其余地图点的匹配了，这样就保证了地图点在当前帧上的均匀匹配。

实如今reprojector.cpp里的reprojectMap（）函数里。

 

 

匹配方式：特征对齐（feature alignment）

首先，找到选择观测角度最小的关键帧；

而后，计算两帧之间的仿射矩阵；

其次，找到关键帧中最适合匹配的那一层（已经有图像金字塔）

最后，用LK光流法完成地图点与图像像素点的匹配（实如今feature_alignment.cpp里）。

匹配不是在极线上搜索的。

匹配的原理以下，

 

用图像块计算灰度偏差没什么好说的，光流嘛，注意这里，

因为两帧（参与匹配的关键帧和当前帧）有位姿差别，两个匹配窗口须要通过仿射变化（Ai）。

2.2.2.3 优化位姿

经过以上步骤，就找到了局部地图点与当前帧的匹配关系。接下来，根据匹配关系，优化位姿。

 

优化位姿的思想是缩小重投影偏差函数，偏差函数如(14)。

其中ui是匹配点，π(T,)是通过不太精确的位姿，投影到当前帧的投影点。

当偏差函数最小时，认为此时的位姿最精确。