模块
- Tracking 前端追踪
- LocalMapping 建图、优化
- LoopClosing 回环检测
System.cc 系统控制
- 实现System类
- 构造函数
- 线程初始化、开启与相互关联
- LocalMapping 局部地图和Local BA
- LoopClosing 回环检测
- 功能类初始化
- Tracking 前端追踪
- Map 存放关键帧和地图点
- KeyFrameDatabase 用于闭环检测和重定位
- 线程初始化、开启与相互关联
- TrackMonocular函数
- 单目
- 被main函数循环调用的函数
- 主要通过调用Tracking类的GrabImageMonocular实现功能
- TrackStereo
- 双目
- TrackRGBD
- RGBD
Tracking.cc 前端追踪
- 实现Tracking类
- GrabImageMonocular函数
- 转为灰度图像
- 用图像构造帧,即Frame类
- Frame类
- 构造函数
- 设置帧ID
- 调用ORBextractor类提取当前帧ORB特征,均匀化策略为Bucket
- 初始化时待提取的特征点数量设置为其余时候的二倍
- 特征点去畸变并保存去畸变2D坐标
- 双目则继续计算双目匹配,记录右目横坐标和恢复的深度
- 单目填充右目坐标和深度为-1
- 网格化记录,便于查找图像某个区域内的特征
- 构造函数
- Track函数
- 初始化,调用MonocularInitialization函数
- 特征点数大于100时,将该帧设置为初始化参考帧,等待下一帧进来进行匹配
- 若下一帧点数不到100,删除参考帧,重新开始初始化
- 两帧特征匹配,若匹配数不足100,删除参考帧,重新开始初始化
- 匹配成功后,调用Initializer类Initialize函数进行三角化,然后设置两帧位姿、初始化地图
- Initializer类Initialize函数
- 用匹配点进行RANSAC,同时计算F和H
- 用F或H恢复两帧间R、T,T无尺度,归一化
- 三角化地图点
- CreateInitialMapMonocular函数
- 设置用于初始化的两帧为关键帧,加入地图
- 将三角化的地图点加入地图、加入关键帧、对地图增加观测帧
- 更新共视关系和Spanning Tree(KeyFrame类UpdateConnections函数)
- 对刚生成的地图进行Global BA(Optimizer类)
- 初始化尺度,保证地图点中位数深度为1,并用设置好的尺度更新位姿和地图点
- 更新最新关键帧的参考关键帧,更新地图的参考关键点
- 当上帧追踪成功,继续追踪,估计当前帧位姿时,有两种追踪方式,都是用于建立当前帧与地图点之间的2D-3D匹配
- TrackWithMotionModel
- 优先选择这种方法
- 更新上一帧位姿
- 用相邻帧位姿匀速外推,设置当前帧位姿
- 上一帧地图点投影到当前帧,在周边范围内找2D-3D匹配点,若匹配成功数太少,增加邻域范围,再次搜索;若再次失败,则返回失败
- 优化当前帧位姿,对于当前帧变成了外点的地图点,更新状态
- TrackReferenceKeyFrame
- 若零速或刚刚进行过重定位,选择这种方法
- 用上一帧位姿设置当前帧位姿
- 和**参考关键帧(即上一个关键帧)**特征匹配,BoW加速
- 优化当前帧位姿,更新外点状态
- TrackWithMotionModel
- 当追踪失败,无法估计当前帧位姿时,Relocalization函数重定位
- 计算当前帧BoW,在关键帧词袋库中找候选匹配帧
- 对每个候选帧进行BoW加速特征匹配
- 对匹配数量超过20的候选帧,PnP估计位姿
- 将估计位姿代入优化,内点数少时,尝试放宽投影匹配的邻居半径范围,再次匹配、代入优化,直到找到符合足够内点数量的候选帧,退出
- TrackLocalMap
- 估计出当前帧位姿时,进一步寻找和局部地图的联系,找到更多当前帧与地图的2D-3D匹配,refine
- 这里的LocalMap和局部地图线程没关系,这里是Tracking为了进行PnP而自己构建2D-3D点的关联
- 构建关联:首先把与当前帧3D地图点有共视的关键帧加入到局部关键帧,把局部关键帧包含的地图点都加入局部地图点
- 上一步过程中找到最多共视帧,作为参考关键帧
- 投影、匹配
- 优化当前帧位姿
- 对于作为优化结果内点的3D地图点,更新其观测数
- 若对地图追踪成功
- 更新速度预测
- 去掉优化后为外点的地图点
- 检查是否满足插入关键帧的条件,若满足则插入新关键帧
- 距离上一帧关键帧经过了足够多帧数
- 局部建图线程空闲
- 等等
- 新关键帧插入
- 创建关键帧
- 更新参考关键帧
- 若为双目或RGBD,则双目三角化,三角化近处点并插入局部地图
- 新关键帧插入局部地图
- 初始化,调用MonocularInitialization函数
LocalMapping.cc 后端
- 功能
- 建图,三角化、将关键帧插入Map(除了初始化三角化和插帧到Map由Tracking完成,其余时候都经由LocalMapping来完成)
- 维护共视关系
- 优化
- CheckNewKeyFrames函数
- 检查是否有新关键帧被加入队列
- ProcessNewKeyFrame函数
- 作用:维护共视关系,维护地图点观测信息
- 取出队首关键帧
- 获取该关键帧的地图点
- 更新地图点观测信息、更新地图点描述子
- 调用KeyFrame的UpdateConnections函数更新共视关系、Spanning Tree
- 将关键帧插入地图中
- MapPointCulling函数
- 对刚加入的地图点(这种情形出现在双目,在ProcessNewKeyFrame加入了双目三角化的地图点)检查,目标是剔除掉跟踪成功率太低的地图点
- CreateNewMapPoints函数
- 作用:三角化
- 检索共视程度最高的n帧关键帧
- 该关键帧和n帧中的每一帧都进行三角化
- 三角化时,检测基线长度,太短则跳过
- BoW搜索获取该关键帧与待匹配关键帧的匹配,过程中借助极线距离滤除误匹配,并跳过在待匹配帧中已经三角化的点
- 三角化,并进行视差检测,太小则跳过
- 三角化后进行重投影误差校验、尺度一致性校验
- 校验通过后更新地图点观测,将地图点加入关键帧和地图
- SearchInNeighbors函数
- 作用:地图点融合
- 检索共视程度最高的n帧关键帧和二级邻居关键帧,作为融合候选帧
- 循环,对所有候选帧,调用ORBmatcher类Fuse函数
- 将该帧地图点重投影到融合候选帧上
- 若地图点距离相机太近/视角太大则跳过
- 和投影后坐标一定范围内的特征点匹配,找到最近的匹配,即待融合点
- 若待融合点已经是3D点,且其观测数不如新的点的观测数多,则替换它
- 若待融合点还不是3D点,则新增3D点
- 再将候选帧地图点都投影到当前帧上来融合
- 融合后更新共视关系等
- Optimizer类LocalBundleAdjustment函数
- 找当前关键帧的共视帧作为局部关键帧
- 加入所有共视帧所观测到的地图点作为局部地图点
- 找到观测到局部地图点且不是局部关键帧的那些关键帧,固定
- 局部关键帧位姿作为顶点加入,不固定
- 固定关键帧位姿作为顶点加入,固定
- 局部地图点坐标作为顶点加入,设置边缘化
- 加入边,将相机位姿和地图点坐标关联起来,为边设置观测量即2d坐标,为边设置信息矩阵,设置内参
- 第一次BA求解
- 卡方检测和正深度检测,剔除外点,设置下次不优化
- 第二次BA求解,由于有外点剔除,这次设置不使用核函数
- 第二次外点剔除:如果超限,直接去掉该地图点与该相机之间的关联
- 更新优化结果
- 该帧加入LoopCloser
LoopClosing.cc 回环
- 检查队列中是否有新一帧
- DetectLoop函数 检测回环
- 将当前关键帧加入数据库
- 如果距离上次回环检测未超过10帧关键帧,退出回环检测
- 获取共视关键帧,计算共视关键帧与当前帧的最小BoW距离,作为阈值
- 通过数据库检测得分超过阈值的候选回环帧
- 对每个候选关键帧,检测其共视关键帧,将其共视关键帧和起本身作为子候选组
- 遍历之前的所有子候选组,对于之前的单个子候选组,遍历当前子候选组中的每一帧,确认当前子候选组中是否有关键帧存在于之前的子候选组中,若有,则当前子候选组连续性为之前子候选组得分加一,并将当前子候选组加入当前记录;若没有,连续性设置为0,加入记录
- 若连续性得分达到阈值,认为发生回环
- 用当前子候选组更新之前的子候选组
- 连续性体现了时序层面上的回环概率,如果在当前连续几帧都在某一个子候选组中有候选回环帧,那么这个子候选组的连续性将被多次累加,分数会比较高。这里体现需要多个关键帧的加入,才能确认回环发生。
- ComputeSim3函数
- 回环发生时执行
- 对于每个连续性达标的候选回环关键帧,和当前关键帧用BoW加速特征匹配
- 若特征匹配数足够,Sim3Solver计算Sim3变换
- 若上述计算有效,得到Sim3变换,进一步用Sim3变换进行辅助特征匹配,得到更准确的匹配结果,进行Sim3优化,对上一步结果refine
- 若上一步内点数足够,计算完成,退出候选关键帧的循环
- 获取回环帧的共视关键帧,进一步获取共视关键帧观测到的地图点,把这些地图点的集合和当前关键帧进行匹配(投影加速),若有足够的匹配数量,回环检测成功
- CorrectLoop函数 改正传播、位姿图优化
- 获取当前关键帧的共视关键帧
- 用当前关键帧改正后的位姿和共视帧与当前帧的相对位姿计算共视帧改正后的位姿
- 改正共视帧的地图点坐标
- 当前帧和回环帧地图点融合,或当前帧没有的地图点,需要加入
- 进一步融合当前帧和回环帧共视帧的地图点
- 位姿图优化
- 新建线程进行全局BA
Details
1. 不同追踪模式
- mbOnlyTracking? mbVO? 两个标志位打开与否的区别?
- 正常模式(mbOnlyTracking = 0)
- 仅追踪模式(mbOnlyTracking = 1)
- 该模式下不会筛选关键帧,也不会向局部地图线程添加关键帧,相当于关掉后端
- mbVO仅在该模式下起作用
- 当TrackWithMotionModel函数中追踪上的匹配点数太少时,mbVO被打开
- mbVO关闭时,和正常模式过程一样
- mbVO打开时,不断尝试用运动模型估计位姿并做重定位,重定位成功后mbVO才被关闭。因为追踪点数少,所以根本不Track Local Map
- 总结区别:
- mbOnlyTracking关掉后,关掉了后端和建图
- mbVO:追踪点数少时,放弃Track Local Map,不断尝试重定位
2. 关于双目和RGBD
- 构造帧
- 双目双线程提取ORB特征
- 特征点去畸变
- 计算双目匹配
- 利用水平极线
- 对于一个左目特征,只可能出现在其行号的右目特征进行匹配;列方面用最小深度/最大视差来剪枝
- refine匹配结果
- 双目深度恢复
- 启发式滤波
- 初始化
- 点数大于500即可
- 创建关键帧
- 利用双目深度恢复的结果,将3D点加入地图
- 该帧加入局部地图
- 创建关键帧
- 双目时,利用双目深度恢复加入一部分地图点
3. 后端为什么不做边缘化?
- ORB-SLAM中地图点剔除机制和共视图选取local mapping大小的机制,都保证与被剔除的点和关键帧相关的协方差矩阵块比较稀疏。所以直接丢弃它们,可能对优化结果影响很小。
- 细节详见 留白君-ORB-SLAM在进行局部BA时为什么没有进行边缘化思考
4. 关于ORB特征提取与Bucket均匀化
https://blog.csdn.net/qq_44047943/article/details/112385607
…
- PnPSolver
- Sim3
- Loop Closing Optimize