-
SLAM通常指在机器人或者其他载体上通过传感器数据进行采集和计算,得到对其自身位置姿态定位和场景地图信息的系统。一个经典的基于视觉传感器的Visual SLAM一般包括前端视觉里程计、后端优化、回环检测、建图4个模块。前端视觉里程计模块用于估计相邻图像之间相机的位姿变换以及得到局部地图;后端优化模块对相机位姿及局部地图信息进行优化得到全局一致的轨迹和地图;回环检测模块用于检测传感器是否又回到之前的位置,优秀的回环检测可以有效减少累计漂移误差;建图模块则根据之前得到的跟踪全局地图建立所需要的地图。
经典的视觉里程计采用的方法主要有基于特征的方法和直接法。基于特征的方法通过提取特征以及特征匹配得到图像帧间的几何对应关系,优化重投影误差来估计传感器位姿。早期,基于特征的方法主要侧重于特征点的提取与匹配。其中,由Mur-Artal等[1]提出的ORB-SLAM2是一个同时支持单目、双目及深度相机的基于特征点的SLAM系统,它使用三线程结构取得了很好的跟踪和建图效果,一定程度上保证了轨迹与地图的全局一致性。由于基于特征点的方法不足以提供有效的约束且在低纹理场景中容易失败,不少学者开始尝试结合线、平面、边缘等其他特征。Pumarola等[2]提出了一个同时结合了点和线特征的SLAM系统,该系统在低纹理环境下表现稳定。Hsia等[3]提出的KDP-SLAM(Keyframe-based Dense Planar SLAM)是一种基于关键帧密集平面匹配的SLAM系统,显著地降低了漂移误差。Li等[4]提出了利用点、线、面的结构特性包括线与线之间的平行性和相交性、点线之间的共面性来跟踪相机运动,并在低纹理环境下得到了更高的系统稳定性和较为精确的跟踪结果。
直接法通过最小化图像中像素之间的光度误差来估计传感器的运动信息,一方面能够弥补基于特征的方法在某些特征缺失场景下失败的问题,另一方面能够直接得到稠密的地图显示。LSD-SLAM(Large-Scale Direct Monocular SLAM)[5]是Engel等提出的第一个不需要计算特征点且能构建半稠密地图的单目SLAM系统,但该系统对相机内参及曝光非常敏感,在快速运动时容易丢失。他们后期又提供了一个更为稳健、准确度更高的包含光度校准的直接法视觉里程计[6],但这个系统不包括回环检测,无法降低累积误差。Gao等[7]在这个直接视觉里程计的基础上加入基于词袋的回环检测功能,减少了累积误差。
基于特征的方法在特征丰富的区域准确率较高且计算代价较小,但在特征点缺失的无纹理场景下容易失败。而直接法不需要特征点,可以在特征缺失的场合下使用,但依赖于灰度不变性,易受光照影响。因此,有不少学者尝试将这两种方法结合以得到对环境更为鲁棒的系统。Younes等[8]提出了FDMO(Feature Assisted Direct Monocular Odometry)方法,它在直接法出现故障的场景,如大基线时使用ORB特征进行位姿估计,提升了直接法系统的鲁棒性。LEE等[9]提出了一种半直接法,以直接法跟踪局部每帧的相机运动,以基于ORB特征的图优化方法优化跟踪关键帧的相机运动,得到了一个准确性和鲁棒性且速度上都有提高的SLAM系统。这两种SLAM系统都是单目的混合SLAM系统,无法得到稠密的三维地图。NICOLA等[10]提出以libviso2特征法作为双目LSD-SLAM的运动估计先验,得到了一个精度较高且较为稳定的双目SLAM系统,但它使用特征法作为SLAM系统的前端位姿估计,在特征缺失的环境中可能会跟踪失败。
在环境勘探工程中,机器人常在较空旷环境及非光滑表面行进,通过双目或深度相机对环境进行勘探,并需要通过采集得到的图片及视频对三维地形进行重建。针对SLAM系统因为相机抖动、非匀速的运动、快速的旋转及场景结构单一、纹理简单等问题而导致跟踪失败的情况,本文提出了一个鲁棒的具有建图功能的R-ORB SLAM系统。该系统的创新点主要体现在两方面:(1)该系统基于ORB-SLAM2系统,同时采用一种轻量级的位姿估计作为特征视觉里程计的先验,在跟踪失败的情况下,将先验位姿的结果参与位姿估计;(2)通过相机模型计算得到关键帧点云,通过对每个关键帧点云进行拼接得到全局点云地图,使用VoxelGrid滤波器[11]进行下采样,得到稠密三维点云地图,使用Poisson算法[12]实现表面重建,得到三维Mesh地图。该系统充分利用了图片的光度和深度信息,提高了系统的准确率和鲁棒性。
-
ORB-SLAM2系统是基于ORB(Oriented FAST and Rotated BRIEF)特征的特征法SLAM。前端通过高斯金字塔模型提取均匀的ORB特征点,保证ORB特征的旋转不变性和尺度不变性。
在ORB-SLAM2系统的双目模式和RGB-D模式中,特征关键点由3个坐标
${x_s} = \left( {{u_{\rm{L}}},{v_{\rm{L}}},{u_{\rm{R}}}} \right)$ 表示,其中$\left( {{u_{\rm{L}}},{v_{\rm{L}}}} \right)$ 为该特征关键点在左图(RGB-D相机中的RGB图像)中的像素坐标,${u_{\rm{R}}}$ 为右边图像中的水平像素坐标,在RGB-D模式下,${u_{\rm{R}}}$ 是一个虚拟的坐标,可由式(1)得到。其中:
${f_x}$ 是相机水平焦距;$b$ 是相机基线,$d$ 是对应像素深度信息。根据传统相机模型可以将特征点映射到三维空间中。在运动跟踪模型中,假设传感器的运动速度是恒定的,前一帧的位姿可以作为下一帧位姿估计的先验,将上一帧形成的特征三维点根据初始位姿投影至下一帧,在下一帧的投影点附近可以找到对应的匹配点,使用g2o[13]中提供的Levenberg-Marquardt算法优化相机旋转矩阵
${{R}} \in SO\left( 3 \right)$ 和平移向量${{t}} \in {{\rm{R}}^3}$ ,目标函数为匹配成功的世界坐标系下的3D点${{{X}}^i} \in {{\rm{R}}^3}$ 以及2D关键点$x_{\left( \cdot \right)}^i$ 之间的重投影误差:其中:
$\rho $ 是鲁棒Huber损失函数;${{\varSigma}}$ 是与关键点尺度相关的协方差矩阵;$\chi $ 为所有匹配点;投影函数${\pi _{\left( \cdot \right)}}$ 可以由式(3)表示:其中:
$\left( {{f_x},{f_y}} \right)$ 是相机焦距;$\left( {{c_x},{c_y}} \right)$ 是光圈中心;${\left[ {X,Y,Z} \right]^{\rm{T}}}$ 为某一3D地图点坐标。根据运动跟踪模型中得到的位姿进行局部优化。局部优化是优化一组拥有较多共视点的关键帧
${{{K}}_L}$ 和这些关键帧中所包含的所有点${{{P}}_L}$ 。设${{{K}}_F}$ 为能观测到这些点的其他关键帧,${\chi _k}$ 为${{{P}}_L}$ 中与关键帧k的关键点匹配上的点,则优化如下重投影误差:新加入的每一个关键帧都要与之前的关键帧做回环检测。ORB-SLAM2的回环检测基于词袋模型。
-
ORB-SLAM2在相机运动估计中使用李群
$SE\left( 3 \right)$ 和对应的李代数$se\left( 3 \right)$ 来表示刚体变换[1]。在李代数$se\left( 3 \right)$ 中,相机位姿的变换用一个6维的向量${{\xi}} = \left( {{\xi _1},{\xi _2},{\xi _3},{\xi _4},{\xi _5},{\xi _6}} \right)$ 表示,其中${{\tau}} = \left( {{\xi _1},{\xi _2},{\xi _3}} \right)$ 表示变换的平移部分,$\rho = \left( {{\xi _4},{\xi _5},{\xi _6}} \right)$ 表示变换的旋转部分,使用对数映射可以将刚体变换$T$ 映射到其对应的${{\xi}} $ :其中符号
$ \wedge $ 表示将该向量变换成其对应的反对称矩阵,那么要将向量${{\xi}} $ 变换为对应的${{T}}$ 则使用: -
ORB-SLAM2系统在相机发生抖动(图1(a))、以及特征点数量较少、场景空旷简单(图1(b))的情况下,极易发生跟踪失败的问题,而其系统本身自带的重定位功能在发生跟踪失败时,只能回到之前经过的某个位置重新启动跟踪。这样,当相机在新的场景中持续运动时则会彻底跟踪失败。针对该问题,本文提出了一种将光度信息和几何特征相结合的R-ORB SLAM系统。
-
R-ORB SLAM系统框图如图2所示。首先对输入的图片序列进行图片矫正、金字塔分层模型及特征提取等预处理操作,然后根据重定位、运动模型或者本文提出的基于光度信息的轻量级位姿估计方法(Roughly Pose Estimation,RPE)得到初始位姿对前一帧进行跟踪,最后根据优化得到的位姿结果生成地图。在建图模块中,保留ORB-SLAM2系统的建图线程用于跟踪,另外使用点云拼接线程拼接每个关键帧点云得到三维点云地图,对得到的点云地图使用VoxelGrid滤波器进行下采样滤波,对滤波后的点云使用Poisson表面重建算法得到三维地图模型。R-ORB SLAM算法主要适用于RGB-D深度传感器。
-
RPE模型提取前一帧图像梯度大于阈值的像素,并假设灰度不变,即假设一个空间点在各个视角下成像的灰度不变。将前一帧的任意一点通过与特征法相似的投影方法投影至当前帧平面,此时优化的不再是重投影误差而是光度误差,公式如下:
其中:
${{K}}$ 为相机内参矩阵;${{{X}}^i} \in {{\rm{R}}^3}$ 为与前一帧匹配成功的地图点;$x_{\left( \cdot \right)}^i$ 为后一帧图像平面的对应点;$I\left( \cdot \right)$ 表示该点的光度;${Z_2}$ 为${{{X}}^i}$ 经过$\exp \left( {{{{\xi}} ^ {\wedge} }} \right)$ 变换后第二帧相机坐标系下的Z轴坐标。为求解该优化问题,给相机位姿添加一个小扰动
$\exp \left( {\delta {{{\xi}} ^ {\wedge} }} \right)$ ,得到每一点的光度误差:令
$q = \delta {{{\xi}} ^ {\wedge} }\exp \left( {{{{\xi}} ^ {\wedge} }} \right)X$ 表示扰动分量在第二帧图片建立的相机坐标系下的坐标,$u = \frac{1}{Z}Kq$ 表示它的像素坐标,利用一阶泰勒展开得到:其中:
$\frac{{\partial {I_2}}}{{\partial u}}$ 是$u$ 处的像素梯度;$\frac{{\partial u}}{{\partial q}}$ 为投影方程关于相机坐标系下的三维点的导数;$\frac{{\partial q}}{{\partial \delta {{\xi}} }}$ 为变换后的三维点对变换的导数。设${{J}}$ 为误差相对于李代数的雅可比矩阵,那么误差可由下式表示:其中,
使用该式计算优化问题的雅可比矩阵,然后使用高斯牛顿法或者Levenberg-Marquardt方法计算增量迭代求解。
-
使用RPE模型作为位姿估计模型,与运动模型一起为ORB-SLAM2系统提供先验。
ORB-SLAM2系统中的初始位姿是由运动模型和重定位得到的,系统根据该初始位姿将地图点重投影至图像平面,在投影得到的位置附近寻找匹配点,并在该初始位姿的基础上进行优化。当传感器违背了运动模型做不规律的运动时,使用运动模型得到的初始位姿不能较好地为后续优化提供先验,导致跟踪失败问题。通常这样的情况并不会发生在相机的整个运动路线中,而只是局部发生。针对这种情况,结合2.2节的轻量级位姿粗估计,设计了一个权重模型,将ORB-SLAM2系统得到的不够准确的位姿和RPR模型得到的位姿相结合,得到一个估计值,作为跟踪失败时当前帧的位姿,并在此插入关键帧以防止后续的跟踪失败。
设
${N_{{\rm{thre}}}}$ 是基于特征跟踪是否成功的阈值,使用系统计算得到的位姿匹配成功的地图点数量大于该阈值则代表跟踪成功。用式(12)计算特征法的权值${W_{\rm{f}}}$ :其中,
${\rm{MapMatches}}$ 表示由位姿粗估计得到的位姿作为先验特征法优化后匹配成功的地图点数量。那么RPE结果的权值为跟踪失败时的当前帧的位姿
$Tcw$ 则由式(14)得到:其中
$Tc{w_{\rm{f}}}$ 为由特征法优化后的位姿,而$Tc{w_{\rm{r}}}$ 为位姿粗估计得到的位姿。 -
ORB-SLAM2系统只保留一些在跟踪过程中被反复检测并优化的特征三维点,简单增加跟踪过程中的地图点,不仅会降低跟踪的速度,还会增加一些可能错误的约束导致优化失败。本文将建图和跟踪分离开来,由建图线程单独进行建图。
使用点云拼接的方式建立稠密的点云地图。由跟踪线程得到每个关键帧的位姿和与前一关键帧的变换矩阵,使用该变换矩阵进行关键帧点云的拼接。每个关键帧的点云都可由深度图和RGB图以及常规针孔相机模型得到,对于RGB图中任意一个二维点
$x\left( {u,v} \right)$ 得到当前帧点云中点${{P}}\left( {X,Y,Z,R,G,B} \right)$ 计算公式如式(15):其中:
$s$ 是尺度因子,即深度图里给的数据与实际距离的比例;$R$ ,$G$ ,$B$ 是该点的颜色信息。将每一个关键帧的点云通过变换矩阵
${{T}}$ 变换到设定的世界坐标系中进行拼接(一般情况下即初始化成功的第一帧图片所建立的三维坐标系)。对于当前关键帧点云中的空间点${{P}} = \left( {X,Y,Z,R,G,B} \right)$ ,通过式(16)变换将${{P}}$ 转换为世界坐标系中的空间点${{{P}}^{'}} = \left( {{X^{'}},{Y^{'}},{Z^{'}},R,G,B} \right)$ :由于ORB-SLAM2系统采用多关键帧的策略,如果采用每个关键帧的点云进行拼接,会导致大量点云的冗余问题。因此,本文使用VoxelGrid滤波器对每个关键帧的点云进行下采样,在保持点云几何特征的同时,减少点的数量。该滤波器通过输入的点云数据创建一个三维体素栅格,在每个体素内,用体素中所有点的重心来表示其他点。
当全局优化线程结束后,可得到一个全局的、经过滤波的三维点云,使用Poisson表面重建算法可以得到一个mesh类型的三维模型,后续颜色的渲染可通过MeshLab软件[14]进行。
-
在两个流行的数据集TUM RGB-D[15]和ICL-NUIM[16]中开展实验,评估R-ORB SLAM系统的性能,并将R-ORB SLAM系统与其他SLAM系统进行比较。实验在内存为8 GB,CPU为Intel® Core™ i7-8 750H,携带了GTX 1 060的笔记本电脑上进行。由于多线程会造成实验结果的非确定性,实验在每个视频序列上运行5次,得到平均轨迹精度。使用绝对轨迹均方根误差 (Absolute Trajectory Root-Mean-Square Error,ATE RMSE)、失败帧率 (Failure Ratio, FR)、表面重建误差 (Surface Reconstruction Error, SRE)评估SLAM系统的跟踪精度、鲁棒性和重建精度。ATE RMSE通过计算估计位姿与真实位姿之间的欧式距离,得到绝对轨迹误差的均方根误差;FR表示跟踪失败的图片帧在整个图片序列中所占的比例;SRE由重建表面的每一点到最近真实3D模型的表面的平均距离计算得到。实验中,R-ORB SLAM系统中的
${N_{{\rm{thre}}}}$ 取ORB-SLAM2系统中的默认阈值。 -
TUM RGB-D数据集包含来自RGB-D传感器的室内序列,这些序列分组在几个类别中,包括手持式采集类及机器人采集类等,以评估不同纹理、不同运动速度、照明和结构条件下的SLAM方法。该公开数据集基准还提供了许多有用的工具,可用于预处理数据集和评估SLAM系统跟踪结果。
本文从TUM RGB-D数据集中选择了几个常用的序列,并与ORB-SLAM2、DVO-SLAM[17]、RGB-D SLAM[18]、Kintinuous[19]、Elastic Fusion[20]进行了比较,结果如表1所示。实验结果显示,在几乎每个序列上,R-ORB SLAM系统比ORB-SLAM2系统的跟踪精度都略有提高,与其系统相比,仅在1个序列上略低于Elastic Fusion。
System ATE RMSE fr1/desk fr1/desk2 fr1/room fr2/desk fr2/xyz fr3/office fr3/nst DVO-SLAM 0.021 0.046 0.043 0.017 0 0.018 0 0.035 0.018 RGB-D SLAM 0.026 - 0.087 0.057 0 - - - Kintinuous 0.037 0.071 0.075 0.034 0 0.029 0 0.030 0.031 Elastic Fusion 0.020 0.048 0.068 0.071 0 0.011 0 0.017 0.016 ORB-SLAM2 0.016 0.022 0.047 0.009 0 0.004 0 0.010 0.019 R-ORB SLAM 0.015 0.021 0.043 0.008 8 0.003 6 0.009 0.017 表 1 TUM RGB-D数据集上的绝对轨迹均方根误差
Table 1. Comparison of ATE RMSE on TUM RGB-D dataset
在TUM RGB-D数据集中,fr2/360、fr2/slam、fr2/slam3 图片序列通过安装在Pioneer机器人顶部的Kinect相机拍摄采集得到。由于机器人的运动非匀速且较快,整个环境尺度比办公室场景大。剧烈的抖动以及尺度较大的场景对SLAM系统的稳定性是一个挑战。大多数SLAM系统在这几个序列中都会出现跟踪失败,包括ORB-SLAM2。TUM RGB-D数据集中还提供了用于检测环境结构和纹理对SLAM系统影响的序列。其中,fr3/nostructure_notexture_far记录了摄像头沿木制平面移动时拍摄的图片序列,几乎无场景结构和纹理特征;fr3/structure_notexture_far记录了一个由木板制成的锯齿形结构而几乎无纹理特征的场景,大部分SLAM系统在这两个场景中都极易跟踪失败。
R-ORB SLAM、ORB-SLAM2和Elastic Fusion系统在这5个序列上的失败帧率如表2所示,表中fr3/ns_nt_far、fr3/s_nt_far分别是指fr3/nostr-ucture_notexture_far、fr3/structure_notexture_far序列。从表2可以看出,Elastic Fusion和ORB-SLAM2系系统的重定位可以恢复部分跟踪,但失败帧率仍然较高。本文方法在这5个序列中全部跟踪成功,鲁棒性较ORB-SLAM2系统有了显著的提高。
System FR fr2/360 fr2/slam fr2/slam3 fr3/ns_nt_far fr3/s_nt_far Elastic Fusion 61.6 52.7 32.3 4.7 0 ORB-SLAM2 52.3 39.9 4.5 75.0 68.0 R-ORB SLAM 0 0 0 0 0 表 2 TUM RGB-D数据集的失败帧率
Table 2. Comparison of failure ratio on TUM RGB-D dataset
图3示出了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得到的粗略位姿作为结果,启动并维持跟踪。
图4为ORB-SLAM2系统和R-ORB SLAM系统在fr2/360序列上的示例图。图4(a)中ORB-SLAM2系统由于相机的轻微抖动,已跟踪失败,而R-ORB SLAM系统仍有部分特征点跟踪成功,如图4(b)所示。
-
ICL-NUIM数据集[16]通常用于对RGB-D、视觉测距和SLAM算法进行基准测试,是一个对评估表面重建质量很重要的数据集。它包括由手持深度相机拍摄的起居室和办公室两个场景,每个场景都提供真实测量位姿,但起居室场景还提供了一个真实的3D模型以供评估表面重建的质量。
本文从ICL-NUIM数据集中选择了4个序列,并将R-ORB SLAM系统与其他SLAM系统的跟踪精度进行比较,结果如表3所示。
System ATE RMSE kt0 kt1 kt2 kt3 Average DVO-SLAM 0.104 0 0.029 0.191 0.152 0.119 RGB-D SLAM 0.026 0 0.008 0.018 0.433 0.121 Kintinuous 0.072 0 0.005 0.010 0.355 0.111 ElasticFusion 0.009 0 0.009 0.014 0.106 0.035 ORB-SLAM2 0.007 6 0.162 0.018 0.019 0.052 R-ORB SLAM 0.007 4 0.093 0.019 0.012 0.033 表 3 ICL-NUIM数据集上的ATE RMSE
Table 3. Comparison of ATE RMSE on the ICL-NUIM dataset
从表3可以看出,虽然R-ORB SLAM系统的ATE RMSE误差在4个序列中并不总是最低,但其平均误差最小,仍然具有较高的精度和稳定性。
为了进一步考察表面重建的精度,在ICL-NUIM的起居室数据集上,将本文系统和其他系统进行了比较,结果如表4所示。
System SRE kt0 kt1 kt2 kt3 Average DVO-SLAM 0.032 0 0.061 0.119 0.053 0.066 RGB-D SLAM 0.044 0 0.032 0.031 0.167 0.069 Kintinuous 0.011 0 0.008 0.009 0.150 0.045 ElasticFusion 0.007 0 0.007 0.008 0.028 0.013 Ours+TSDF 0.001 9 0.073 0.020 0.019 0.033 R-ORB SLAM 0.007 0 0.009 0.009 0.004 0.007 表 4 ICL-NUIM数据集的表面重建误差SRE
Table 4. Comparison of surface reconstruction error on the ICL-NUIM dataset
实验中,将VoxelGrid滤波器设置网格为0.05×0.05×0.05。表4中Ours+TSDF是将R-ORB SLAM系统得到的定位结果,使用TSDF(Truncated Signed Distance Function)[21]算法进行重建。在实验中TSDF中的体素也划分为0.05×0.05×0.05。
从表4可以看出,R-ORB SLAM系统的重建模块在kt1、kt2序列中并不是效果最好的,但在kt0和kt3序列中得到了很好的重建精度,平均表面重建精度最小。而简单的应用TSDF算法并没有得到很好的重建结果。图5示出了本文方法在kt0序列上重建的结果。图5(a)是kt0序列真实3D模型中的一个表面;图5(b)是未使用Poisson表面重建算法时R-ORB SLAM系统得到的稠密点云;图5(c)是使用Poisson表面重建算法的重建结果;图5(d)为Ours+TSDF的重建结果。由于视角和设定的坐标问题,重建的表面看起来与真实模型是镜像的。从图5可以看到,使用TSDF算法可以得到更光滑且平整的表面,但由于使用了体素划分,在本应圆滑的表面如枕头、台灯,却无法显示原本的弧度,丢失了部分细节,如右下角的盆栽,而本文提出的直接融合点云的方法则能够得到贴近物体原型的重建模型。
-
本文针对RGB-D 传感器提出了一个使用光度信息和ORB特征相结合的R-ORB SLAM系统,可以有效减少系统因为相机抖动、非匀速的运动、快速的旋转以及场景结构单一、纹理简单等问题而导致跟踪失败的情况。同时,通过在系统中加入拼接、滤波等点云处理,并采用Poisson表面重建算法实现了地图三维重建。在公开数据集上的实验结果显示,R-ORB SLAM系统在跟踪和重建上都有较好的精度和稳定性。在本文的重建部分,ORB-SLAM2得到的稀疏地图仅用于跟踪,而实际上该地图上的点拥有更准确的位置信息,未来,考虑将这些地图点的信息利用起来得到更精确的重建效果。
一种基于光度信息和ORB特征的建图SLAM
SLAM With Mapping Based on Photometric Information and ORB Features
-
摘要: 针对同步定位与建图(Simultaneous Localization and Mapping, SLAM)系统经常因为相机抖动及场景结构单一等问题而导致跟踪失败的情况,以及重建三维地图的任务,提出了一种混合SLAM方法(R-ORB SLAM)。该方法采用一种基于光度误差的位姿粗略估计作为特征视觉里程计的先验,在ORB-SLAM2跟踪失败的情况下,使用该结果参与位姿估计。同时,在建图模块,对由每个关键帧点云拼接得到的全局点云地图使用VoxelGrid滤波器进行下采样,得到去除冗余点的稠密三维点云地图。通过对三维点云地图使用Poisson算法实现表面重建,得到三维地图模型。在两个流行的公开数据集上的实验结果表明,本文方法能有效解决跟踪失败问题,实现三维地图重建,具有较高的跟踪精度与重建精度。
-
关键词:
- RGB-D SLAM /
- 光度误差 /
- 特征法 /
- 三维地图重建
Abstract: Aiming at the tracking failure caused by camera jitter or low-texture environment in slam, and for the task of 3D map reconstruction, a hybrid slam method R-ORB slam for depth camera is proposed in this paper. A pose estimation method based on photometric error is used as the prior of the feature-basedodometer, and the result of the method is used to participate in pose estimation in the case of ORB-SLAM2 tracking fail. At the same time, for generating dense three-dimensional point map with non-redundant points, a filter is used for down sampling of the global point cloud map obtained by fusion all point clouds of each key frame. Then, by using Poisson algorithm to reconstruct the surface of 3D point cloud map, the 3D map model is generated. Experiments on two popular open datasets show that the proposed method can effectively solve the problem of tracking failure and realize 3D reconstruction with high tracking accuracy and reconstruction accuracy.-
Key words:
- RGB-D SLAM /
- photometric error /
- feature-based method /
- 3D map reconstruction
-
表 1 TUM RGB-D数据集上的绝对轨迹均方根误差
Table 1. Comparison of ATE RMSE on TUM RGB-D dataset
System ATE RMSE fr1/desk fr1/desk2 fr1/room fr2/desk fr2/xyz fr3/office fr3/nst DVO-SLAM 0.021 0.046 0.043 0.017 0 0.018 0 0.035 0.018 RGB-D SLAM 0.026 - 0.087 0.057 0 - - - Kintinuous 0.037 0.071 0.075 0.034 0 0.029 0 0.030 0.031 Elastic Fusion 0.020 0.048 0.068 0.071 0 0.011 0 0.017 0.016 ORB-SLAM2 0.016 0.022 0.047 0.009 0 0.004 0 0.010 0.019 R-ORB SLAM 0.015 0.021 0.043 0.008 8 0.003 6 0.009 0.017 表 2 TUM RGB-D数据集的失败帧率
Table 2. Comparison of failure ratio on TUM RGB-D dataset
System FR fr2/360 fr2/slam fr2/slam3 fr3/ns_nt_far fr3/s_nt_far Elastic Fusion 61.6 52.7 32.3 4.7 0 ORB-SLAM2 52.3 39.9 4.5 75.0 68.0 R-ORB SLAM 0 0 0 0 0 表 3 ICL-NUIM数据集上的ATE RMSE
Table 3. Comparison of ATE RMSE on the ICL-NUIM dataset
System ATE RMSE kt0 kt1 kt2 kt3 Average DVO-SLAM 0.104 0 0.029 0.191 0.152 0.119 RGB-D SLAM 0.026 0 0.008 0.018 0.433 0.121 Kintinuous 0.072 0 0.005 0.010 0.355 0.111 ElasticFusion 0.009 0 0.009 0.014 0.106 0.035 ORB-SLAM2 0.007 6 0.162 0.018 0.019 0.052 R-ORB SLAM 0.007 4 0.093 0.019 0.012 0.033 表 4 ICL-NUIM数据集的表面重建误差SRE
Table 4. Comparison of surface reconstruction error on the ICL-NUIM dataset
System SRE kt0 kt1 kt2 kt3 Average DVO-SLAM 0.032 0 0.061 0.119 0.053 0.066 RGB-D SLAM 0.044 0 0.032 0.031 0.167 0.069 Kintinuous 0.011 0 0.008 0.009 0.150 0.045 ElasticFusion 0.007 0 0.007 0.008 0.028 0.013 Ours+TSDF 0.001 9 0.073 0.020 0.019 0.033 R-ORB SLAM 0.007 0 0.009 0.009 0.004 0.007 -
[1] MUR-ARTAL R, TARDÓS J D. ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB-D cameras[J]. IEEE Transactions on Robotics, 2017, 33(5): 1-8. doi: 10.1109/TRO.2017.2755360 [2] PUMAROLA A, VAKHITOV A, AGUDO A, et al. PL-SLAM: Real-time monocular visual SLAM with points and lines[C]//2017 IEEE International Conference on Robotics and Automation (ICRA). Singapore: IEEE, 2017: 4503-4508. [3] HSIAO M, WESTMAN E, ZHANG G, et al. Keyframe-based dense planar SLAM[C]//IEEE International Conference on Robotics & Automation. Singapore: IEEE, 2017: 5110-5117. [4] LI H, YAO J, BAZIN J C, et al. A monocular SLAM system leveraging structural regularity in Manhattan world[C]//IEEE International Conference on Robotics & Automation. USA: IEEE, 2018: 2518-2525. [5] ENGEL J, SCHÖPS T, CREMERS D. LSD-SLAM: Large-scale direct monocular SLAM[C]// Computer Vision - ECCV 2014. Switzerland: Springer, 2014: 834-849. [6] ENGEL J, KOLTUN V, CREMERS D. Direct sparse odometry[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2017, 40(3): 611-625. [7] GAO X, WANG R, DEMMEL N, et al. LDSO: Direct sparse odometry with loop closure[C]// IEEE International Conference on Intelligent Robots and Systems (IROS). USA: IEEE, 2018: 2198-2204. [8] YOUNES G, ASMAR D, ZELEK J. FDMO: Feature assisted direct monocular odometry[EB/OL]. arxiv.org. 2018-04-15. [2020-01-10]. https://arxiv.org/pdf/1804.05422.pdf. [9] LEE S H, CIVERA J. Loosely-coupled semi-direct monocular SLAM[J]. IEEE Robotics and Automation Letters, 2019, 4(2): 399-406. doi: 10.1109/LRA.2018.2889156 [10] NICOLA K, DAVID D, SEBASTIAN H, et al. Feature-based visual odometry prior for real-time semi-dense stereo SLAM[J]. Robotics and Autonomous Systems, 2018, 109: 1-46. doi: 10.1016/j.robot.2018.08.007 [11] RUSU R B, COUSINS S. 3D is here: Point cloud library (PCL)[C]// IEEE International Conference on Robotics & Automation(ICRA). Shanghai, China: IEEE, 2011: 1-4. [12] KAZHDAN M, BOLITHO M, HOPPE H. Poisson surface reconstruction[C]// Proceedings of 4th Eurographics Symposium on Geometry Processing. Switzerland: Eurographics Association Aire-la-Ville, 2006: 61-70. [13] KUMMERLE R, GRISETTI G, STRASDAT H, et al. G2o: A general framework for graph optimization[C]// IEEE International Conference on Robotics & Automation. Shanghai: IEEE, 2011: 3607-3613. [14] CIGNONI P, CALLIERI M, CORSINI M, et al. MeshLab: an open-source mesh processing tool[C]// Eurographics Italian Chapter Conference 2008. Salerno, Italy: Springer Verlag, 2008: 129-136. [15] JÜRGEN STURM, ENGELHARD N, ENDRES F, et al. A benchmark for the evaluation of RGB-D SLAM systems[C]// 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. Portugal: IEEE, 2012: 573-580. [16] HANDA A, WHELAN T, MCDONALD J, et al. A benchmark for RGB-D visual odometry, 3D reconstruction and SLAM[C]// 2014 IEEE International Conference on Robotics and Automation (ICRA). Hong Kong, China: IEEE, 2014: 1524-1531. [17] KERL C, STURM J, CREMERS D. Dense visual SLAM for RGB-D cameras[C]// IEEE/RSJ International Conference on Intelligent Robots & Systems. Tokyo, Japan: IEEE, 2014: 2100-2106. [18] ENDRES F, HESS J, STURM J, et al. 3-D mapping with an RGB-D camera[J]. IEEE Transactions on Robotics, 2014, 30(1): 177-187. doi: 10.1109/TRO.2013.2279412 [19] WHELAN T, KAESS M, JOHANNSSON H, et al. Real-time large-scale dense RGB-D SLAM with volumetric fusion[J]. International Journal of Robotics Research, 2015, 34(4/5): 598-626. [20] WHELAN T, SALAS-MORENO R, GLOCKER B, et al. ElasticFusion: Real-time dense SLAM and light source estimation[J]. International Journal of Robotics Research, 2016, 35(14): 1697-1716. doi: 10.1177/0278364916669237 [21] ZENG A, SONG S, NIEßNER M, et al. 3DMatch: Learning local geometric descriptors from RGB-D reconstructions[C]// IEEE Conference on Computer Vision & Pattern Recognition. USA: IEEE, 2017: 199-208. -