【一文足以系列】ORB SLAM2完整详细算法流程
目录
- 算法目的
- 算法应用场景
- 算法优点
- 相关概念点
- 内涵算法
- 算法实现
-
- tracking线程
-
- 步骤
- 单目相机初始化
-
- 成功条件
- 单目相机初始化器
- 初始化后续
- 初始位姿估计
-
- 跟踪局部地图
- 关键帧的创建
- 参考关键帧
- 局部建图线程
-
- 构建地图步骤
- 好的地图点标准
- 创建新地图点
- 地图点融合
-
- 冗余关键帧的标准
- 回环检测线程
-
- 步骤
- 回环匹配
-
- 连续性检测
- 更详细的解释
- sim3 + 闭环矫正
- 算法对比
-
- PTAM
- SVO SLAM
- VIO SLAM
- DS-SLAM
- Dyna-SLAM
- DSO-SLAM
- ORB-SLAM3
- 算法缺点
- 其他
-
- 共视图、扩展树、本质图
-
- 共视图
- 扩展树
- 本质图
- 零散项
-
- 地图点相关
- 双目特征点相关
- 参考链接
算法目的
位姿估计 + 建图
在建图的同时不断优化自身位姿 提高建图的准确度
为广大视觉SLAM算法打下了基础
算法应用场景
- 室内室外均可
- 有明显的并且数量合理的回环检测
- 单目、双目、RGBD
- 算法本身可能无法直接使用 但是为后续算法提供了模版框架
算法优点
- 室内室外均可
- 有明显的并且数量合理的回环检测
- 单目、双目、RGBD
- 算法本身可能无法直接使用 但是为后续算法提供了模版框架
相关概念点
特征点：提取的是ORB特征 包括特征点和它的描述子
关键帧：由若干帧中选出的重要的帧（依据选取间隔、特征点数量等）
参考关键帧：构建这个地图点的关键帧为参考关键帧。
地图点：由关键帧的特征点三角化出的地图点
BA：对位姿和地图点同时进行优化（EM）
BOW：词袋 是特征点的聚类
BOW向量：BOW相似度得分 用一个向量描述整个图像（图像中出现狗 猫 但是没有汽车 记为【1， 1， 0】）得分就是计算两个图像之间的差值
ORB特征点：如角点 边缘点 特征点法中ORB特征点 + 相对应的描述子
描述子：对ORB特征点的位置进行描述
光流法：最小化光度误差 估计相机的运动 不实用描述子但使用特征点
直接法：先估计相机的运动位姿 计算前后两帧相对应的像素点的灰度值 通过减小灰度值的差值 优化相机位姿（可以使用任意像素点而不一定使用特征点 更不需要描述子）
内涵算法
三角化：三角测量算法 根据两帧图像恢复出深度 即地图点
对极几何：根据两帧单目图像恢复出相机运动 生成对极约束 对极约束中包括基础矩阵和本质矩阵 实际中通过求解本质矩阵得到相机变换位姿更多 但本算法使用的是基础矩阵
单应矩阵：恢复相机运动 场景中的特征点都落在了同一平面上 则可以通过单应矩阵进行求解
PnP：已知一帧图像的三维点和另一帧图像的二维点计算出相机位姿
RANSAC：将图像中的外点刨除 外点会极大影响计算结果 多次迭代拟合保留内点
BA优化：e = z - h(T, p) z为像素坐标 T为相机位姿（外参）对应的李群 p为三维点坐标 优化变量：R、t和p坐标 已知：z
sim3：用三对关键帧对应的地图点进行位姿求解（包括旋转矩阵、平移矩阵、尺度变换）
算法实现
tracking线程
三大线程：跟踪线程（主线程）、局部建图线程、回环检测线程
步骤
- ORB 特征点提取
- 相机初始化
- 根据前一帧图片估计出当前帧的位姿
- 基于当前帧的位姿构建出局部地图 局部地图的作用是优化当前帧的位姿
- 决定是否生成关键帧
单目相机初始化
成功条件
连续两帧间成功三角化超过100个点，则初始化成功
初始化帧即为参考帧 第二帧的特征点个数要大与100
单目相机初始化器
当特征点共面或者相机发生纯旋转时 基础矩阵的自由度下降 如果此时继续使用基础矩阵进行求解 则求解结果会极大的受到噪声的影响 为了避免这种情况的发生 本算法采用H和F同时计算的方法 选取卡方检验得分较高的那个结果
单目相机初始化步骤：
- 若单目相机初始化器还没创建，则创建初始化器
- 若上一帧特征点数量足够但是当前帧的特征点数量过少 则匹配失败 删除初始化器
- 在初始化帧和当前帧进行特征匹配
- 若匹配的特征点数目太少，则匹配失败，删除初始化器
- 进行单目初始化
- 创建初始化地图
初始化后续
- 将两个关键帧插入地图
- 处理所有的地图点（添加地图点 构建关键帧和地图点的双向联系等）
- 全局BA 优化所有关键点位姿和地图点
- 归一化平移尺度和坐标点尺度 更新地图点坐标和位姿
初始位姿估计
初始化成功后 每次传入一帧新图像 都会先进行初始位姿估计 然后生成局部地图 再对局部地图中的地图点和当前位姿进行BA
恒速运动模型估计：认为速度恒定 进行匹配 若匹配成功（词袋匹配点大于10）则认为速度恒定 根据速度恒定模型计算初始化位姿 + BA
参考帧估计位姿：将当前关键帧共视点最多的帧设为关键参考帧 与这一个关键参考帧帧进行匹配 若匹配成功则使用上一帧的位姿作为初始化的位姿 + BA
重定位估计位姿：根据词袋相似度寻找到与当前帧相近的关键帧（设为参考关键帧）
对每一个参考关键帧都初始化一个PnPsolver 保存成指针向量
每一个PnPsolver都进行最多三次特征匹配
第一次特征匹配：计算初始位姿 + BA只优化位姿（优化位姿过程中对内点和外点根据进行区分 如果g2o :: chi2误差较大 此边的值和和其他边不符 认为是外点）如果内点数量达不到要求 进行第二次特征匹配
第二次特征匹配：通过投影的方式将关键帧中未匹配的地图点投影到当前帧中, 生成新的匹配 重复上述操作
第三次特征匹配：如果第二次内点数量在30-50之间 进行最后一次抢救 重新投影匹配 使用更严格的描述子阈值
步骤：
- 检查是否初始化
- 检查恒速运动莫模型估计是否成功
- 若没成功则使用参考帧估计位姿
- 若还没成功则使用重定位估计位姿
- 若还没成功则标记跟踪lose
跟踪局部地图
成功估计当前帧的初始化位姿后 基于当前位姿更新局部地图并优化当前帧位姿 流程：
- 更新局部地图
- 将局部地图点投影到当前帧特征点上
- BA 优化当前帧位姿
- 更新地图点观测数值 统计内点个数
- 根据内点个数判断是否跟踪成功
关键帧的创建
关键帧的要求：
当前帧的质量 当前帧的地图点要足够多 同时与参考关键帧的重合程度不能太大
时间间隔
是否进行过重定位 如果进行过重定位 重定位后的位姿不会太准 不能当作参考帧
参考关键帧
根据参考关键帧估计初始位姿
根据参考帧估计地图点的平均观测距离
参考帧的指定：
创建新关键帧后会将新创建的关键帧设为参考关键帧
将当前关键帧共视程度最高的关键帧设为参考关键帧
局部建图线程
构建地图步骤
run函数：死循环
- 检查缓冲队列中是否含有关键帧
- 处理缓冲队列中的第一个关键帧
- 剔除坏点
- 创建新地图点补充坏点（对极几何或三角化）
- 将当前关键帧与共视关键帧地图点融合
- 局部BA优化
- 剔除冗余关键帧
- 将当前关键帧加入闭环检测中
好的地图点标准
如果地图点经过了连续三个关键帧未被剔除 则认为是好的地图点
剔除准则：
召回率 < 0.25 && 创建的三帧内观测数目少于2
创建新地图点
将当前关键帧分别与共视程度最高的前10(单目相机取20)个共视关键帧两两进行特征匹配,生成地图点.
对于双目相机的匹配特征点对,可以根据某帧特征点深度恢复地图点,也可以根据两帧间对极几何三角化地图点,这里取视差角最大的方式来生成地图点.
地图点融合
在将地图点反投影到帧中的过程中,存在以下两种情况:
- 若地图点反投影对应位置上不存在地图点，则直接添加观测
- 若地图点反投影位置上存在对应地图点，则将两个地图点合并到其中观测较多的那个（替换）
冗余关键帧的标准
关键帧90%的地图点可以被超过3个其他关键帧观测到
回环检测线程
步骤
- 取出缓冲队列头部关键帧 作为当前检测闭环关键帧
- 如果距离上次回环检测时间太短 则不进行回环检测
- 计算当前关键帧与共视关键帧间的最大相似度（后面有详细解释）
- 根据相似度寻找当前关键帧的闭环候选关键帧
- 在当前的关键帧组和之前的连续关键帧组之间寻找匹配
- 维护循环变量 当前帧的闭环候选关键帧组当作下一帧之前的闭环候选关键帧组
回环匹配
- 寻找当前关键帧的共视关键帧并计算当前帧和共视关键帧的BOW相似度得分 求出最低得分
- 选取闭环候选关键帧与当前帧的共视关系（BOW相似度得分）要大于最低得分 则认为匹配成功
- 将所有匹配成功的量层层过滤 选取过滤之后相似度得分最高的关键帧作为最终的闭环候选帧
- 对最终的闭环候选帧做连续性检测
连续性检测
- 每个闭环候选关键帧将与共视关键帧构成一个子候选组
- 检测“子候选组”中每一个关键帧是否存在于“连续组”，如果存在 nCurrentConsistency++，则将该“子候选组”放入“当前连续组vCurrentConsistentGroups”
- 如果nCurrentConsistency大于等于3，那么该“子候选组”代表的候选关键帧过关
通过连续性检测的候选关键帧为准备进入闭环矫正的关键帧
更详细的解释
连续的概念：不同组之间有一个及以上共同关键帧
连续链：1–2--3–4 防止形成网状结构 采用菊花链的方式
对连续组进行维护 存的是子候选组 每个子候选组都维护 ‘连续链’ 这个变量
连续组更新方式：所有被检测到与当前子候选组有关联的 都会在连续组链后面 + 1
sim3 + 闭环矫正
矫正前停掉局部建图线程和局部BA线程
- 寻找两关键帧之间的粗匹配来粗略估计sim3
- 由粗略估计出的sim3来寻找两关键帧之间更多的匹配
- 构建图优化模型，固定地图点通过两关键帧的地图点到两帧的重投影误差来优化sim3
- 本质图BA优化 优化所有地图点和关键帧位姿
算法对比
PTAM
PTAM最大的贡献是提出了tracking、mapping双线程的架构
tracking线程只需要逐帧更新相机位置姿态，可以很容易实现实时计算
而mapping线程并不需要逐帧更新，有更长的处理时间 这种基于优化的算法比滤波法在单位计算时间可以得到更高的精度。
这种多线程的处理方式也更顺应现代CPU的发展趋势。之后的视觉SLAM算法几乎全部沿用了这一思想。
SVO SLAM
SVO算法：结合了特征点法和直接法的视觉里程计
SVO算法首先使用了图像中的特征点 但是没有计算描述子（描述子的计算时间漫长 但可以描述特征点所在位置）通过初始化相机位姿来大约估计两帧图像之间匹配同一地图点的两个像素点 计算两个像素点的灰度值并构建灰度误差函数 通过最小化灰度误差函数来优化两帧间的相机位姿（相机位姿如果不够好 两个像素点的灰度值会相差很大）
SVO只是一种里程计 高博将SVO和ORBSLAM算法相结合 跟踪速度是原来的3倍
VIO SLAM
视觉惯性里程计/视觉惯性系统
将相机和IMU数据进行融合 ORB-SLAM中运动估计采用构建重投影误差方程来优化运动估计 在VIO SLAM中直接使用IMU的数据进行运动估计
根据是否把图像特征加入特征向量中 分为紧耦合算法和松耦合算法
松耦合：视觉运动估计和惯导运动估计分为两个进程 将最后的结果进行融合
紧耦合：充分利用了传感器数据 但是算法整体的维度特别高 需要很高的计算量
紧耦合包括算法：MSCKF、ROVIO
DS-SLAM
在动态环境中对运动的物体进行了更好的处理 和ORB SLAM相比具有更高的精度和鲁棒性
Dyna-SLAM
同Dyna-SLAM 处理了动态环境中对运动的物体 Mask R-CNN + 多视图几何检测动态物体
二者融合后便可以检测出那些运动的没有先验知识的物体（例如，被人推动的椅子，拿着的书本）。最后只使用静态区域ORB特征点进行相机位姿估计。由动态区域的Mask筛选得到静态区域的特征点，进而进行跟踪和建图，此过程与ORB-SLAM完全相同。
DSO-SLAM
使用直接法 是传统特征点法的五倍速度 但保证了原始的精度
ORB-SLAM3
增加了IMU和鱼眼相机 提高了鲁棒性和精度 以及以下的细节：
- 保存了很多琐碎的地图：主要的作用发挥在Tracking线程Lost时，之前Lost后需要回到原先的位置进行Relocalization()，但是现在，如果tracking线程丢失，ORBSLAM3会在之前的所有小地图中进行查询匹配，如果匹配成功，则tracking线程继续；如果匹配不成功，则重新开辟一个小的地图；
- 新增最大后验概率，用于初始化并且提炼IMU数据
- loop and map merging 线程：因为有了很多琐碎的地图，因此新增了map merging的部分，如果两个地图有重合部分，且重合处属于当前Active的Map，则对其进行LoopCorrection，类似于之前的检测到回环。如果两个地图重合的部分属于别的地图，这就类似于找到了其他两个地图的重合点，因此就将他们merge到一起。
算法缺点
- 初始化时最好保持低速运动，对准特征和几何纹理丰富的物体。
- 旋转时比较容易丢帧，特别是对于纯旋转，对噪声敏感，不具备尺度不变性。
- 如果使用纯视觉 slam 用于机器人导航，可能会精度不高，或者产生累积误差，漂移，尽管可以使用 DBoW 词袋可以用来回环检测。最好使用 VSLAM+IMU 进行融合，可以提高精度上去，适用于实际应用中机器人的导航。
- 代码中的bug和性能有待提升等问题
其他
共视图、扩展树、本质图
共视图
如果两个关键帧的共视地图点超过了15个 就增加一条边 视为共视。所有的这些边连在一起形成了共视图。
共视图是无向加权图 边的权重就是共视地图点的数目
作用：
跟踪局部地图扩大搜索范围
局部建图里关键帧之间的新建地图点
闭环检测 重定位检测
优化
感性认识共视图的作用：重定位进行候选关键帧的选取时 得到匹配好的候选关键帧 并将和这个关键帧具有共视关系的关键帧全部拿出来 选出评分最高的关键帧作为最终选择出来的候选关键帧
扩展树
对于所有的关键帧 每一个关键帧都能够得到和当前关键帧共视程度最大的关键帧 将当前关键帧和共视程度最大的关键帧连在一起就形成了扩展树
本质图
本质图为扩展树的连接关系
共视关系好的（大与100）的连接关系
形成闭环的连接关系 闭环后的地图点变动后新增加的连接关系
本质图中节点也是所有关键帧
本质图是浓缩版的共视图 只保留权重大与100的边 就是保留了共视程度更高的关键帧
作用：
检测回环时 利用本质图对相似变换sim3进行位姿图优化
零散项
地图点相关
地图信息 = 关键帧数组 + 特征点数组
地图点在关键帧的索引 = 地图点对应的特征点在关键帧的索引
地图点对应特征点 特征点不一定能三角化出地图点
地图点map：关键帧和该地图点在这个关键帧里对应的特征点的索引
参考关键帧：构建这个地图点的关键帧为参考关键帧，如果这个参考关键帧被删除了，则指定第一个观测到这个地图点的关键帧为参考关键帧
双目特征点相关
双目特征点匹配步骤：
粗匹配：根据两个特征点的描述子和金字塔层级进行配对 两个特征点的描述子的距离不能太远 金字塔层级不能相差太多
精匹配：在两个特征点的附近进行窗口滑动 计算两个窗口之间的距离的大小 距离不能过大
亚像素插值：在右目相机的特征点的左右各选取一个特征点 分别计算左目特征点分别到右目特征点以及附近特征点的距离 并拟合成二次函数 求二次函数的最小值 即为最好的匹配结果
删除离群点：匹配距离大于平均匹配距离2.1倍的特征点被认为误匹配
参考链接
原视频链接 讲的非常好！