重拾SLAM

1 跟踪

1.1 初始化

2D-2D计算位姿，尺度不确定

3D-2D统一尺度

根据特征点共面与非共面的约束关系不一致，共面计算单应性矩阵，非共面计算基础矩阵，根据约束关系计算误差，根据比值选择某种约束

1.2 跟踪

1. 跟踪参考帧（根据词袋模型计算匹配，然后PNP求解得到当前帧的位姿）
2. 跟踪运动模型（根据上一帧的姿态计算当前帧的位姿，将上一帧的MapPoint投影到当前帧，在某一半径内寻找匹配点，然后PNP求解）
3. 跟踪局部地图（前面两种方法是帧与帧之间的联系，这里是通过更多的3D-2D的匹配对位姿进行约束【covisibility graph  局部关键帧：与当前帧拥有共同MapPoint（大于10）的关键帧】）

1.3重定位

1. 遇到跟踪丢失情况，需要移动镜头重定位
2. 具体主要通过关键帧数据库查找，找出与当前关键帧有关的关键帧，确定候选关键帧
3. 对每一个候选关键帧，通过BoW匹配，如果有足够多的匹配（匹配特征数大于某一阈值 15），构建PNP求解当前帧的位姿

• EPNP算法
• 5次RANSAC迭代，计算相机外参
• 优化帧：若匹配特征数小于50，则将候选关键帧的mappoint投影到当前帧继续寻找匹配，若匹配数没有大于50，则更改投影匹配阈值，再次匹配，只有匹配大于50才说明重定位成功，最后对大于50匹配对的帧再次对阵进行优化

1.4确定关键帧

• 关键帧不要太稠密（关键帧之间要有一定的间隔）
• 当前帧至少匹配到了50个MapPoint
• 当前帧匹配到的MapPoint个数不能大于参考关键帧对应MapPoint的90%，不然参考关键帧可代替此关键帧
• 局部地图优化空闲的时候

2Local Mapping（等待关键帧的出现）

2.1添加关键帧

每增加一个KeyFrame，维护从visibility graph，Spanning tree ,Map 以及该帧的词袋表示的匹配，为三角化做准备

2.2新增Map Point

1. 得到当前关键帧cur_keyframe在covisibility graph中邻接的一些关键帧（MapPoint数目为20）ref_keyframe
2. 在ref_keyframe 上在级线上搜索并三角化（基线（cur_keyframe与ref_keyframe与ref_keyframe对应的MapPoint的深度均值不能太小；对未匹配的特征点通过orb词汇数进行加速匹配））
3. 根据匹配点对，通过三角化计算3d 点（检查三角化之后的点在相机前，在参考帧上的重投影误差进行检查，对尺度进行检查）
4. 确定MapPoint的属性（平均观察方向、观测距离、最佳描述子等）
5. 产生了新的MapPoint之后，该点可能被其他关键帧观测到，为之前帧添加对应关系

• 根据covisibility graph 找到邻近关键帧（mappoint =20）
• 遍历邻近关键帧，对每一个关键帧再次计算有关系的关键帧（mappoint=5）得到二级邻接关键帧
• 遍历当前帧的一级和二级邻接关键帧（each_keyframe），将当前帧的mappoint投影到each_keyframe，，确定对应的特征是否已经有对应的mappoint，如果有选择mappoint对应观测量多的那个帧作为该mappoint的对应帧，没有则添加到each_keyframe对应的特征上

2.3 剔除Map Point

1. 检测该点是否为坏点（观测到该点的关键帧的数目小于3），坏点直接删除
2. 能观测到该点的帧应该不小于理论上观测到该点的帧的1/4 （理论上能观测到该点表示该点能投影到局部关键帧上的个数； 实际上能观测到该点表示优化后（该点可能为外点）能投影到的个数）
3. 从创建该Map Point 开始到现在过去了至少两个关键帧，但是观测到该点的关键帧不超过两个

2.4KeyFrame的剔除

目的 减少存储压力和Bundle Adjustment 压力

根据covisibility Graph 确定局部关键帧，对所有的局部关键帧遍历（each_keyframe）；对each_keyframe所对应的每一个MapPoint（each_map_point）分析，与该each_map_point对应的特征个数大于3【总结：each_keyframe上对应的MapPoint能被其他至少其他三个关键帧观测到90%以上】

3 Loop closing 

3.1 检测回环

Local Mapping线程结束以后，在Loop closing插入关键帧cur_keyframe，则loop closing 线程开始处理

1. 根据covisibility Graph 计算局部关键帧each_keyframe
2. 计算cur_Keyframe的词袋向量与所有each_keyframe的词袋向量的相似度，得到最小相似度
3. 在关键帧数据库中查找相似度不小于最小相似度的关键帧作为闭环候选关键帧
4. 对每一个候选关键帧检查其与之前回环的一致性（连续三次的关键帧对应相同的候选闭环帧，则认为该帧为闭环帧）

3.2 计算相似变换

1. 计算当前关键帧与所有闭环候选关键帧的特征匹配，根据特征的匹配，确定两帧之间的MapPoint 的匹配（匹配数大于30）
2. 通过RANSAC计算相似变换Sim3，然后根据这个SIm3 优化所有的对应
3. 两帧之间互相投影，根据各自对应的MapPoint寻找对应帧的匹配，最后看两个匹配一致的数目
4. 优化重投影误差
5. 将闭环帧及其相邻帧对应的MapPoint投影到当前帧（用sim3），寻找更多的匹配,若匹配大于40 则计算成功

3.3 闭环融合

通过上一步骤我们确定了闭环帧及对应的Sim3

1. 将当前关键帧对应的MapPoint换成闭环帧对应的MapPoint（这里认为闭环帧对应的MapPoint要更加准确）
2. 闭环帧相邻帧和当前关键帧相邻帧也要对应
3. 重新构建从visibility graph

3,4 优化Essential  Graph

1. 将地图上所有的关键帧设为顶点，估计量为每个关键帧对应的Sim3,闭环帧不优化
2. 添加闭环边，当前帧的集合和闭环帧的集合构成图中的边（权重小于100）的不考虑，边的观测值是两帧之间的相对变化量，信息矩阵是单位阵
3. 添加正常边，遍历所有的关键帧，找到关键帧对应的父节点（Spanning tree的边），边的观测值是两帧之间的相对变换量，信息矩阵是单位阵，继续遍历当前帧对应的闭环帧，如果闭环帧在当前帧之间，则添加边，继续对covisibility graph 权重大于100构成边，最后优化
4. 总结（把Essential Graph做一次全局优化，把Sim3 带有的误差平均分到环路的各个关键帧）
5. 最后再跑一次全局的Bundle adjustment(将地图中的所有KeyFrame设置为图顶点，所有MapPoint作为顶点，将MapPoint对与观测到该MapPoint的KeyFrame构成边，边的观测值是MapPoint在KeyFrame中对应特征的位置，边的信息矩阵，考虑误差是一个像素*尺度)

参考知识

高博http://www.cnblogs.com/gaoxiang12/

深蓝https://mp.weixin.qq.com/s/mcdgZghgACpV15WXHcRhcw

https://mp.weixin.qq.com/s/5dG6I8fIA-LXaCNE6iXGZw