码迷,mamicode.com
首页 > 其他好文 > 详细

ORB随便记一记

时间:2019-09-27 10:35:16      阅读:482      评论:0      收藏:0      [点我收藏+]

标签:das   app   增加   添加   队列   涵盖   提取   并行计算   运行时   

论文摘取

(这部分看的是泡泡机器人的翻译)

基于特征点、单目、完全自动初始化,基于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]。误差和成本计算如附录所示。优化过后,每一个地图云点都根据关键帧的校正进行变换。

 

 

 

 

 

自己的一些问题:

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()
  • 记录位姿信息,用于轨迹复现,如果跟踪失败,则相对位姿使用上一次值

 

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():

 

ORB随便记一记

标签:das   app   增加   添加   队列   涵盖   提取   并行计算   运行时   

原文地址:https://www.cnblogs.com/dlutjwh/p/11548156.html

(0)
(0)
   
举报
评论 一句话评论(0
登录后才能评论!
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码!