ORB_SLAM2 原理、论文解读、代码流程

最新推荐文章于 2024-07-10 21:17:57 发布

AD狗东

最新推荐文章于 2024-07-10 21:17:57 发布

阅读量9.3k

点赞数 41

文章标签： 1024程序员节 slam 计算机视觉算法人工智能

本文链接：https://blog.csdn.net/adam_dogg/article/details/120922483

版权

ORB_SLAM2 原理+论文解读+代码流程

算法原理
代码流程

前言

算法原理部分：和论文讲解的顺序一致，但同时包含了代码中体现的策略和实现方法
代码流程部分：介绍了文件的调用关系，以及程序主体中重要函数的功能、流程、数据结构

算法原理

算法由三大部分组成，分别是 Tracking、LocalMapping、LoopClosing 三个线程

Tracking

特征点的提取（ORB Extraction）
在此之前，特征点的提取方法有sift，surf等，但相比ORB都比较耗时。ORB算法先提取FAST角点，再计算BRIEF描述子，最终利用Bow词袋模型来匹配特征点。
- 建立图像金字塔：将原图像按不同比例缩小，得到多张图片，分别对每张图片进行下述操作。
- FAST角点提取（对所有像素点进行提取）
  - 将图像转为灰度图，选取像素 $p$ ，假设他的亮度为 $I_p$
  - 设定一个阈值 $T$ ，其大小与 $I_p$ 成正比
  - 以 $p$ 为中心，半径为3的圆上选16个像素点
  - 如果有连续 $N$ 个点亮度大于 $I_p+T$ 或小于 $I_p-T$ ，则 $p$ 被认为是一个FAST角点
- BRIEF描述子（对所有的FAST角点计算描述子）
  - 计算每个FAST角点圆心到质心点的方向
  - 将每个角点对应的圆域进行旋转，旋转至所有角点对应的方向相同。
  - 圆域内选取N个点对（默认指定了128对点），通过将亮度的比较结果，将角点映射为：只由0、1组成的128维向量，将此向量作为BRIEF描述子
  - 最后，对每个FAST角点，都生成了128维描述子。
  - 当被拍摄物体放大或者缩小时，FAST角点会产生不同的提取结果（比如缩小会引起角点消失），但在图像金字塔的其他层中依然可以提取到缩放前对应的FAST角点，从而具有良好的尺度不变性。当被拍摄物体发生旋转时，其FAST角点的方向也会随之旋转，计算BRIEF描述子前，将所有FAST角点旋转至同一方向，相当于将所有拍摄对象都旋转到同一方向，从而具有良好的旋转不变性
- BoW 词袋模型（对图片、特征进行查找和索引）
  - 词袋模型作为所有描述子的字典，用于量化两张图或两个特征点的相似性
  - 字典的数据结构为 k叉树，上层是下层的聚类，最底层的每个叶子节点对应的是最小特征单元：也就是BRIEF描述子
  - 词袋模型的好处：给一特征点找匹配时，只需查找k叉树，找到与此特征的描述子最近的特征点，作为匹配点，降低了查找匹配的复杂度
旋转不变性来自：提取FAST特征时，计算了每个角点的中心点到质心点的方向向量，即使物体发生了旋转，也可利用方向向量来 “纠正”。
尺度不变性来自：对每一帧，建立4层的高斯金字塔，分别在每层金字塔上做特征提取，即使A图中的特征在B图中变小，但在A图金字塔高层，特征也会变小，同样可以与B图对应特征匹配。
点云初始化
这部分在论文中没有提及。发生在整个算法刚开始运行时，点云的初始化是此后每一帧能成功定位的前提
- 作用：特征匹配、三角化，得到初始点云图，对于单目来说，就是将P2P问题转为PnP问题
- 步骤(以单目为例）：
  - 初始选取2个特征点较多的帧（ $特征点数 > 100$ )，并在词袋中进行特征匹配
  - 并行计算单应性矩阵 $H$ 和基础矩阵 $F$ 。
    当对极约束不成立的时候（相机只有旋转没有平移），或场景都在同一平面时，用单应性矩阵来计算两帧之间的变换矩阵。否则用基础矩阵计算变换矩阵
  - 将第一帧的位姿设置为基准位姿，第二帧的位姿为上一步所求出的变换矩阵
  - 创建地图点对象MapPoint，恢复出所有匹配点对应的世界坐标
位姿初始化（Initial Pose Estimation）
对 $C u r r F r a m e$ (此刻的帧)位姿优化之前，先要有一个大概的估计，作为之后优化位姿的初值。根据当前跟踪状态的不同，一共有三种初始化方法：根据参考关键帧估计，根据匀速运动模型估计，重定位。除此之外，还要对此帧观测到的特征点进行估计
- 作用：估计帧的位置，以及观测到的局部地图点
- 步骤：
  - 跟踪状态为LOST时：
    - 用重定位估计位姿（Relocation），用词袋模型寻找与 $C u r r F r a m e$ 最相似的帧 $R e l o c F r a m e$ ，进行特征匹配
    - 统计 $C u r r F r a m e$ 上匹配点数量，数量太少则认为匹配效果差，保持状态为LOST，结束对此帧的处理，等待下一帧；数量足够的话，进行一次仅优化位姿的BA（motion-only BA），数量不够则舍弃此帧
  - 跟踪状态NORMAL时：
    - 根据匀速运动模型估计位姿，默认上一帧到 $C u r r F r a m e$ 的变换矩阵等于上上帧到上一帧的变换矩阵。（track with Motion model）
    - 将上一帧（LastFrame）观测到的MapPoint 投影到估计的位姿上，观测有多少投影点与特征点位置重合，重合则认为是一对匹配点，如果重合的足够多，则进行一次仅优化位姿的BA，如果重合的个数太少，表明匹配失败改用参考关键帧估计；
  - 根据参考关键帧估计位姿：（Track with RefFrame）
    - 用词袋模型，将 $C u r r F r a m e$ 与参考关键帧（最新的关键帧）进行特征匹配
    - 匹配后执行一次仅优化位姿的BA（motion-only BA）
    - 统计匹配特征点数，数量少则认为匹配失败，将状态改为LOST，结束对此帧的处理，等待下一帧；
- 需要注意：初始化后的 $F r a m e$ 中仍会有大量特征点没建立匹配，这也体现了tracking线程的重要思想：在尽可能少的建立匹配的情况下，初始化出相机的位姿。
局部位姿优化（Track Local Map）
经过上面的初始化，失败的 $F r a m e$ ，说明其与地图联系不够紧密，无法建立稳健的约束和估计；而与地图联系紧密的 $F r a m e$ 则初始化成功。
但刚刚初始化所得位姿仅依赖于与某一帧（前一帧或参考关键帧）中的匹配关系，置信度较低。
接下来，我们在局部图中寻找更多的匹配关系，利用这些匹配关系进一步优化 $C u r r F r a m e$ 的位姿。
- covisibility graph定义：covisibility graph，是无向加权图，如果两个关键帧之间有共视关系（共同观测到>15个地图点）就连成一条有权边，这些边和所有关键帧节点组成共视图。
- 在共视图中，针对 $C u r r F r a m e$ 定义一组关键帧为局部关键帧，局部关键帧由四种邻居组成。这四种邻居为：
  - 与 $C u r r F r a m e$ 有共视点的帧（一级共视帧）
  - 与一级共视帧有共视点的前十个帧（二级共视帧）（按共视点个数排序）
  - 一级共视帧的子关键帧
  - 一级共视帧的父关键帧（与一级共视帧共视程度最高的帧）
- 将 $C u r r F r a m e$ 、局部关键帧 、以及这些帧观测到的 MapPoint作为：局部图 （ $l o c a l m a p$ ）
- 将 $l o c a l m a p$ 中的MapPoint向 $C u r r F r a m e$ 投影
- 对 $C u r r F r a m e$ 再进行一次仅位姿BA（motion-only BA）优化，估计出各个局部关键帧的位姿
关键帧的审核与生成 (New Keyframe Decision)
- 检测系统是否允许关键帧的插入
  - only_Tracking 模式下，不需要关键帧
  - $l o c a l m a p$ 被 loopclosing线程占用，不能插入关键帧
  - 关键帧总数较多，且距离上一个关键帧距离很近，不需要插入关键帧
- 审核