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,)是经过不太精确的位姿，投影到当前帧的投影点。

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