ORB_SLAM 源码解析 0

发表于 | 分类于 | | 次阅读

ORB-SLAM_源码解析

前言

整体输入：

  1. 图像+时间码(传感器融合)
  2. 相机内参(标定好的)
    -相机zoom不可改变， focus尽量不变
    -给定标定图像的大小，计算内参跟图像大小有关(如果对采集图像进行缩放处理，则内参相应缩放)
  3. 相关配置文件，如指定特征检测描述子，给出对应特征描述子的词汇表，具体内部设置的一些经验值等

关键类：

1. Tracking

  1. 跟踪要处理每帧图像数据
  2. 面向对象思想->封装
    -将获取的图像数据封装成帧，后续处理都是针对于帧处理
  3. 具体Frame的结构
  • Image->存储图像数据(输入)
  • Camera->存储相机内参信息(输入)
  • Feature->存储帧对应的特征数据(计算) ( 一对多的关系)
  • Mat->存储帧对应相机姿态(计算)

1.1 track

1.1.1 Track reference key frame

​ 初始化的时候根据参考关键帧估计当前帧位姿

1.1.2 Track with motion model

​ 根据运动模型，估计当前帧位姿

1.1.3 Track Local Map

​ 局部地图中的Map point找到了，跟上面一样，将这些Map Point进行投影到当前帧中，寻找匹配，确定3D-2D的关系

  1. 首先确定Map Point能否投影到当前帧上
  2. Map Point的世界坐标转到相机坐标系下，看点的深度是否为正
  3. Map Point投影到图像上，是否超出图像边界
  4. Map Point在深度范围内(每个Map Point产生的最大最小深度)
  5. 当前视图方向与MapPoint的平均视图方向的夹角大于60度

​ 总的来说，这三种跟踪(包括前面的两种)最终目标就是找到Map Point与当前帧的特征形成对应关系，通过PnP对当前帧的位姿进行求解。

1.2 重定位问题

​ 我们会遇到跟踪丢失的情况，那接下来我们需要移动镜头进行重定位

  1. 具体主要通过关键帧数据库进行查找，找出与当前帧有关系的关键帧，确定候选关键帧
  2. 对每一一个候选关键帧，通过Bow进行匹配，如果有足够的匹配(匹配特征数超过15个) ，则构建PnP进行求解，计算出当前帧的相机外参。
  • EPnP算法
  • 5次RANSAC迭代，计算相机外参
  • 优化帧，如果匹配特征数没有50个，则将候选关键帧对应的map point投影到当前帧继续寻找匹配，匹配完成之后再次优化帧，如果匹配数还是没有大于50 ，则更改投影匹配阈值，再次匹配，只有匹配的个数大于50 ，才说明重定位成功，最后对大于50个匹配对的帧再次对帧进行优化。

1.3 跟踪确定关键帧

​ 跟踪除了计算每帧的位姿，还有一个很重要的目的是确定关键帧
​ 何时添加关键帧，或者说什么是关键帧：

  1. 关键帧不要太稠密(相邻两个关键帧之间有一定的间隔，原作实现时大约是30帧的间隔)
  2. 当前的帧至少匹配到了50个Map Point
  3. 当前帧匹配到的Map Point的个数不能超过参考关键帧(与当前帧拥有最多map point的关键帧)对应Map Point的90%，说明这个时候参考关键帧可以替代当前帧，还没有必要创建关键帧
  4. 局部地图优化空闲的时候

2. Local Mapping

2.1 处理新关键帧

​ 每添加一个keyframe. ，维护Covisibility Graph ， Spanning Tree ， Mapl以及计算该帧的词袋表示确定匹配，为三角化做准备

2.2 生成新的Map Point

  1. 得到当前关键帧(cur_ keyframe)在Covisibility Graph中邻接的一些关键帧(共同的map point数单目设置20 )

  2. 对邻接的关键帧进行遍历 (每一个设为ref keyframe) ，在极线上进行搜索并三角化

    1. 基线( cur keyframe5ref keyframe )与ef keyframe对应的map point的深度均值比值不能太小，这样形成的3d点不够精确
    2. 对未匹配的特征点，首先通过orb的词汇树进行加速匹配

  3. 根据匹配点对，通过三角化计算3d点

    1. 检测三角化之后的点在相机前
    2. 在参考帧的重投影误差进行检测
    3. 对尺度进行检查

  4. 确定Map Point的相关属性(平均观察方向，观测距离，最佳描述子等)

    产生了新的Map Point之后r该Map Point可能会被其他关键帧找到，为之前帧添加对应关

系，或者该Map Point与其他关键帧之间创建的Map Point有交集，具体做法为：

  1. 根据covisibility grapb找到邻接关键帧(共同map point数20 )
  2. 遍历邻接关键帧，对每一个邻接关键帧再次计算有关系的关键帧(共同map point数5 )得到二级邻接关键帧
  3. 遍历当前帧的一级邻接和二级邻接的关键帧( each_ keyframe ) ，将当前帧对应的map point投影到each_ keyframe上，确定对应的特征是否已经有对应的map point ，如果有，我们选择map point对应观测量多的那个，没有则添加到each_ keyframe对应的特征上

2.3 Map Point剔除

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

2.4 关键帧剔除

  1. 根据Covisibility Graph确定局部关键帧，对所有局部关键帧都进行遍（each_keyframe）
  2. 对each_keyframe对应的每一个Map Point进行分析，与该Map Point对应的特征的个数大于3
  3. 如果each_frame上对应的Map Point能被其他至少3个关键帧观测到90%以上的话，就剔除它

2.5 local BA

​ 添加完了关键帧及对应mappoint，有了这些新的约束，进行一次局部bundle adjustment，具体做法:

  1. 根据Covisibility Graph确定局部关键帧，设为图节点
  2. 根据局部关键帧，确定局部的map point ，设为图节点
  3. 再找到那些可以看到局部map point的关键帧，但是不是局部关键帧的，到时在优化的时候，设置为fixed ，不进行优化
  4. 把局部map point中每一个map point与观察到该map point的所有关键帧构建边， 边的观测值为map point对应的特征坐标，边的信息矩阵，考虑误差是一个像素*尺度
  5. 去除一些不好的边，再次进行优化，即对keyframe对应的相机外参和mappoint的世界坐标系中的位置进行了修正

3. Loop Closing

​ 通过Local Mapping我们对KeyFrame的位姿和Map Point的位置进行了优化，但是实际计算出的位姿还是有误差的， 误差积累之后，则尺度不再统一，这就会产生尺度漂移问题，轨迹的误差也会越来越大，因此需要进行回环检测来解决它。

3.1 检测回环

Loop Candidates Detection

​ Local Mapping线程处理之后，在Loop closing插入关键帧cur_ keyframe ，则Loop Closing线程处理，首先闭环检测，具体处理:

  1. 根据Covisility Graph确定局部关键帧( each_ keyframe )
  2. 计算cur_ keyframe的词袋向量与所有each_ keyframe的词袋向量的相似度，得到最小相似度
  3. 在关键帧数据库中找到相似度不小于最小相似度的关键帧作为闭环的候选关键帧
  4. 一对每一 个候选关键帧检测其与之前回环的一致性。主要也就是通过连续三次的关键帧对应相同的候选闭环帧，则认为该帧为闭环帧。

3.2 计算相似变换

Compute the Similarity Transformation

  1. 找到了当前关键帧可能的候选闭环帧，计算当前关键帧与可能的候选闭环帧的匹配，根据特征的匹配，确定两帧之间map point的匹配(匹配数大于20 )
  2. 通过RANSAC计算Sim3 ，然后根据这个Sim3进行处理，优化所有的对应。
  3. 两帧进行相互投影各自根据自己对应的map point寻找对应帧的匹配，最后看两个匹配一致的数目。
  4. 优化重投影误差optimizeSim3
  5. 将闭环帧及其相邻帧对应的mappoint投影到当前帧上，这边用sim3 ，寻找更多的匹配
  6. 如果匹配数大于40 ，则认为计算成功

3.3 闭环融合

Loop Fusion
通过上一步计算，我们确定了闭环帧以及对应的Sim3，我们要做的是：

  1. 将当前关键帧对应的map point换成闭环帧对应的map point (这边都包括相邻的帧对应)
  2. 重新构建covisibility graph（考虑到环，需要更新相邻关键帧）

3.4 优化Essential Graph

在Essential graph上进行姿态图优化

  1. 将地图中的所有关键帧设为顶点，估计量为每个关键帧对应的Sim3 ，闭环帧不做优化
  2. 添加闭环边，当前帧的集合与闭环帧的集合构成图中的边，权重小于100的不考虑，边的观测值是两帧之间的相对变化量，信息矩阵是单位阵
  3. 添加正常边，遍历所有的关键帧，找到关键帧对应的父节点也就是Spanning tree的边，边的观测值是两帧之间的相对变化量，信息矩阵是单位阵，继续遍历当前帧所对应的闭环帧，如果闭环帧在当前帧之间，则添加边，继续对covisibilitygraph权重大于100构成边，最后优化

4. Viewer

给咱来个🍰，啾咪