ORB-SLAM
一、introduction
PTAM
- 缺少闭环检测
- 缺少对于闭塞(occlusion)的处理
- 重定位时对视角的不变性低
- 自启动(bootstrapping)时需要人工干涉
ORB SLAM
以PTAM为基础
contributions:
- 使用ORB features在tracking、mapping、relocalization、loop closing任务中,保证在视点和光照变化时的不变性。
- 使用covisibility graph,使得能在大规模环境中实时运行。
- 基于essential graph的实时闭环检测。
- 实时相机重定位
- 提出基于模型选择的自动地图初始化方法,能够在平面和非平面环境下创建初始地图。
- 使用
宽进严出,”宽进严留“ (generous in the spawning but very restrictive in the culling)的survival of the fittest方法,选取关键帧和地图点。
二、system overview
总结一下就是,tracking、local mapping、loop closing三个线程并行,这三个部分都使用ORB特征。mapping部分的关键帧和地图点存储宽进快出,数据结构比较常规。loop closing部分使用DBoW2进行匹配以检测闭环。构造了两图一树,表示关键帧之间的临近关系,这个两图一树暂时不知道是用在哪儿的。
1. feature choice
特征的选择要求:
- 每张图片的提取时间小于33ms
- 具有旋转不变性
选取ORB特征。 并在tracking、mapping、place recognition(用于重定位和闭环检测)中都使用ORB特征。
2. 线程
分为三个并行线程:tracking、local mapping、loop closing
2.1. tracking
功能:每一帧相机位姿的估计以及关键帧的检测。
步骤:
- 当前帧特征抽取,与上一帧特征匹配,得到当前位姿的初始估计。
- 若匹配失败(由于遮挡或两帧差距过大),则使用place recognition模块进行全局的重定位,得到初始位姿。
- 使用motion-only BA进行位姿估计的优化。
- 从局部地图模块得到当前视角的局部地图(由全局关键字的convisibility graph维护),根据重投影,搜索当前帧特征点和局部地图点的匹配。
- 根据得到的匹配结果,再次优化位姿估计。
- 决定当前帧是否作为关键帧插入。
2.2. local mapping
功能:处理新进入的关键帧,执行局部BA,以达到当前相机位姿附近的最优的局部重建。维护地图,加入新的点,去除低质量的点和关键帧。
步骤:
- 插入新的关键帧
- 使用exigent point culling policy去除低质量的点。
- 若当前帧的特征点和局部地图中的点不匹配,则covisibility graph的连接关键帧中搜索对应关系,三角化出新的点,插入。
- 执行局部BA。
- 去除低质量的局部关键帧。
2.3. loop closing
功能:在每个新关键帧执行闭环检测,若检测到闭环,则做调整。
步骤:
- 对新进入的关键帧执行闭环检测。
- 若检测到闭环,计算相似性转换,得到累积误差。
- 将闭环的两侧对齐,融合重复点。
- 在essential graph上,根据相似性限制,执行位姿图优化,以达到全局一致性。
3. 地图点、关键帧的描述及选取
3.1. 地图点 p i p_i pi
包含:
- 世界坐标系中的三维坐标 X w , i X_{w,i} Xw,i
- 观测方向 n i n_i ni。是所有观测到这个点的关键帧的中心视角的平均单位向量。(是只有一个吗??)
- ORB描述子 D i D_i Di。 是所有观测到它的关键帧的描述子中,汉明距离最小的一个。
- d m a x , d m i n d_{max},d_{min} dmax,dmin。(不知道是啥的距离??)
3.2. 关键帧 K i K_i Ki
包含:
- 相机位姿 T i w T_{iw} Tiw,从世界坐标系到相机坐标系。
- 相机内参,包含焦距和焦点。
- 此帧的所有ORB特征,无论是否与地图点关联。未扰动的。
3.3. 选取方式
宽容选取,紧急去除。去除冗余关键帧、误匹配地图点、漏匹配地图点。能够提高地图探索时的鲁棒性,外点少。
4. covisibility graph and essential graph
essential graph的权重阈值是多少?比spaaning tree小?
好像这不是三个图,而是两个图。
spanning tree指的就是essential graph,essential graph是covisibility graph的最小生成树(MST)。
4.1. covisibility graph
无向有权图。表示关键帧之间的相邻关系。
- 点:关键帧
- 边:若两个关键帧之间共同观测到的地图点大于15,则连一条边
- 权重:两个关键帧之间共同观测到的地图点数量
4.2. essential graph
保留covisibility graph中的全节点,保留很少量的边。
4.3. spanning tree
从最初关键帧开始持续维护的树,是covisibility graph的子图。
当新关键帧插入:此节点连接到与他有最多共同观测的节点上
当关键帧删除:更新受影响的其他节点的连接关系
essential graph包含spanning tree,这个树的权重阈值很高(100)。
5. bags of words place recognition
基于DBoW2的词袋模型,实现回环检测和重定位。词典由数据集抽取的ORB描述子离线制作。
维护一个数据集:包含单词到包含它的关键帧的索引。对于overlap的情况,将这些重叠的关键帧建成covisibility graph。(这儿不太懂,这个图是干嘛的,之前的方法是怎么做的)
trick:在对比两组ORB特征的相似性时,强制只匹配单词树特定等级的属于相同节点的特征,以加速。(???这节点是啥,单词树的等级是啥?是K叉树吗)
三、 地图自动初始化
目标:计算相邻两帧之间的位姿,用于三角化匹配点,得到其深度,以初始化地图点。
方法:同时并行两个几何模型。
- 对于平面:使用单应矩阵
- 对于非平面:使用基础矩阵
使用启发式方法选择一个模型,并使用该模型的特定方法恢复其位姿。
算法流程:
总结一下是,先提取特征点并匹配,然后同时计算单应和基础两个模型,根据得分选则其中的一个,计算出来矩阵后,根据矩阵算出相机位姿,再三角化算出匹配点深度。最后用BA优化位姿和深度。
1. 匹配特征点
对当前帧 F c F_c Fc抽取ORB特征,与参考帧 F r F_r Fr进行匹配。若匹配到的数量不足,则重置参考帧(这个重置是什么意思?)
2. 并行计算两个模型
并行的计算单应矩阵 H c r H_{cr} Hcr和基础矩阵 F c r F_{cr} Fcr。
X c T F c r X r = 0 X_c^{T} F_{cr}X_r=0 XcTFcrXr=0
X C = H c r X r X_C=H_{cr}X_r XC=HcrXr
在RANSAC迭代中,使用DLT和8点法分别计算单应矩阵和基础矩阵,二者的迭代次数固定且相同,每次迭代用的点分别是4和8。每次迭代,使用对称转换误差(symmetric transfer errors)计算模型分数 S M S_M SM,并维护最高得分。
3. 模型选择
选取标准:
R H = S H S H + S F R_H=\frac {S_H}{S_H+S_F} RH=SH+SFSH
平面、低视差,选择单应模型, R H > 0.45 R_H>0.45 RH>0.45。
非平面、高视差,选择基础矩阵模型, R H < = 0.45 R_H<=0.45 RH<=0.45。
4. 相机运动与结构恢复
选择相应模型并计算,可以得到矩阵。根据这个矩阵求出相机运动(R,t),三角化得到点的深度。
对于单应矩阵模型:使用参考文献[23]的方法得到8种可能的假设,对其分别三角化,选取低视差、景深为正(点在相机前面)、低重投影误差的假设。
对于基础矩阵模型:使用内参矩阵 K K K将其转为本质矩阵。
E r c = K T F r c K E_{rc}=K^{T}F_{rc}K Erc=KTFrcK
使用奇异值分解得到4种可能的假设,对其分别三角化,选取景深为正的假设。
5. BA
使用BA优化初始位姿和深度。
四、tracking
此部分的整个系统始终运行的平行三线程之一,其作用是:对每一帧,估计其相机位姿,选择新的关键帧。
(这部分和初始化的关系是啥?初始化只用了前面一部分的帧,初始化了一部分的地图点吗?初始化部分是用单目2D-2D的方式,然后再 通过tracking,使用3D-2D的方式,估计随后的每一帧位姿?)
(这里的提取ORB特征,和初始化部分的应该是不一样的,这里在八个尺度上进行,初始化部分只在一个尺度上,这里得到的ORB描述子,应该也没用于初始化?但是初始化用的是啥呢,初始化的“当前帧”和这里的“新帧”不是一个吗?)
基于前一帧的位姿初始化,是建立当前帧与上一帧之间的联系;基于全局重定位的位姿初始化,是建立当前帧与某一关键帧的联系;基于局部地图的位姿估计,是建立当前帧与局部关键帧地图的联系,也就是通过搜索更多的3D-2D的匹配点来对更精确的估计当前帧的位姿。
1. 提取ORB特征
先对图像提取固定量的FAST角点,尽量保证角点分布均匀。然后计算焦点的朝向和ORB描述子。
最终得到ORB描述子,用于所有模块的特征匹配。
2. 根据前一帧初始化相机位姿
如果前一帧的估计成功,则使用匀速运动模型(constant velocity motion model)估计当前帧的位姿,并使用guided search在当前帧中搜索上一帧的地图点。这个模型是认为相机处于匀速运动,根据匀速运动来估计当前帧位姿的初始值。
之后根据搜索到的点匹配关系(3D-2D),优化当前帧的相机位姿。若没有搜索到足够的匹配点,则在地图点的周围搜索,看有没有匹配。
3. 根据全局重定位初始化相机位姿
若前一帧丢失,则把当前帧转化为词袋向量,查询关键帧数据库实现全局重定位。
对于数据库中的每一个关键帧,使用RANSAC和PnP算法,估计相机初始位姿,若由足够多的内点, 就优化这个位姿(咋优化的???),并根据这个关键帧,使用guided search,找到匹配的3D-2D点,再次优化相机位姿。若内点足够多,当前帧则被tracking到。
4. 基于局部地图的追踪
得到初始化的相机位姿和3D-2D匹配点集后,根据局部地图丰富匹配点集,对相机位姿再次优化。
局部地图的选取:与当前帧具有相同观测地图点的关键帧、以及这些关键帧在covisibility graph上的邻居关键帧。其中,与当前帧有最多共同观测地图点的关键帧,就是参考帧。
对这些关键帧观测到的每一个地图点:
- 计算地图点到当前帧上的投影,若超出边界,不要。
- 计算当前帧视角 v v v与地图点的平均视角方向 n n n(是地图点数据结构中的一个成员)的夹角,若 v ⋅ n < c o s 6 0 ° v·n<cos60^{°} v⋅n<cos60°,不要。
- 计算地图点到当前帧相机中心的距离 d d d,若不在地图点的区间 [ d m i n , d m a x ] [d_{min},d_{max}] [dmin,dmax]内(也是地图点数据结构的一个成员),不要。
- 计算尺度, d / d m i n d/d_{min} d/dmin
- 笔记地图点的描述子 D D D(还是它数据结构的一个成员),和当前帧未匹配的ORB特征描述子,选择尺度因子和距离最近的作为最优匹配对(什么意思???),把这组匹配加入当前帧的3D-2D匹配集。
遍历完成后,用丰富的3D-2D匹配集再次优化当前帧的位姿。
5. 选取新关键帧
用于决策当前帧是否作为关键帧加入,其条件是:
- 与上次全局重定位间隔20帧以上。目的是确保良好的重定位。
- 局部地图线程空闲,或与上一次关键帧插入间隔20帧以上。目的是不打扰局部地图线程,不太频繁的更新地图。
- 当前帧追踪了至少50个地图点。目的是确保良好的追踪,当前帧的质量要好。
- 当前帧追踪的地图点在其参考帧的90%以下。目的是确保最小的视角变化。(这不太懂,这样岂不是每一个关键帧的点都越来越少?)
五、local mapping
这部分的平行三线程之一,它的任务是:处理每一个新关键帧,在其插入的时候,管理地图点和关键帧的集合,进行局部BA优化。
1. 关键帧插入
关键帧插入的时候,要更新以下内容:
- covisibility graph。新关键帧作为新节点插入,并与所有与其有共同观测的节点连边。
- spanning tree。连接到与与其有最多共同观测的节点上。
- 词袋向量。计算其词袋向量。
2. 地图点剔除
地图点剔除策略是:在其创建后的前三个关键帧之内,对其稳定性测试。过了这段视时间后,进入稳定状态,不容易被剔除。
创建后前三帧内的稳定性条件:
- 追踪系统预测,在至少25%的帧中,这个点都能被观测到。(这个帧比关键帧要密集的多!)
- 该地图点创建后,至少有连续三个关键帧观测到它。
稳定状态后的剔除条件:
- 任意时间,观测到它的关键帧少于3。
- 关键帧被剔除。
- 局部BA把它当作外点而丢弃。
3. 地图点创建
K i K_i Ki:当前关键帧
K c K_c Kc:covisibility graph中与 K i K_i Ki相连的关键帧
对于每一个 K i K_i Ki中未匹配的ORB特征,在 K c K_c Kc中寻找它的匹配点,并丢弃不满足堆积约束的ORB特征。找到匹配对后,对其三角化。
若其满足:
- 景深为正
- 视差条件
- 重投影误差条件
- 尺度一致性
则作为新地图点加入。
如果这个新创建的地图点还能被其他帧匹配到,就把它当作3D-2D对,用于优化那一帧的相机位姿[四-4]。
4. 局部BA
优化的目标有:
- 当前关键帧 K i K_i Ki
- 与 K i K_i Ki相连的关键帧 K c K_c Kc
- K i K_i Ki和 K c K_c Kc能观测到的所有地图点
同时,能观测到这些被优化的地图点的关键帧,也参与优化,但其值固定不变。
过程中会抛弃被判定为外点的地图点。
5. 关键帧剔除
若一个关键帧观测到的90%的地图点,能被其他三个关键帧观测到,则剔除这个关键帧。
六、loop closing
闭环检测部分对于当前最后一个被local mapping线程处理完的关键帧, 进行回环的检测和优化。
这部分看不太懂啊。
大概是,先进行闭环检测,先检测候选帧,从候选帧中检测是不是闭环帧。
检测到闭环帧之后,消除累积误差。通过计算得到的相似性矩阵,调整当前帧局部的关键帧位姿、调整它们观测到的地图点、调整他们相关的covisibility graph、最后通过位姿图优化调整全局的关键帧位姿和地图点。
1. 闭环检测
首先检测闭环候选帧:
- 根据词袋向量,计算当前帧与其所有covisibility graph上相邻帧的相似性,得到最小值 s m i n s_{min} smin
- 查询关键帧数据库,所有所有相似性得分小于 s m i n s_{min} smin的关键帧都排除
- 排除与当前帧直接相连的关键帧
- 其他关键帧作为闭环候选帧
确定闭环帧:
- 若连续三个候选闭环帧是一致的(???),则作为闭环帧。
2. 计算相似变换
单目slam中有七个自由度可以累积漂移:三个平移、三个旋转、一个缩放。
计算当前帧和闭环帧的相似变换,可以得知闭环中的累积误差,也能用来验证闭环的有效性(确定闭环候选帧是不是闭环帧吗?)。
首先计算当前帧和闭环帧分别对应的地图点,其ORB特征的相似性。(闭环帧和当前帧虽然是同一个地方,但是却生成了不同的地图点,要找出来这些相同真实点的不同地图点的匹配对?)得到3D-3D点对,对于每一个闭环候选帧(为啥这里还是候选帧??)使用RANSAC算法迭代,计算相似性矩阵。
若计算的相似性矩阵有足够的内点,就优化这个候选帧,然后使用guided search,得到更多的匹配点对,再次优化,若计算得到的相似性矩阵仍然有足够的内点,则选为闭环帧。
3. 闭环融合
检测到闭环,要用闭环帧纠正当前帧的错误。
调整关键帧位姿:
当前帧的位姿 T i w T_{iw} Tiw,被相似性矩阵作用,调整其位姿估计。covisibility graph中与当前帧相连的关键帧,其位姿也会被调整。
调整地图点:
闭环帧和其邻居观测到的地图点,都被投影到当前帧和其邻居中,并寻找3D-3D匹配对。这些被匹配到的地图点,和相似性矩阵中的内点进行融合。
更新covisibility graph:
所有参与融合的关键帧,都更新其边的信息(?),并创建与回环帧相连的边(地图点融合了,这个帧就能看到地图点,就与回环帧有边了)。
4. essential graph优化
调整全局:
为了有效的闭合回环,在essential graph上进行位姿图优化。优化后,对每个地图点,根据其中一个观测到它的关键帧的校长,对其进行变换。
七、 实验
进行了三组实验:
- NewCollege Dataset,考察系统整体表现
- TUM RGB-D,考察定位精度、重定位、生存期
- KITTI,考察实时大尺度场景操作、定位精度、图优化效率
评测标准:
- 定位精度:ATE(absolute trajectory error)
- 轨迹精度:RMSE
1. NewCollege Dataset
20fps, 512*382, 存在闭环和视角的快速旋转。
2. TUM RGB-D Benchmark
有轨迹标签,适于评测定位精度。测试时,去除了弱纹理、纯旋转、无运动的部分。
3. KITTI
有高精度的轨迹标签,含有回环。