论文摘取
(这部分看的是泡泡机器人的翻译)
基于特征点、单目、完全自动初始化,基于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算法执行闭环检测和重定位
视觉词典
关键帧重叠问题
字典树强制匹配加快特征点匹配,用于三角化新的点云,闭环检测和重定位
引入方向一致性改进匹配点
地图自动初始化
分为平面视图(单映矩阵)和非平面视图(基础矩阵),启发式方法选择模型。算法步骤:
- 查找初始点对(ORB)匹配
- 并行计算两个模型:单映矩阵和基础矩阵,前者为四对特征点,后者为8对特征点,每次迭代时给每个模型计算一个分值SM,从单映矩阵和基础矩阵中选择分值高的。分值都不高则重启
- 模型选择:启发式计算RH
- 运动和从运动到结构的重构:单映矩阵是否能执行,基础矩阵计算本证矩阵后奇异值分解,见slam14讲7
- BA:全局BA优化,见附录
跟踪
A.ORB特征提取
在8层图像金字塔上提取FAST角点,金字塔尺度因子为1.2。如果图像分辨率较高,则适当增加角点数。根据角点计算ORB特征描述子。用于后续特征匹配。
B.通过前一图像帧估计相机的初始位姿
如果上一帧图像跟踪成功,则认为摄像头处于匀速运动。利用前后帧的匹配点对当前相机进行进一步优化。未找到足够匹配点则扩大搜索范围。
C.通过全局重定位来初始化位姿
若扩大搜索范围依然找不到足够匹配点,则计算当前帧的词袋向量选取若干帧备选,对于每个备选帧执行pnp算法计算当前帧的位姿(Ransac迭代求解)。如果找到一个姿态能够涵盖足够多的有效点,则搜索该关键帧对应的更多电云。最后基于找到的所有匹配点对相机位姿进行优化。
D.跟踪局部地图
当获得了相机位姿和一组初始的匹配点,则可以将更多的点云投影到图像上以找到更多的匹配点。为了降低复杂度,只映射局部地图。该局部地图包含一组关键帧K1,它们和当前关键帧有共同的地图点,还包括与关键帧K1在covisibility graph中相邻的一组关键帧K2,局部地图中有一参考关键帧Kref∈K1,它与当前帧有最多的共同地图点。对K1和K2的每个地图点,在当前帧下做如下搜索:
- 计算地图点云在当前帧图像中的投影点x,超出图像边缘则删除。
- 计算当前视图射线v和地图点云平均视图方向n的夹角,如果n<cos(60),删除对应点云。
- 计算地图点云到相机中心的距离d,若其不在地图点云的尺度不变区间内,即d∉[d_min,d_max],删除该点。
- 计算每帧图像的尺度比d/d_min
- 对比地图中点的特征描述子D和当前帧中还未匹配的ORB特征,在预测的尺度层和靠近x的点做最优匹配。
相机位姿最后通过当前帧获得所有的地图点进行优化。(目的是在当前点帧和局部地图之间找到更多的匹配点对优化位姿)。
E.新关键帧的判断标准
- 距离上一次全局重定位后需要超过20帧图像
- 局部地图构建处于空闲状态,或距上一个关键帧插入后,已有超过20帧图像
- 当前帧跟踪少于50个地图点
- 当前帧跟踪少于参考关键帧K_ref点的90%
局部地图构建
A.关键帧插入
首先更新covisibility graph,包括:添加一个关键帧节点Ki,检查与Ki有共同点的其他关键帧,用边线连接。计算该关键帧的词袋,并利用三角法生成新的地图点。
B.地图点云筛选
三角化的点为了保留在地图中,必须在其创建后的头三个关键帧中通过一个严格的测试,该测试确保留下的云点都是能被跟踪的,不是由于错误的数据而被三角化的。该点必须满足以下条件:
- 跟踪线程必须在超过25%的图像中找到该特征点
- 如果创建地图点经过了多个关键帧,那么它必须至少是能够被其他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]。误差和成本计算如附录所示。优化过后,每一个地图云点都根据关键帧的校正进行变换。
自己的一些问题:
1.
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()
- 记录位姿信息,用于轨迹复现,如果跟踪失败,则相对位姿使用上一次值
TrackLocalMap()函数:
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():
- 初始化一个Sim3Solver列表
- 对于每一闭环候选帧,将当前帧mpCurrentKF与闭环候选关键帧pKF匹配
- 通过Bow加速获取mpCurrentKF与pKF之间的匹配特征点,如果匹配点过少,则将该候选帧剔除
- 否则构造Sim3求解器设定Ransac迭代参数,每个候选帧最多迭代五次,总迭代次数达到最大限制还没有求出合格的Sim3变换,该候选帧剔除
- 上一步通过Sim3求解器求出当前候选闭环帧pKF与当前帧的R、t、s(变换尺度)。之后通过描述子进行匹配,匹配更多的特征点,更新mvpMapPointMatches。
- 更新vpMapPointMatches后再根据Sim3优化。如果求出的内点超过20,则当前候选帧即为选出来的闭环帧。
- 得到从世界坐标系到该候选帧的Sim3变换,Scale=1。跳出遍历
- 取出闭环帧的相连关键帧,得到它们的MapPoints放入mvpLoopMapPoints。
- 将闭环匹配上关键帧以及相连关键帧的MapPoints投影到当前关键帧进行投影匹配SearchByProjection():根据Sim3变换,将每个mvpLoopMapPoints投影到mpCurrentKF上,并根据尺度确定一个搜索区域,根据该MapPoint的描述子与该区域内的特征点进行匹配,如果匹配误差小于TH_LOW即匹配成功,更新mvpCurrentMatchedPoints。
- 判断当前帧与检测出的所有闭环关键帧是否有足够多的MapPoints匹配
- 清空mvpEnoughConsistentCandidates
CorrectLoop():
- 停止局部地图,防止插入新关键帧
- 根据共视关系更新当前帧与其它关键帧之间的连接UpdateConnections()
- 通过位姿传播,得到Sim3优化后,与当前帧相连的关键帧的位姿,以及它们的MapPoints
- 先将mpCurrentKF的Sim3变换存入,固定不动,其它的关键帧根据相对关系得到Sim3调整的位姿,得到闭环g2o优化后各个关键帧的位姿
- 得到调整相连帧位姿后,修正这些关键帧的MapPoints
- 将Sim3转换为SE3,根据更新的Sim3,更新关键帧的位姿
- 根据共视关系更新当前帧与其它关键帧之间的连接
- 检查当前帧的MapPoints与闭环匹配帧的MapPoints是否存在冲突,对冲突的MapPoints进行替换或填补,如果有重复的MapPoint(当前帧和匹配帧各有一个),则用匹配帧的代替现有的
- 通过将闭环时相连关键帧的mvpLoopMapPoints投影到这些关键帧中,进行MapPoints检查与替换SearchAndFuse()
- 更新当前关键帧之间的共视相连关系,得到因闭环时MapPoints融合而新得到的连接关系
- 进行EssentialGraph优化,LoopConnections是形成闭环后新生成的连接关系,不包括步骤7中当前帧与闭环匹配帧之间的连接关系
- 添加当前帧与闭环匹配帧之间的边(这个连接关系不优化)
- 新建一个线程用于全局BA优化RunGlobalBundleAdjustment()