CN111707281B - 一种基于光度信息和orb特征的slam系统 - Google Patents
一种基于光度信息和orb特征的slam系统 Download PDFInfo
- Publication number
- CN111707281B CN111707281B CN202010616900.4A CN202010616900A CN111707281B CN 111707281 B CN111707281 B CN 111707281B CN 202010616900 A CN202010616900 A CN 202010616900A CN 111707281 B CN111707281 B CN 111707281B
- Authority
- CN
- China
- Prior art keywords
- pose
- map
- tracking
- frame
- optimization
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000005457 optimization Methods 0.000 claims abstract description 48
- 230000033001 locomotion Effects 0.000 claims abstract description 26
- 230000000007 visual effect Effects 0.000 claims abstract description 16
- 238000001514 detection method Methods 0.000 claims abstract description 7
- 238000000034 method Methods 0.000 claims description 26
- 238000013507 mapping Methods 0.000 claims description 14
- 230000008569 process Effects 0.000 claims description 11
- AYFVYJQAPQTCCC-GBXIJSLDSA-N L-threonine Chemical compound C[C@@H](O)[C@H](N)C(O)=O AYFVYJQAPQTCCC-GBXIJSLDSA-N 0.000 claims description 6
- 230000004927 fusion Effects 0.000 claims description 4
- 239000000284 extract Substances 0.000 claims description 3
- 230000002159 abnormal effect Effects 0.000 claims description 2
- 239000011159 matrix material Substances 0.000 description 16
- 230000009466 transformation Effects 0.000 description 14
- 230000006870 function Effects 0.000 description 12
- 238000002474 experimental method Methods 0.000 description 9
- 230000000694 effects Effects 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 102000008115 Signaling Lymphocytic Activation Molecule Family Member 1 Human genes 0.000 description 3
- 108010074687 Signaling Lymphocytic Activation Molecule Family Member 1 Proteins 0.000 description 3
- 238000010276 construction Methods 0.000 description 3
- 230000004807 localization Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 238000003780 insertion Methods 0.000 description 2
- 230000037431 insertion Effects 0.000 description 2
- 238000007781 pre-processing Methods 0.000 description 2
- 230000005856 abnormality Effects 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 230000002411 adverse Effects 0.000 description 1
- 230000003190 augmentative effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- NMVPEQXCMGEDNH-TZVUEUGBSA-N ceftazidime pentahydrate Chemical compound O.O.O.O.O.S([C@@H]1[C@@H](C(N1C=1C([O-])=O)=O)NC(=O)\C(=N/OC(C)(C)C(O)=O)C=2N=C(N)SC=2)CC=1C[N+]1=CC=CC=C1 NMVPEQXCMGEDNH-TZVUEUGBSA-N 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000009877 rendering Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
- G01C21/32—Structuring or formatting of map data
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Image Analysis (AREA)
Abstract
本发明涉及一种基于光度信息和ORB特征的SLAM系统，包括：前端视觉里程计模块，基于输入的RGB‑D流，提取ORB特征，跟踪前一帧以及局部地图实现相机位姿优化，确定新关键帧，所述相机位姿优化中，选择重定位模型、匀速运动模型或者基于光度信息的轻量级位粗估计模型中的一个为当前帧设置相机位姿初值；局部建图模块，由所述新关键帧对地图点进行更新，基于新关键帧、与它相连接的关键帧位姿以及地图点位置进行局部优化；后端优化模块，对于每一个关键帧进行回环检测，实现全局优化。与现有技术相比，本发明具有鲁棒、准确等优点。
Description
技术领域
本发明涉及机器人同时定位与地图构建技术领域，尤其是涉及一种基于光度信息和ORB特征的SLAM系统。
背景技术
在环境勘探工程中，机器人在较空旷环境及非光滑表面行进，通过双目或深度相机对环境进行勘探，并需要通过采集得到的图片及视频对三维地形进行重建。
Simultaneous Localization and Mapping(SLAM)是同时定位与地图构建技术，它能够通过传感器在未知环境中的运动，对传感器进行实时定位，同时得到未知环境的三维结构。SLAM根据其中使用的传感器可以大体分为激光SLAM和视觉SLAM。视觉SLAM由于其使用的彩色或深度相机等在价格、便捷性和通用性等方面的突出优势，得到了越来越多的关注。视觉SLAM在机器人、增强现实和自动驾驶等领域具有广阔的应用前景。
但是，现有SLAM系统还存在因为相机抖动、非匀速的运动、快速的旋转、场景结构单一以及纹理简单等问题而导致跟踪失败的情况。如现有的一种ORB-SLAM2系统，在相机发生抖动以及特征点数量较少(场景空旷简单)的情况下，极易发生跟踪失败的问题。而其系统本身自带的重定位功能在跟踪失败发生时，只能在回到之前经过的某个位置启动重新跟踪。这样，当相机在新的场景中持续运动时则会彻底跟踪失败。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种鲁棒的基于光度信息和ORB特征的SLAM系统。
本发明的目的可以通过以下技术方案来实现：
一种基于光度信息和ORB特征的SLAM系统，包括：
前端视觉里程计模块，基于输入的RGB-D流，提取ORB特征，跟踪前一帧以及局部地图实现相机位姿优化，确定新关键帧，所述相机位姿优化中，选择重定位模型、匀速运动模型或者基于光度信息的轻量级位粗估计模型中的一个为当前帧设置相机位姿初值；
局部建图模块，由所述新关键帧对地图点进行更新，基于新关键帧、与它相连接的关键帧位姿以及地图点位置进行局部优化；
后端优化模块，对于每一个关键帧进行回环检测，实现全局优化。
进一步地，当跟踪失败时，采用重定位模型估算相机位姿初值；
当相机在运动过程中基本保持匀速状态时，采用匀速运动模型计算相机位姿初值；
当相机进行非匀速运动时，采用轻量级位粗估计模型计算相机位姿初值。
进一步地，所述轻量级位粗估计模型，提取前一帧图像梯度大于阈值的像素，通过投影方法投影至当前帧平面，优化光度误差，通过增量迭代求解，得到所述相机位姿初值。
进一步地，所述前端视觉里程计模块包括：
跟踪判断单元，基于当前帧的所述ORB特征获得与前一帧或者局部地图的匹配点对，在相机位姿优化后，判断优化后的相机位姿的内点数量是否大于设定阈值Nthre，若是，则跟踪成功，否则跟踪失败。
进一步地，当跟踪成功时，在当前帧同时满足以下条件时确定当前帧为新关键帧：
1)距离上次重定位超过20帧；
2)当局部建图模块空闲时或者距离上一次插入关键帧已超过20帧；
3)当前帧跟踪了超过50个点；
4)当前帧对相关关键帧跟踪成功的点少于90％；
当跟踪失败时，以跟踪前一帧以及局部地图得到位姿Tcwf和RPE模型得到的位姿Tcwr按一定权重相结合得到一个估计值作为当前帧的位姿，并判定当前帧为关键帧。
进一步地，所述位姿Tcwf的权值Wf为：
其中，MapMatches为位姿为Tcwf时跟踪局部地图的内点数量，Nthre为设定阈值。
进一步地，所述局部建图模块中，对地图点进行更新具体为：
产生新地图点、删除异常地图点和删除被少于三个关键帧检测到的地图点。
进一步地，所述局部建图模块采用的优化目标函数为重投影误差函数。
进一步地，所述后端优化模块中，若检测到回环，则将新的关键帧与之前对应的关键帧和共视地图点关联起来，校正关键帧的位姿和地图点的位置信息，对关联起来的关键帧与地图进行全局优化。
进一步地，所述后端优化模块中，根据优化后的位姿融合关键帧点云，使用VoxelGrid滤波器进行下采样，得到稠密的点云地图。
与现有技术相比，本发明具有如下有益效果：
1、本发明充分利用了图片的光度和深度信息，在容易跟踪失败的场景下能够持续地提供稳定且较为准确的位姿估计。
2、本发明局部建图模块根据新加入的关键帧对地图点进行更新，删除掉一些不能稳定跟踪的点，提高准确度，同时为了防止过多关键帧对优化过程造成不良的影响，删减已有的关键帧。
3、本发明对关键帧的确定设定多个条件，保证了较好的重定位效果和跟踪效果。
附图说明
图1为本发明的流程示意图；
图2为实施例中R-ORB SLAM系统与ORB-SLAM2在5个序列中的跟踪结果路径俯视图，其中，(2a)～(2e)分别为fr2/360、fr2/slam、fr2/slam3、fr3/ns_nt_far和fr3/s_nt_far序列；
图3为ORB-SLAM2系统和R-ORB SLAM系统在序列fr2/360序列上的效果示例，其中，(3a)为ORB-SLAM2系统，(3b)为R-ORB SLAM系统；
图4为kt00序列的某一角度重建结果，其中，(4a)为kt00序列的真实3D模型中一个表面，(4b)为未使用Poisson表面重建算法时R-ORB SLAM得到的稠密点云，(4c)为使用Poisson表面重建算法的重建结果，(4d)为Ours+TSDF的重建结果。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。
如图1所示，本发明提供一种基于光度信息和ORB特征的SLAM系统(R-ORB SLAM系统)，主要包括前端视觉里程计、局部建图和后端优化三个模块，采用轻量级的位姿估计作为特征视觉里程计的先验，在跟踪失败的情况下，将先验位姿的结果参与位姿估计；在建图模块中，通过相机模型计算得到关键帧点云，对每个关键帧点云进行拼接得到全局点云地图，并使用VoxelGrid滤波器进行下采样，得到稠密三维点云地图，对点云地图使用Poisson算法实现表面重建，得到三维Mesh地图。其中：
前端视觉里程计模块对输入的图片流进行预处理，并通过重定位、运动模型或者RPE模型为当前帧设置相机位姿初值，通过跟踪前一帧以及局部地图优化相机位姿，根据优化后匹配成功的地图点数量判断是否跟踪成功，若跟踪成功则判断该帧是否为关键帧，否则将计算出的初始位姿和优化后的位姿根据权重模型计算当前帧位姿结果，并将当前帧设置为关键帧，系统得到新的关键帧后进入局部建图线程。
局部建图模块中根据新加入的关键帧对地图点进行更新，删除掉一些不能稳定跟踪的点，加入当前关键帧的新地图点，并对新关键帧以及与它相连接的一些关键帧的位姿以及一些地图点的位置进行局部BA优化。优化后，为了防止过多关键帧对优化过程造成不良的影响，需要删减已有的关键帧。
后端优化模块中，对于每一个关键帧，都需进行回环检测判断相机是否运动到曾经的位置上，以减少随相机运动而产生的漂移误差。若检测到回环则将新的关键帧与之前对应的关键帧和共视地图点关联起来，并校正关键帧的位姿和地图点的位置信息，最后对关联起来的关键帧与地图进行全局优化。
此外，本发明为了重建全局一致的地图，在建图线程中，根据优化后的位姿融合关键帧点云，使用VoxelGrid滤波器进行下采样，得到稠密的点云地图。
1、前端视觉里程计
前端视觉里程计模块的作用是根据相邻图片之间的对应关系，估算相邻帧之间传感器的运动以及局部地图。R-ORB SLAM的视觉里程计基于ORB特征，主要包括初始位姿估计、ORB特征提取与匹配、跟踪前一帧和局部地图、权重模型以及关键帧决策。
1.1初始位姿估计
前端视觉里程计模块在对图片进行校正等预处理后，会为后续的位姿优化设置一个初始值，这个初始值由重定位模型、匀速运动模型或RPE模型决定。重定位模型为当跟踪失败时，在词袋数据库中搜索类似的关键帧并使用EPnP算法估算位姿，通过内点数量判断重定位是否成功。匀速运动模型则默认相机在运动过程中基本保持匀速状态，该帧的运动与前一帧的运动相等，从而计算出位姿作为当前帧初始位姿。而当相机进行非匀速运动时，R-ORB SLAM采用一个基于光度信息的轻量级位粗估计模型(Roughly Pose Estimation，RPE)来为位姿优化设定初值。该模型提取前一帧图像梯度大于阈值的像素，并做出灰度不变的假设，即假设一个空间点在各个视角下成像的灰度是不变的。将前一帧的任一点通过投影方法投影至当前帧平面，优化光度误差，公式如下：
其中：K为相机内参矩阵；xi为前一帧中图像梯度大于阈值的像素，Xi为前一帧的地图点；I(·)表示获取某像素的亮度，指数映射exp(ξ^)如下：
其中，ξ为相机位姿的李代数表示，R为旋转矩阵，t为平移向量，T为变换矩阵，φ＝θa，θ为φ的模长，a为φ的单位方向向量。
Z2为Xi经过exp(ξ^)变换后第二帧相机坐标系下的Z轴坐标。经过变换，给相机位姿添加一个小扰动exp(δξ^)得到每一点的光度误差：
其中，X、Y、Z为地图点的三维坐标，
使用该式计算优化问题的雅可比矩阵，然后使用高斯牛顿法或者Levenberg-Marquardt方法计算增量迭代求解，得到初始位姿。
1.2ORB特征提取与匹配
前端视觉里程计模块在得到初始位姿后，对图片提取特征并对图片之间的特征点进行匹配。根据SLAM的要求，考虑实时性及成本，本发明选择ORB特征进行数据关联。ORB特征使用的关键点是一种添加了旋转不定性的FAST角点Oriented FAST，它的描述子是BRIEF(Binary Robust Independent Elementary Feature)。
1.3跟踪前一帧以及局部地图
在得到当前帧与前一帧或者局部地图的匹配点对后使用g2o优化相机位姿。在使用前一帧以及局部地图对当前帧位姿进行优化后，根据该位姿的内点数量判断跟踪是否成功。若内点数量大于Nthre，则基于ORB特征跟踪成功，否则，跟踪失败。
1.4权重模型以及关键帧判断
当跟踪成功时，进行关键帧判断，判断该帧是否为关键帧。由于后续还可以删除冗余关键帧，故在前端视觉里程计中插入较多关键帧，以使系统更具有鲁棒性。判定当前帧为关键帧的4个条件如下：
1)距离上次重定位超过20帧，这是为了保证较好的重定位效果；
2)当局部建图模块空闲时或者距离上一次插入关键帧已超过20帧，这是为了保证建图模块能处理每个关键帧；
3)当前帧跟踪了超过50个点，这是为了保证较好的跟踪效果；
4)当前帧对相关关键帧跟踪成功的点少于90％，这是为了避免太过频繁插入相似度较高的帧。
当当前帧同时满足以上4个条件时，则判定当前帧为关键帧。
当传感器违背了运动模型做不规律的运动时，使用运动模型得到的初始位姿不能较好的为后续优化提供先验，导致跟踪失败问题。通常这样的情况并不发生在相机整个运动路线中，而只是局部发生。针对这种情况，结合RPE模型，R-ORB SLAM设计了一个权重模型，将跟踪前一帧以及局部地图得到的不够准确的位姿Tcwf和RPE模型得到的位姿Tcwr相结合得到一个估计值，作为跟踪失败时当前帧的位姿，并判定当前帧为关键帧。
MapMatches为位姿为Tcwf时跟踪局部地图的内点数量，则Tcwf的权值Wf为：
RPE模型的结果Tcwr的权值Wr为：
Wr＝1-Wf (7)
那么当前帧的位姿Tcw设置为：
Tcw＝WfTcwf+WrTcwr (8)
2、局部建图
2.1插入新关键帧
当局部建图模块接收到新关键帧KFi时，需将该关键帧KFi插入到局部地图中。首先，需要更新共视图和生成树。接着，计算新关键帧的词袋表示。R-ORB SLAM系统使用DBoW2词袋模型库。
插入新的关键帧，会加入新的地图点，在此之前需对已存在的地图点进行筛选，删除掉部分可能造成异常的地图点。原则上，一个关键点生成后需接受后续关键帧的检验，若不能被较多关键帧检测到，则会删除该地图点。而已经生成较久的地图点仍需保证至少被三个关键帧检测成功。对于当前关键帧中未与局部地图点匹配成功的其他关键点，若能在其他关键帧中观测到，则将该点作为新地图点加入局部地图。
2.2局部BA优化
加入新关键帧KFi后，同时对该关键帧位姿、共视图中与该关键帧相连的所有节点关键帧位姿以及他们所观测到的地图点位置进行优化。优化目标函数为重投影误差函数，如式(9)所示：
其中，fpoint为由式(10)计算的重投影误差，Hpoint为对应的鲁棒核函数。
其中TcwPw为匹配成功的世界坐标系中的3D点在相机坐标系下的位置，um为该点对应的当前帧像素坐标，ρ函数为投影函数。
BA优化的复杂度随关键帧的数量增加而增加，所以经过局部BA优化后，需对关键帧进行筛选，删除局部地图中部分冗余的关键帧，当某一关键帧中90％的地图点可以被其他三个关键帧观测到，则删除该关键帧。
3、后端优化与建图
3.1回环检测与优化
传感器在探测过程中可能会回到之前经过的位置，回环检测就是为了判断出传感器是否又回到了之前的位置，为系统提供更多的约束，从而减少累积漂移误差，获得全局一致的估计。由式(11)得到，vi表示词袋向量，计算共视图中与当前关键帧KFi共视地图点数量大于30的关键帧的相似度，取其中的最小相似度为smin，所有关键帧中与KFi相似度大于smin且并不与KFi直接相连的为回环候选关键帧。
对所有回环候选关键帧KFi，进行ORB特征匹配，xi,a和xl,a为某一匹配点对的像素坐标，xi,a为该特征点在关键帧KFi坐标系下齐次坐标，xl,a为该特征点在KFi坐标系下齐次坐标，使用RANSAC方法迭代求得相似变换矩阵Sil，并对该变换矩阵进行优化，一对匹配点将产生一对误差项ei,a和el,a：
ei,a＝xi.a-π(SilXl,a) (12)
则优化的目标函数为：
Ω为相关的协方差矩阵。优化结束后得到内点大于40的相似变换矩阵，则接受该回环进入回环融合。
回环融合将重复的地图点融合起来，将当前关键帧观测到的地图点和回环关键帧的地图点关联起来，由于累积漂移问题不仅存在于相机位姿上也存在于地图点中，故越早确立的地图点信息更为准确。在共视图中加上回环的边，然后根据相似变换矩阵调整新关键帧相邻节点关键帧的位姿，最后构成本质图，本质图中保存所有关键帧节点，但仅在两节点共视地图点数量超过100时连接两节点。
构成本质图后，对本质图中节点进行位姿优化，将回环误差均摊到所有关键帧中，为了减少尺度漂移此处的优化基于关键帧之间的相似变换矩阵。误差项如下：
Slw,Siw为i帧和l帧相对于世界坐标系的变换矩阵，此时优化的目标函数为
Λi,l为协方差矩阵。最后，对地图中所有的关键帧与地图点进行全局优化，该部分目标函数与式(9)一致，唯一的区别是此处优化对象为所有的关键帧与地图点。
3.2建图
在前部分的地图中只保留一些在跟踪过程中被反复检测并优化的特征三维点，简单增加跟踪过程中的地图点，不仅会降低跟踪的速度，还会增加一些可能错误的约束导致优化失败，故需在后端优化模块中添加建图单元建立全局一致的稠密地图。本发明将建图和跟踪分离开来，由建图线程单独进行建图。
使用点云拼接的方式建立稠密的点云地图，以便后续进行表面重建。由前述过程得到每个关键帧的位姿和与前一关键帧的变换矩阵，使用该变换矩阵进行关键帧点云的拼接。每个关键帧的点云都可由深度图和RGB图以及常规针孔相机模型得到，对RGB图中任意一个二维点x(u,v)得到当前帧点云中点P(X,Y,Z,R,G,B)计算公式如式(17)：
其中，(fx,fy)是相机焦距，(cx,cy)是相机中心s是尺度因子，即深度图里给的数据与实际距离的比例；d是该点对应的深度值；R、G、B是该点的颜色信息。
将每一关键帧的点云通过变换矩阵Tcw由式(18)变换到设定的世界坐标系中进行拼接(一般情况下即初始化成功的第一帧图片所建立的三维坐标系)。由于整个系统保存的关键帧数量较多，如果采用每个关键帧的点云进行拼接，会导致大量点云的冗余问题。因此，本文使用VoxelGrid滤波器对每个关键帧的点云进行下采样，在保持点云几何特征的同时，减少点的数量。该滤波器对输入的点云数据创建一个全局三维体素栅格，将每个体素中的点用该体素内所有点的重心来表示，只保留该重心点，可以减少点云数量，降低噪声影响。
P'＝TP (18)
其中，
为变换矩阵，相机的位姿可以通过该矩阵表示，该变换属于欧式变换，它的逆变换为/>
当全局建图结束后，可得到一个全局的经过滤波的三维点云，使用Poisson表面重建算法可以得到一个mesh类型的三维模型，后续颜色的渲染可通过MeshLab软件进行。
实施例
在两个流行的数据集TUM RGB-D和ICL-NUIM中开展实验，评估提出的R-ORB SLAM系统性能，并将R-ORB SLAM系统与ORB-SLAM2系统进行了比较。
实验在内存为8GB，CPU为
CoreTM i7-8750H，携带了GTX 1060的笔记本电脑上进行。
由于多线程会造成实验结果的非确定性。实验在每个视频序列上都运行五次，得到平均轨迹精度。使用绝对轨迹均方根误差ATE RMSE(Absolute Trajectory Root-mean-square error)、失败帧率FR(Failure Ratio)、表面重建误差SRE(SurfaceReconstruction error)来评估SLAM系统的跟踪精度、鲁棒性和重建精度。ATE RMSE通过计算估计位姿与真实位姿之间的欧式距离，得到绝对轨迹误差的均方根误差；失败帧率FR表示跟踪失败的图片帧在整个图片序列中所占的比例；表面重建误差SRE由重建表面的每一点到最近真实3D模型的表面的平均距离计算得到。实验中，R-ORB SLAM中的阈值Nthre＝30。
1、跟踪精度实验
本实施例从TUM RGB-D数据集中选择了几个常用的序列，并与经典的ORB-SLAM2进行比较，结果如表1。
表1 TUM RGB-D数据集上的绝对轨迹均方根误差ATE RMSE(m)
实验结果显示，在几乎每个序列上，本发明提出的R-ORB SLAM系统较原始ORB-SLAM2，跟踪精度都略有提高。在TUM RGB-D数据集中，fr2/360、fr2/slam、fr2/slam3 3个图片序列是通过安装在Pioneer机器人顶部安装的Kinect相机拍摄采集得到的。由于机器人的运动非匀速且较快，整个环境尺度较办公室内较大。剧烈的抖动以及尺度较大的场景都对SLAM系统的稳定性是一个挑战。大多数上述SLAM系统在这几个序列中都会出现跟踪失败，包括ORB-SLAM2。同时，TUMRGB-D数据集中还提供了用于检测环境结构和纹理对SLAM系统影响的序列。其中，fr3/nostructure_notexture_far记录了摄像头沿木制平面移动时拍摄的图片序列，该场景几乎无场景结构和纹理特征，fr3/structure_notexture_far序列记录了一个由木板制成的锯齿形结构而几乎无纹理特征的场景。大部分SLAM系统在这两个场景中都极易跟踪失败。
R-ORB SLAM、ORB-SLAM2在这5个序列上的失败帧率FR结果如表2所示(表中，fr3/ns_nt_far，fr3/s_nt_far分别是指fr3/nostr-ucture_notexture_far和fr3/structure_notexture_far序列)。
表2 TUM RGB-D数据集的失败帧率FR
从表2可以看出，ORB-SLAM2在这5个序列中都极易失败，虽然ORB-SLAM2的重定位可以恢复部分跟踪，但失败帧率仍然较高，本发明方法在这5个序列中全部跟踪成功，鲁棒性较ORB-SLAM2系统有了显著的提高。
图2示出了R-ORB SLAM系统与原ORB-SLAM2在这5个序列中的跟踪结果。可以看到，在fr2/360序列中，ORB-SLAM2在序列开始不久就由于轻微的抖动而跟踪失败，当序列进行至中后半段才重定位成功，而R-ORB SLAM系统跟踪较稳定，且准确度较高。在fr2/slam中，由于较高的相机运动速度和高抖动，ORB-SLAM2在部分路径中跟踪完全失败，R-ORB SLAM系统全部跟踪成功了，但可以看出，由于抖动导致了一定的跟踪误差。
在fr2/slam3序列中，ORB-SLAM2在路线的后半段，因为相机的突然的旋转加速和抖动，从而跟踪失败，而R-ORB SLAM系统全程跟踪成功，但却仍有一定程度的漂移。在fr3/nostructure_notextur-e_far和fr3/structure_notexture_far序列上，ORB-SLAM2系统运行时，在序列开始就由于无足够的特征地图点而导致初始化失败，而R-ORB SLAM系统会使用RPE得到的粗略位姿作为结果，启动并维持跟踪。
图3为ORB-SLAM2系统和R-ORB SLAM系统在序列fr2/360序列上的示例图。图(3a)中ORB-SLAM2由于相机的轻微抖动，系统已跟踪失败，而如图(3b)所示，R-ORB SLAM系统仍有部分特征点跟踪成功。
2、表面重建实验
ICL-NUIM数据集通常用于对RGB-D、视觉测距和SLAM算法进行基准测试，是一个对评估表面重建质量的很重要的数据集。
为了考察表面重建的精度，在ICL-NUIM的起居室数据集上，将本发明方法和Ours+TSDF算法进行了比较，结果如表3所示。实验中，将VoxelGrid滤波器设置网格为0.05×0.05×0.05。Ours+TSDF算法是将本发明R-ORB SLAM系统得到的定位结果，使用TSDF(TruncatedSigned Distance Function)算法进行重建。在实验中TSDF中的体素也划分为0.05×0.05×0.05。
表3 ICL-NUIM数据集的表面重建误差SRE(m)
从表3可以看出，R-ORB SLAM系统的重建模块得到了很好的重建精度。图4示出了本发明方法在kt00序列上重建的结果。
图(4a)是kt00序列的真实3D模型中一个表面，图(4b)是未使用Poisson表面重建算法时R-ORB SLAM得到的稠密点云，图(4c)是使用Poisson表面重建算法的重建结果，图(4d)为Ours+TSDF的重建结果。由于视角和设定的坐标问题，重建的表面看起来与真实模型是镜像的。从图4可以看到，使用TSDF算法可以得到更光滑且平整的表面，但由于使用了体素划分，在本应圆滑的表面如枕头、台灯，却无法显示原本的弧度，丢失了部分细节如右下角的盆摘，而本发明的直接融合点云的方法则能够得到贴近物体原型的重建模型。
以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。
Claims (10)
1.一种基于光度信息和ORB特征的SLAM系统，其特征在于，包括：
前端视觉里程计模块，基于输入的RGB-D流，提取ORB特征，跟踪前一帧以及局部地图实现相机位姿优化，确定新关键帧，所述相机位姿优化中，选择重定位模型、匀速运动模型或者基于光度信息的轻量级位姿粗估计模型中的一个为当前帧设置相机位姿初值；
局部建图模块，由所述新关键帧对地图点进行更新，基于新关键帧、与它相连接的关键帧位姿以及地图点位置进行局部优化；
后端优化模块，对于每一个关键帧进行回环检测，实现全局优化。
2.根据权利要求1所述的基于光度信息和ORB特征的SLAM系统，其特征在于，当跟踪失败时，采用重定位模型估算相机位姿初值；
当相机在运动过程中保持匀速状态时，采用匀速运动模型计算相机位姿初值；
当相机进行非匀速运动时，采用轻量级位姿粗估计模型计算相机位姿初值。
3.根据权利要求1或2所述的基于光度信息和ORB特征的SLAM系统，其特征在于，所述轻量级位姿粗估计模型，提取前一帧图像梯度大于阈值的像素，通过投影方法投影至当前帧平面，优化光度误差，通过增量迭代求解，得到所述相机位姿初值。
4.根据权利要求1所述的基于光度信息和ORB特征的SLAM系统，其特征在于，所述前端视觉里程计模块包括：
跟踪判断单元，基于当前帧的所述ORB特征获得与前一帧或者局部地图的匹配点对，在相机位姿优化后，判断优化后的相机位姿的内点数量是否大于设定阈值Nthre，若是，则跟踪成功，否则跟踪失败。
5.根据权利要求1所述的基于光度信息和ORB特征的SLAM系统，其特征在于，当跟踪成功时，在当前帧同时满足以下条件时确定当前帧为新关键帧：
1)距离上次重定位超过20帧；
2)当局部建图模块空闲时或者距离上一次插入关键帧已超过20帧；
3)当前帧跟踪了超过50个点；
4)当前帧对相关关键帧跟踪成功的点少于90％；
当跟踪失败时，以跟踪前一帧以及局部地图得到位姿Tcwf和RPE模型得到的位姿Tcwr按一定权重相结合得到一个估计值作为当前帧的位姿，并判定当前帧为关键帧。
6.根据权利要求5所述的基于光度信息和ORB特征的SLAM系统，其特征在于，所述位姿Tcwf的权值Wf为：
其中，MapMatches为位姿为Tcwf时跟踪局部地图的内点数量，Nthre为设定阈值。
7.根据权利要求1所述的基于光度信息和ORB特征的SLAM系统，其特征在于，所述局部建图模块中，对地图点进行更新具体为：
产生新地图点、删除异常地图点和删除被少于三个关键帧检测到的地图点。
8.根据权利要求1所述的基于光度信息和ORB特征的SLAM系统，其特征在于，所述局部建图模块采用的优化目标函数为重投影误差函数。
9.根据权利要求1所述的基于光度信息和ORB特征的SLAM系统，其特征在于，所述后端优化模块中，若检测到回环，则将新的关键帧与之前对应的关键帧和共视地图点关联起来，校正关键帧的位姿和地图点的位置信息，对关联起来的关键帧与地图进行全局优化。
10.根据权利要求1所述的基于光度信息和ORB特征的SLAM系统，其特征在于，所述后端优化模块中，根据优化后的位姿融合关键帧点云，使用VoxelGrid滤波器进行下采样，得到稠密的点云地图。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010616900.4A CN111707281B (zh) | 2020-06-30 | 2020-06-30 | 一种基于光度信息和orb特征的slam系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010616900.4A CN111707281B (zh) | 2020-06-30 | 2020-06-30 | 一种基于光度信息和orb特征的slam系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111707281A CN111707281A (zh) | 2020-09-25 |
| CN111707281B true CN111707281B (zh) | 2023-05-30 |
Family
ID=72544025
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010616900.4A Active CN111707281B (zh) | 2020-06-30 | 2020-06-30 | 一种基于光度信息和orb特征的slam系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111707281B (zh) |
Families Citing this family (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112200850B (zh) * | 2020-10-16 | 2022-10-04 | 河南大学 | 一种基于成熟特征点的orb提取方法 |
| CN112233180B (zh) * | 2020-10-23 | 2024-03-15 | 上海影谱科技有限公司 | 基于地图的slam快速初始化方法、装置和电子设备 |
| CN112380312B (zh) * | 2020-11-30 | 2022-08-05 | 北京智行者科技股份有限公司 | 基于栅格检测的激光地图更新方法、终端及计算机设备 |
| CN112862803B (zh) * | 2021-02-26 | 2023-09-26 | 中国人民解放军93114部队 | 基于边缘和特征点融合的红外成像slam方法和装置 |
| CN113514058A (zh) * | 2021-04-23 | 2021-10-19 | 北京华捷艾米科技有限公司 | 融合msckf和图优化的视觉slam定位方法及装置 |
| CN113326769B (zh) * | 2021-05-28 | 2022-11-29 | 北京三快在线科技有限公司 | 高精地图生成方法、装置、设备及存储介质 |
| CN113808203B (zh) * | 2021-08-20 | 2024-04-26 | 北京工业大学 | 一种基于lk光流法与orb-slam2的导航定位方法 |
| CN113872693B (zh) * | 2021-09-29 | 2022-08-23 | 佛山市南海区广工大数控装备协同创新研究院 | 基于关键帧选取的可见光定位与深度相机的紧耦合方法 |
| CN115143958A (zh) * | 2022-04-28 | 2022-10-04 | 西南科技大学 | 一种基于gpu加速的多传感器融合的slam方法 |
| CN115371664B (zh) * | 2022-09-06 | 2024-08-13 | 重庆邮电大学 | 一种基于几何尺度密度和信息熵的移动机器人地图动态维护方法及系统 |
| CN116625380B (zh) * | 2023-07-26 | 2023-09-29 | 广东工业大学 | 一种基于机器学习和slam的路径规划方法及系统 |
| CN117451035B (zh) * | 2023-12-25 | 2024-02-27 | 江苏中科重德智能科技有限公司 | 一种激光slam自适应自动更新地图的方法及系统 |
| CN117710469B (zh) * | 2024-02-06 | 2024-04-12 | 四川大学 | 一种基于rgb-d传感器的在线稠密重建方法及系统 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107830854A (zh) * | 2017-11-06 | 2018-03-23 | 深圳精智机器有限公司 | 基于orb稀疏点云与二维码的视觉定位方法 |
| WO2019169540A1 (zh) * | 2018-03-06 | 2019-09-12 | 斯坦德机器人（深圳）有限公司 | 紧耦合视觉slam的方法、终端及计算机可读存储介质 |
| CN110610520A (zh) * | 2019-08-29 | 2019-12-24 | 中德（珠海）人工智能研究院有限公司 | 一种基于双球幕相机的视觉定位方法及系统 |
| CN110782494A (zh) * | 2019-10-16 | 2020-02-11 | 北京工业大学 | 一种基于点线融合的视觉slam方法 |
-
2020
- 2020-06-30 CN CN202010616900.4A patent/CN111707281B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107830854A (zh) * | 2017-11-06 | 2018-03-23 | 深圳精智机器有限公司 | 基于orb稀疏点云与二维码的视觉定位方法 |
| WO2019169540A1 (zh) * | 2018-03-06 | 2019-09-12 | 斯坦德机器人（深圳）有限公司 | 紧耦合视觉slam的方法、终端及计算机可读存储介质 |
| CN110610520A (zh) * | 2019-08-29 | 2019-12-24 | 中德（珠海）人工智能研究院有限公司 | 一种基于双球幕相机的视觉定位方法及系统 |
| CN110782494A (zh) * | 2019-10-16 | 2020-02-11 | 北京工业大学 | 一种基于点线融合的视觉slam方法 |
Non-Patent Citations (1)
| Title |
|---|
| 李奎霖 ; 魏武 ; 高勇 ; 李艳杰 ; 王栋梁 ; .基于LDSO的机器人视觉定位与稠密建图的应用.微电子学与计算机.2020,(02),全文. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111707281A (zh) | 2020-09-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111707281B (zh) | 一种基于光度信息和orb特征的slam系统 | |
| CN109166149B (zh) | 一种融合双目相机与imu的定位与三维线框结构重建方法与系统 | |
| CN111210463B (zh) | 基于特征点辅助匹配的虚拟宽视角视觉里程计方法及系统 | |
| US9613420B2 (en) | Method for locating a camera and for 3D reconstruction in a partially known environment | |
| CN111445526B (zh) | 一种图像帧之间位姿的估计方法、估计装置和存储介质 | |
| Urban et al. | Multicol-slam-a modular real-time multi-camera slam system | |
| CN108564616B (zh) | 快速鲁棒的rgb-d室内三维场景重建方法 | |
| CN108682027A (zh) | 基于点、线特征融合的vSLAM实现方法及系统 | |
| Maity et al. | Edge slam: Edge points based monocular visual slam | |
| Liu et al. | Indoor localization and visualization using a human-operated backpack system | |
| CN110533587A (zh) | 一种基于视觉先验信息和地图恢复的slam方法 | |
| US20130335529A1 (en) | Camera pose estimation apparatus and method for augmented reality imaging | |
| CN108776976B (zh) | 一种同时定位与建图的方法、系统及存储介质 | |
| CN111951201B (zh) | 一种无人机航拍图像拼接方法、装置和存储介质 | |
| KR101869605B1 (ko) | 평면정보를 이용한 3차원 공간 모델링 및 데이터 경량화 방법 | |
| Zhang et al. | Hand-held monocular SLAM based on line segments | |
| CN112418288A (zh) | 一种基于gms和运动检测的动态视觉slam方法 | |
| Zhao et al. | RTSfM: Real-time structure from motion for mosaicing and DSM mapping of sequential aerial images with low overlap | |
| CN110490222A (zh) | 一种基于低性能处理器设备的半直接视觉定位方法 | |
| Ok et al. | Simultaneous tracking and rendering: Real-time monocular localization for MAVs | |
| Wientapper et al. | Reconstruction and accurate alignment of feature maps for augmented reality | |
| Zhang et al. | A stereo SLAM system with dense mapping | |
| Shao | A Monocular SLAM System Based on the ORB Features | |
| Demirdjian et al. | Using multiple-hypothesis disparity maps and image velocity for 3-D motion estimation | |
| Kuschk et al. | Real-time variational stereo reconstruction with applications to large-scale dense SLAM |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |