ORB-SLAM 2 整理

 基本知识：

1、提取ORB描述子

1. 计算8层图像金字塔
2. 将每层金字塔图像按网格划分，并对每个网格提取Fast角点。

   Fast角点提取规则：（FAST角点定义：某个点与邻域的点相差太大。）灰度图像中就是与相邻点灰度值梯度较大的点即为Fast角点提取规则。

3. 利用八叉树结构，将所有角点按区域分配。
   八叉树结构：一种树状数据结构，主要用于空间划分和最近邻搜索。方便快速搜索寻找。

4. 利用质心计算每个角点相对于质心的角度。

5. 利用特征点的方向对随机选取的点对进行旋转变换后再确定特征的表达，得到新的描述子，具有旋转不变性。

   

2、特征匹配

1. 计算当前帧的bow特征向量，构建词典。
2. 设置匹配阈值
3. 匹配属于同一个Bow词汇的特征点：a.查找当前帧和匹配帧Bow特征向量节点ID一样的词汇。b.每个词汇ID可能包含同一图像的多个特征点。c.对两帧间对应节点的所有特征点进行逐一匹配。d.利用最近邻算法筛选最佳匹配特征。e.统计匹配对之间特征点的主方向。
4. 保存接近主方向的点对，其余点进行滤除。

地图初始化： 

如上图

注：

• 若发现三角化重建出来的地图云点重投影误差太大，则说明单应矩阵或者基础矩阵分解出来的R T不可信，需要重新进行初始化。
• 若重投影误差满足要求，则接受该结果，进行BA优化，优化相机位姿以及地图云点。
• 若参考帧与当前帧特征点匹配数量较低，就重置初始化流程。

一、追踪线程

任务：从相机获取输入图像，输出每帧图像位姿信息用于定位，同时选出关键帧输给局部建图线程。

注意：单目slam，追踪线程前先需要完成地图的初始化，生成初始的点云地图才可以进入追踪线程任务。

•  特征提取：此线程中的特征提取与匹配更多了一些额外的约束和要求。将图像划分为多个网格，每个网格内必须提取出一点数量的角点。
• 初始位姿估计（粗定位）：包含三种策略

注意：

本质矩阵特性：

• 由于对极约束是等式为零的约束，所以对E乘以任意非零常数，都成立，这也就是说该等式没有尺度约束。
• 根据本质矩阵的表达式为t^R，可以证明本质矩阵E的奇异值必定是[x,x,0]的形式，其称为本质矩阵的内在性质。（此处推导证明可以整一下）
• 本来由于t和R总共是六个自由度，但是由于本质矩阵没有尺度的约束，导致t的z是失效的，也就是说E实际上只有5个自由度。 
• 最终分解得到t和R有四种组合也是因为E与-E等价所导致的。
• 还有一点，在实际工程种，E的奇异值不一定是上述的形式，所以在实际处理过程中，我们将E矩阵进行SVD分解，然后将中间的对角矩阵强行转化为上述奇异值形式。

最后：当特征点共面或者相机发生纯旋转时，基础矩阵的自由度下降，因为此时t的信息较少，就会导致出现所谓的退化。此时就要使用单应矩阵。

        1、使用匀速模型估计出两帧间的位姿变化矩阵。其中匀速模型中的速度即为最近一次的位姿变化之差。接着通过匀速模型将点云投影至当前帧，做重投影误差分析，进一步解算位姿。在mVelocity为空（上一帧与下一帧之间求不出r和t，可能是找不到匹配的特征点对，运动过快，抖动等原因），也就是速度为0时，扩大搜索范围，进行下一种策略。

        2、在上述策略失效时，利用词袋模型将当前帧与之前的关键帧快速匹配起来（匹配点数大于15）。然后将地图点云通过一个初始的位姿矩阵（待求）转到当前帧的图像坐标系下，并且通过最小化重投影误差来优化该位姿矩阵。

        3、若当前帧与关键帧匹配的并不好，也就是找不到好一些的关联帧、关键帧追踪丢失

        （导致此的原因可能为：a.初始化不够好，前期的地图点云太少了，在该位姿下找不到匹配的地图点。b.运动的太快了，都糊了关键帧也质量一般，指定匹配不上。）

        采用第三种策略——重定位。所谓重定位就是利用词袋模型将当前帧图像转化为表征向量，并在全局关键帧数据库中寻找与当前帧匹配较好的一些关键帧用于接下来的PnP的RANSAC迭代。注意：在每次RANSAC迭代后，判断该帧是否可以观测到50个以上的地图点，若可以则结束迭代，重定位成功。若最后仍不成功，则重新开始整个SLAM系统。

 PnP的RANSAC迭代：这个东西可以详细推一下，暂时先鸽了，公式有点多。其大致思路就是用RANSAC迭代取出地图中的4个点，通过P4P的方法先大致估计出一个位姿，在利用这个位姿将其他地图点投影到该帧，接着最小化重投影误差优化位姿。

• 局部地图跟踪 

在初始位姿估计完成的基础上，利用当前帧与局部地图上的多个关键帧建立共视关系，并利用地图点与当前帧的投影关系，对位姿进行更精准的求解。构建pnp问题，最小化重投影误差继续优化位姿参数。 

• 新关键帧选取 

挑取含有足够多的地图云点，且与上一个挑选帧有足够的时间间隔的帧。   

// 如果关键帧比较少，则考虑插入关键帧

//或距离上一次重定位超过1s，则考虑插入关键帧 

//当前关键帧共视程度最高的关键帧会被设定为当前帧的参考关键帧

 二、局部建图线程

目标：将上一个线程中被挑选的关键帧进行处理，用以更新局部地图，并且将这些关键帧输出到闭环检测线程中。

• 关键帧插入

就是将关键帧更新到词袋模型的在线数据库，并根据地图云点与其他关键帧建立连接，最后将该关键帧添加到地图结构之中。 

• 近期地图点筛选 

将一些质量差的云点剔除（一些点被同时观测的帧数较少，或者上一个观测帧距离这一个观测帧太远） 。这是维护地图点鲁棒性的重要机制。

知识点补充：

Covisilibilty Graph 的顶点是相机的Pose，而边是Pose-Pose的变换关系——所以也算是Pose Graph 一种吧。当两个相机看到相似的空间点时，它们对应的Pose就会产生联系（我们就可以根据这些空间点在照片上的投影计算两个相机间的运动）。根据观测到的空间点的数量，给这个边加上一个权值，度量这个边的可信程度。

Essential Graph 比Covisibility Graph更为简单，ORB-SLAM主要用它来进行全局优化。为了限制优化的规模，ORB-SLAM试图尽量减少优化边的数量。而尽量减少边，又保持连通性的方法，就是做一个最小生成树。

Covisibility 是一直在用的概念，而Essential Graph是orbslam自己提出的概念，为了减小全局回环的计算量。当你自己实现SLAM时，也会碰到这些困难，并设计一些应对的策略，这些就是你的创新性。事实上，随着SLAM时间的增长，如何控制图的结构和优化的规模，仍是现在SLAM有待解决的一个问题。

作者：半闲居士
链接：https://www.zhihu.com/question/42050992/answer/93630213
来源：知乎
著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。

Covisibility graph：共视图：是用来描述不同关键帧可以看到多少相同的地图点：每个关键帧是一个节点，如果两个关键帧之间的共视地图点数量大于15，则这两个节点之间建立边，边的权重是共视地图点的数量;

Spanning tree是covisibility graph的子集，保留了所有的节点（或者说关键帧），但每个节点只保留和最多共视地图点关键帧之间的边;

essential graph：本征图：是简版的covisibility graph，保留了所有的节点，共视地图点数量大于100才会建立边。

• 新地图云点重建 

对于一个新的关键帧，借助共视关系图与相邻的关键帧进行三角化重建，生成地图云点。 

• 局部BA优化 

 地图云点与关键帧最小化重投影误差，求解位姿。

• 局部关键帧优化 

剔除冗余的关键帧（剔除的标准是：该关键帧的90%的MapPoints可以被其它关键帧观测到），以保证地图中关键帧的鲁棒性。 

三、闭环线程

局部建图线程中处理的每个关键帧都要送进闭环线程，进行判断，一旦构成回环则修正位姿图。 

回环线程分为回环检测（候选回环、相似变换计算）以及回环修正（回环融合、位姿图优化）。

• 候选回环：利用词袋模型，将数据库中与当前帧相似度较高的帧挑选出来。作为候选。

步骤1：如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测。

步骤2：遍历所有共视关键帧，计算当前关键帧与每个共视关键的bow相似度得分，并得到最低得分minScore

步骤3：在所有关键帧中找出闭环备选帧

步骤4：在候选帧中检测具有连续性的候选帧

•     1、每个候选帧将与自己相连的关键帧构成一个“子候选组spCandidateGroup”，vpCandidateKFs-->spCandidateGroup
•  2、检测“子候选组”中每一个关键帧是否存在于“连续组”，如果存在nCurrentConsistency++，则将该“子候选组”放入“当前连续组vCurrentConsistentGroups”
• 3、如果nCurrentConsistency大于等于3，那么该”子候选组“代表的候选帧过关，进入mvpEnoughConsistentCandidates

• 计算相似变换：

计算当前关键帧与每个回环候选帧之间的变换关系。单目[sR | t]，若有足够多的数据可以计算该矩阵则接受这个候选帧，回环检测成功.

注意：

单目是求解sim3，因为需要计算尺度

而RGBD则是求解SE3，不需要计算尺度、并且增加了第四个线程：全局BA优化

• 回环融合：

• 位姿图优化：

将全局地图上的关键帧位姿量作为优化变量，也就是位姿图优化。

缺点：频繁计算orb特征，并且三线程对cpu负荷较大。且其单目为稀疏建图，只能满足定位功能。 

 本文引自：机器人SLAM导航核心技术与实战 张虎