ORB随便记一记

Posted 2020-11-16 dlutjwh

论文摘取

（这部分看的是泡泡机器人的翻译）

基于特征点、单目、完全自动初始化，基于PTAM框架。

相关工作

A.位置识别（大概是用于回环检测）

bags of words

FAB-map

DBOW2

covisibility 信息返回多个假设

B.地图初始化

单目SLAM需要初始化，两种方法：Mono-slam和LSD-slam（逆深度参数）

本文采用基于模型的初始化方法

• 平面场景：单应性矩阵
• 非平面场景：基础矩阵

C.单目SLAM

最初：每一帧采用滤波器联合地图特征和相机位姿：处理连续帧图像上浪费计算资源、累计线性误差

基于关键帧的SLAM PTAM：双线程（追踪和地图） FAST角点，无大闭环检测

大尺度单目：前端采用光流，FAST特征匹配，运动BA，后端为基于滑动窗口的BA，7自由度相似性约束进行闭环检测  本文也采用该7自由度位姿优化用于essiontial graph中

×××

 

 

系统框架

A.特征选择

对于构图、跟踪、重定位以及闭环检测都采用相同的特征 每张图像特征提取<33毫秒，且具有旋转不变性

B.三线程

追踪、局部地图构建、闭环检测

跟踪

对每帧图像的相机位置进行定位，插入关键帧，与前一帧匹配得到初始特征点，运动BA优化摄像头位姿，跟丢则全局重定位。获取初始位姿后，则使用系统维护的关键帧的covisibility graph提取一个局部可视化地图。然后通过重投影方法搜索当前帧与局部地图对应的匹配点，并用所有匹配点优化当前位姿。

局部地图

负责处理新的关键帧，对周围相机位姿进行局部BA优化重构。三角化新的地图点，删除冗余的点和关键帧。

闭环检测

对每个新的关键帧进行闭环检测，存在闭环则计算相似变换查看闭累计误差。

最后利用相似性约束对位姿图进行优化，优化的是essensiol graph。

优化算法全部采用g2to中的Levenverg-Marquardt

C.地图点云、关键帧及其选择标准

对每个地图点云保存以下信息：

• 世界坐标系中的3D坐标
• 视图方向n_i，连接点云和对应关键帧光心
• ORB特征描述子
• 根据ORB尺度不变性约束，可观测点云的最大距离d_max和最小距离d_min

关键帧K_i保存以下信息：

• 相机位姿
• 相机内参
• 图像帧提取的ORB特征，经过畸变矫正

D.convisibilty graph 和 essential graph

convisibilty graph：两个关键帧之间同时观测到15个点，则关键帧之间连一条边，权重为公共点数

essential graph：节点数和convisibilty graph一样，但是点更少（类似于最小生成树），插入新的关键帧时，将公共点最多的帧相连。

E.词袋模型的位置识别

基于DBOW2算法执行闭环检测和重定位

视觉词典

关键帧重叠问题

字典树强制匹配加快特征点匹配，用于三角化新的点云，闭环检测和重定位

引入方向一致性改进匹配点

地图自动初始化

分为平面视图（单映矩阵）和非平面视图（基础矩阵），启发式方法选择模型。算法步骤：

1. 查找初始点对（ORB）匹配
2. 并行计算两个模型：单映矩阵和基础矩阵，前者为四对特征点，后者为8对特征点，每次迭代时给每个模型计算一个分值SM，从单映矩阵和基础矩阵中选择分值高的。分值都不高则重启
3. 模型选择：启发式计算RH
4. 运动和从运动到结构的重构：单映矩阵是否能执行，基础矩阵计算本证矩阵后奇异值分解，见slam14讲7
5. BA：全局BA优化，见附录

 跟踪

A.ORB特征提取

在8层图像金字塔上提取FAST角点，金字塔尺度因子为1.2。如果图像分辨率较高，则适当增加角点数。根据角点计算ORB特征描述子。用于后续特征匹配。

B.通过前一图像帧估计相机的初始位姿

如果上一帧图像跟踪成功，则认为摄像头处于匀速运动。利用前后帧的匹配点对当前相机进行进一步优化。未找到足够匹配点则扩大搜索范围。

C.通过全局重定位来初始化位姿

若扩大搜索范围依然找不到足够匹配点，则计算当前帧的词袋向量选取若干帧备选，对于每个备选帧执行pnp算法计算当前帧的位姿（Ransac迭代求解）。如果找到一个姿态能够涵盖足够多的有效点，则搜索该关键帧对应的更多电云。最后基于找到的所有匹配点对相机位姿进行优化。

D.跟踪局部地图

当获得了相机位姿和一组初始的匹配点，则可以将更多的点云投影到图像上以找到更多的匹配点。为了降低复杂度，只映射局部地图。该局部地图包含一组关键帧K1，它们和当前关键帧有共同的地图点，还包括与关键帧K1在covisibility graph中相邻的一组关键帧K2，局部地图中有一参考关键帧Kref∈K1，它与当前帧有最多的共同地图点。对K1和K2的每个地图点，在当前帧下做如下搜索：

1. 计算地图点云在当前帧图像中的投影点x，超出图像边缘则删除。
2. 计算当前视图射线v和地图点云平均视图方向n的夹角，如果n<cos(60)，删除对应点云。
3. 计算地图点云到相机中心的距离d，若其不在地图点云的尺度不变区间内，即d∉[d_min,d_max]，删除该点。
4. 计算每帧图像的尺度比d/d_min
5. 对比地图中点的特征描述子D和当前帧中还未匹配的ORB特征，在预测的尺度层和靠近x的点做最优匹配。

相机位姿最后通过当前帧获得所有的地图点进行优化。（目的是在当前点帧和局部地图之间找到更多的匹配点对优化位姿）。

E.新关键帧的判断标准

1. 距离上一次全局重定位后需要超过20帧图像
2. 局部地图构建处于空闲状态，或距上一个关键帧插入后，已有超过20帧图像
3. 当前帧跟踪少于50个地图点
4. 当前帧跟踪少于参考关键帧K_ref点的90%

局部地图构建

A.关键帧插入

首先更新covisibility graph，包括：添加一个关键帧节点Ki，检查与Ki有共同点的其他关键帧，用边线连接。计算该关键帧的词袋，并利用三角法生成新的地图点。

B.地图点云筛选

三角化的点为了保留在地图中，必须在其创建后的头三个关键帧中通过一个严格的测试，该测试确保留下的云点都是能被跟踪的，不是由于错误的数据而被三角化的。该点必须满足以下条件：

1. 跟踪线程必须在超过25%的图像中找到该特征点
2. 如果创建地图点经过了多个关键帧，那么它必须至少是能够被其他3个关键帧观测到。

一旦一个地图点通过测试，它只能在被少于3个关键帧观测到的情况下移除。这样的情况在关键帧被删除以及局部BA排除异值点的情况下发生。这个策略使得我们的地图包含很少的无效数据。 

C.新地图点创建

新的地图点的创建是通过对covisibility graph中连接的关键帧Kc中的ORB特征点进行三角化实现的。对Ki中每个未匹配的ORB特征，我们在其他关键帧的未匹配云点中进行查找，看是否有匹配上的特征点。这个匹配过程在第三部分第E节中有详细阐述，然后将那些不满足对级约束的匹配点删除。ORB特征点对三角化后，需要对其在摄像头坐标系中的深度信息，视差，重投影误差和尺度一致性进行审查，通过后则将其作为新点插入地图。起初，一个地图点通过2个关键帧观测，但它在其他关键帧中也有对应匹配点，所以它可以映射到其他相连的关键帧中，搜索算法的细则在本文第5部分D节中有讲述。

D.局部BA

局部BA主要对当前处理的关键帧Ki,以及在covisibility graph中与Ki连接的其他关键帧Kc，以及这些关键帧观测到的地图点进行优化。所有其他能够观测到这些点的关键帧但没有连接Ki的会被保留在优化线程中，但保持不变。优化期间以及优化后，所有被标记为无效的观测数据都会被丢弃，附录有详细的优化细节。

E、局部关键帧筛选

为了使重构保持简洁，局部地图构建尽量检测冗余的关键帧，删除它们。这样对BA过程会有很大帮助，因为随着关键帧数量的增加，BA优化的复杂度也随之增加。当算法在同一场景下运行时，关键帧的数量则会控制在一个有限的情况下，只有当场景内容改变了，关键帧的数量才会增加，这样一来，就增加了系统的可持续操作性。如果关键帧Kc中90%的点都可以被其他至少三个关键帧同时观测到，那认为Kc的存在是冗余的，我们则将其删除。尺度条件保证了地图点以最准确的方式保持它们对应的关键帧。

闭环检测

线程抽取Ki——最后一帧局部地图关键帧，用于检测和闭合回环。具体步骤如下：

A、候选关键帧

我们先计算Ki的词袋向量和它在covisibility graph中相邻图像（θmin=30）的相似度，保留最低分值Smin。然后，我们检索图像识别数据库，丢掉那些分值低于Smin的关键帧。这和DBoW2中均值化分值的操作类似，可以获得好的鲁棒性，DBoW2中计算的是前一帧图像，而我们是使用的covisibility信息。另外，所有连接到Ki的关键帧都会从结果中删除。为了获得候选回环，我们必须检测3个一致的候选回环（covisibility graph中相连的关键帧）。如果对Ki来说环境样子都差不多，就可能有几个候选回环。

B、计算相似变换

单目SLAM系统有7个自由度，3个平移，3个旋转，1个尺度因子 [6]。因此，闭合回环，我们需要计算从当前关键帧Ki到回环关键帧Kl的相似变换，以获得回环的累积误差。计算相似变换也可以作为回环的几何验证。

我们先计算ORB特征关联的当前关键帧的地图云点和回环候选关键帧的对应关系，具体步骤如第3部分E节所示。此时，对每个候选回环，我们有了一个3D到3D的对应关系。我们对每个候选回环执行RANSAC迭代，通过Horn方法（如论文[42]）找到相似变换。如果我们用足够的有效数据找到相似变换Sil，我们就可以优化它，并搜索更多的对应关系。如果Sil有足够的有效数据，我们再优化它，直到Kl回环被接受。

C、回环融合

回环矫正的第一步是融合重复的地图点，在covisibility graph中插入与回环相关的的新边缘。先通过相似变换Sil矫正当前关键帧位姿Tiw，这种矫正方法应用于所有与Ki相邻的关键帧，这样回环两端就可以对齐。然后，回环关键帧及其近邻能观测到的所有地图云点都映射到Ki及其近邻中，并在映射的区域附近小范围内搜索它的对应匹配点，如第5部分D节所述。所有匹配的地图云点和计算Sil过程中的有效数据进行融合。融合过程中所有的关键帧将会更新它们在covisibility graph中的边缘，创建的新边缘将用于回环检测。

D、Essential Graph优化

为了有效地闭合回环，我们通过Essential Graph优化位姿图，如第三部分D节所示，这样可以将回环闭合的误差分散到图像中去。优化程序通过相似变换校正尺度偏移，如论文[6]。误差和成本计算如附录所示。优化过后，每一个地图云点都根据关键帧的校正进行变换。

 

 

 

 

 

自己的一些问题：

１．
 frame.cpp中在调用的thread在哪定义的
thread threadLeft(&Frame::ExtractORB,this,0,imLeft);
    thread threadRight(&Frame::ExtractORB,this,1,imRight);
2.关键帧cpp中使用了大量锁，？

3.keyframe.cpp 中GetConnectedKeyFrames为什么要用set
 
4.mappoint.cpp 中PC是什么向量

5.mappoint.h 中mnvisble和mnfound有啥区别  GetFoundRatio函数干嘛的  PredictScale 尺度方面还是不太懂

6.localMapping.cpp中 createnewmapPoints视差原理

7.尺度连续性是什么  局部地图cpp中有提到
8.SearchInNeighbors   局部地图cpp 什么是一级相邻关键帧和二级相邻关键帧 明白了，相邻和相邻的相邻

 

 

代码：

单目启动方式(以TUM数据集为例)：

编译后执行：

./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUMX.yaml PATH_TO_SEQUENCE_FOLDER

TUMX.yaml 为下载的数据集文件，X为1,2,3，由数据集名决定，PATH_TO_SEQUENCE_FOLDER为数据集路径

主函数在mono_tum.cpp中

mono_tum.cpp：

• 加载数据集函数 LoadImages()。由于只由于设定为单目SLAM，因此只加载TUM数据集中的rgb文件。
• 初始化System类对象：SLAM。System构造函数：加载字典和参数文件，创建关键帧数据库，创建并初始化地图(map)、创建画图(FrameDrawer、Mapdrawer)，初始化追踪线程tracking()。初始化局部地图线程并启动run()，初始化回环检测线程并启动run()，如果使用画图则初始化画图线程并启动run()。初始化完之后在进程之间设置指针。
• 按时间戳顺序遍历每一张（帧）图片，启动追踪TrackMonocular()函数。计时。停止所有线程shutdown()。
• 计算时间 保存相机轨迹。

 先看追踪TrackMonocular()：

• 加锁，判断模式，是不是只需要定位
• 判断系统是否重置
• GrabImageMonocular()函数（该函数包含追踪），加锁，更新追踪后的状态。

转到GrabImageMonocular()函数：

• 将图像转化为灰度图（注意通道数和RGB顺序 )
• 创建第一帧Frame()，第一张图像的特征参数不一样  
• 追踪Track()

 

 

tracking()构造函数：

• 加载并设定相机内参、图像矫正系数 baseline * fx、每一帧特征点数、图像金字塔变化尺度、层数、fast特征点阈值（用于特征点提取）
• 创建ORBextractor() 对象，双目则多一个右特征提取器。单目初始化时特征点数×2
• 双目和RGBD设置一个判断3D点远近的阈值： mbf * 35 / fx ？？？
• 对于深度相机，获取参数DepthMapFactor

track()函数：

• 判断是否已经初始化，否则根据相机类型初始化MonocularInitialization()、StereoInitialization()
• 判断模式（是仅仅追踪）
  • 是否需要重定位
  • 动力学位姿队列为空则追踪参考帧
  • 动力学位姿队列非空则根据恒速模型设定当前帧的初始位姿
• 追踪定位模式，且匹配点足够多
  • 是否需要重定位
  • 追踪参考帧：TrackReferenceKeyFrame()
• 该帧匹配到点太少
  • 动力学位姿队列非空则根据恒速模型设定当前帧的初始位姿
  • 重定位
  • 只要重定位成功整个跟踪过程正常进行
•  将最新的关键帧作为参考帧
• 在帧间匹配得到初始的姿态后，对local map进行跟踪得到更多的匹配：TrackLocalMap()，并优化当前位姿
• 更新绘图器
• 更新恒速模型的位姿
• 地图绘制对象设定当前相机位姿
• 排除UpdateLastFrame函数中为了跟踪增加的MapPoints
• 清除临时的MapPoints，这些MapPoints在TrackWithMotionModel()的UpdateLastFrame()函数里生成（仅双目和rgbd），这里生成的仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了
• 判断是否需要添加关键帧：NeedNewKeyFrame()，需要则：CreateNewKeyFrame();
• 删除那些在bundle adjustment中检测为outlier的3D map点
• 跟踪以及重定位都失败，则reset()
• 记录位姿信息，用于轨迹复现,如果跟踪失败，则相对位姿使用上一次值

 

MonocularInitialization()函数：

• 单目初始帧的特征点数必须大于100
• 在初始帧和当前帧之间寻找匹配的特征点SearchForInitialization()
• 如果初始化的两帧之间的匹配点太少（100以内），重新初始化
• 通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints，删除那些无法进行三角化的匹配点
• 将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵,由Rcw和tcw构造Tcw,并赋值给mTcw，mTcw为世界坐标系到该帧的变换矩阵
• 将三角化得到的3D点封装成MapPoints并更新map，并进行BA优化: CreateInitialMapMonocular()

 

 

initialize

 

局部地图 LocalMapping:: run():

• 只要进入LocalMaping队列就不再接收关键帧SetAcceptKeyFrames(false);循环执行知道队列里的帧都被处理完。
• 队列中待处理关键帧非空则构建局部地图：CheckNewKeyFrames()
• 计算关键帧特征点的BoW映射，将关键帧插入地图：ProcessNewKeyFrame()
• 剔除ProcessNewKeyFrame函数中引入的不合格MapPoints：MapPointCulling()
• 三角化新的点：CreateNewMapPoints()
• 如果处理完队列里的所有关键帧，则检查并融合当前关键帧与相邻帧重复的mapPoints，且进行局部BA优化：LocalBundleAdjustment()后剔除冗余关键帧
• 将当前帧加入到闭环检测队列
• 继续添加关键帧SetAcceptKeyFrames(true);

 

 

回环检测LoopClosing::run():

• 循环处理
• 闭环检测队列mlpLoopKeyFrameQueue中的关键帧不为空：CheckNewKeyFrames()（与LocalMap不一样）
• 闭环检测：DetectLoop()
• 计算当前帧与闭环帧的sim3变换：ComputeSim3()
• 修正位姿，更新map：CorrectLoop()

 

DetectLoop()：

• 从队列取出一帧作为当前关键帧，如果距离上次闭环少于十帧，则不闭环检测
• 得到所有共视关键帧，计算当前关键帧与共视帧的bow相似度最低分minScore
• 在所有关键帧中找出闭环候选帧集合：DetectLoopCandidates()
• 在闭环候选帧中检测具有连续性的候选帧（具体这怎么做呢？）子候选组、子连续组、当前候选组

ComputeSim3()：