一种通过改进ORB-SLAM算法实现定位与构图的方法和系统
技术领域
本发明属于机器视觉与机器学习技术领域,更具体地,涉及一种通过改进ORB-SLAM算法实现定位与构图的方法和系统。
背景技术
具有方向的FAST和旋转BRIEF(Oriented FAST and Rotated BRIEF,简称ORB)特征因其提取速度快,还易于匹配和比较等优点受到研究人员的重视。在2015年,Mur-ArtalR等人将ORB引入到即时定位与地图构建(simulation location and mapping,简称SLAM)算法中,提出了使用ORB特征进行定位跟踪和环境建图来实现SLAM系统(即ORB-SLAM系统),该系统已经成为现代SLAM系统中比较易用和完善的系统之一。
然而,传统的ORB-SLAM系统还存在一些不足和问题:首先,整个ORB-SLAM系统都围绕着ORB特征点进行处理,需要对获取的每帧图像都提取ORB特征,这将会消耗大量的运算时间;其次,该系统中的特征检测只能在原始尺寸的图像内进行,当场景发生大尺度变化或者视角变化时,就会产生误差,甚至无法进行特征匹配;最后,该ORB-SLAM系统构建出的环境地图会十分的稀疏,只能在一定程度上达到定位的需求,而无法在导航、避障、交互等其他实际应用中使用。
发明内容
针对现有技术的以上缺陷或改进需求,本发明提供了一种通过改进ORB-SLAM算法实现定位与构图的方法和系统,其目的在于,解决现有ORB-SLAM算法存在的消耗大量运算时间、容易产生误差导致无法进行特征匹配、以及应用范围偏小的技术问题。
为实现上述目的,按照本发明的一个方面,提供了一种通过改进ORB-SLAM算法实现定位与构图的方法,包括以下步骤:
(1)使用机器人携带的深度视觉传感器采集数据集中的连续两幅图像,以分别得到每一幅图像的彩色图像与深度图像,使用显著性区域检测算法对得到的彩色图像进行感兴趣区域初步提取,使用阈值分割法对提取到的感兴趣区域进行优化,以得到优化后的图像;
(2)在离散化尺度空间下使用ORB算法对步骤(1)得到的优化后的两幅图像进行特征提取,以得到两幅图像的特征描述子;
(3)对步骤(2)中得到的两幅图像的特征描述子进行特征匹配,以生成两幅图像特征匹配的结果;
(4)使用步骤(3)得到的两幅图像特征匹配的结果获得变换矩阵,并将该变换矩阵作为算法迭代初值输入到迭代最近点(Iterative Closest Point,简称ICP)算法中,以得到运动估计结果。
(5)对步骤(2)中得到的两幅图像的特征描述子进行特征单词化处理,基于处理后得到的单词构建视觉词典,并利用视觉词典中的单词对每一幅图像进行场景描述;
(6)对场景描述后的图像与视觉词典中出现过的每幅图像分别进行预匹配处理,对所有预匹配处理的结果进行相似度计算,根据相似度计算结果确定多个候选闭环,基于时间连续性从多个候选闭环中确定最终的闭环,用于对步骤(4)得到的运动估计结果进行优化;
(7)使用非线性最小二乘法对步骤(6)优化后的运动估计结果进行位姿图优化,以得到全局最优的机器人位姿和全局一致的运动轨迹,并利用栅格地图法对该运动轨迹进行建图。
优选地,深度视觉传感器是Kinect传感器,显著性区域检测算法是集合多层次卷积特征算法、基于区域的全卷积网络算法、或空间加权相异性算法。
优选地,步骤(2)建立离散化尺度空间是按照如下方式实现:构造出一个由n个图像层li和n个间隔层bi组成的尺度空间,通过对优化后的图像l0进行1.5倍的下采样来获得第一层间隔层b0,然后分别对l0和b0逐层半采样以获取接下来每层的图像层lj和间隔层bj,图像层li的尺度值为σ(li)=2i,间隔层bi的尺度值为σ(bi)=2i×1.5,其中n可以为任意正整数,i∈(0,n-1),j∈(1,n-1)。
优选地,步骤(3)包括以下子步骤:
(3-1)对步骤(2)中得到的两幅图像的每个特征描述子分别进行降维处理;
(3-2)根据降维处理的结果构建对应的哈希函数,并利用该哈希函数构建哈希表;
(3-3)利用步骤(3-2)构建的哈希函数对步骤(3-1)得到的两幅图像降维处理后的每个特征描述子分别进行哈希运算,如果出现本应互相匹配的两个特征描述子对应的哈希桶不相同,或者本应不互相匹配的两个特征描述子对应的哈希桶相同的情况,则设置扰动向量序列(Δ1,Δ2,…,Δp),其中p表示序列长度,其取值范围是小于64,Δ的取值为任意实数,且各不相同,然后将扰动向量序列与第二幅图像对应特征描述子哈希计算的结果相加,从而得到一系列新的哈希值,随后判断这一系列新的哈希值中每一个对应的哈希桶中是否存在有第一幅图像的特征描述子,如果没有,则不处理,如果有,则计算该哈希桶中两幅图像的特征描述子之间的多个汉明距离,并寻找其中最小的汉明距离,该汉明距离对应的两个特征描述子就是两幅图像特征匹配的结果;
(3-4)使用顺序抽样一致性(Progressive Sample Consensus,简称PROSAC)算法对步骤(3-3)得到的两幅图像特征匹配的结果进行剔除误匹配的操作,从而得到最终特征匹配的结果。
优选地,步骤(3-2)中构建哈希函数具体是采用以下公式:
H=hm-1·m+hm-2·(m-1)+...+h0·1
其中hk表示降维后的特征描述子的第k位,m表示降维处理后的维数,且有k∈(0,m-1)。
优选地,变换矩阵包括旋转变换矩阵R,以及位移变换矩阵T;
且有:
Y=RX+T
其中X和Y分别表示已经特征匹配好的两幅图像上的特征描述子的三维坐标。
优选地,步骤(6)具体包括以下子步骤:
(6-1)使用反向索引方法对场景描述后的图像与视觉词典中出现过的每幅图像分别进行预匹配处理,以得到预匹配处理结果;
(6-2)对所有预匹配处理的结果进行基于分层加权匹配的相似度计算,将小于等于预设阈值的相似度计算结果所对应的预匹配结果作为候选闭环;在本实施方式中,预设阈值的范围是大于等于0.6,小于等于1;
本步骤具体采用以下公式:
其中S(X,Y)表示场景描述后的图像与预匹配处理后视觉词典中出现过的图像之间的相似度,L表示视觉词典的层数,ηl表示视觉词典中第l层的匹配强度系数,ΔSl表示视觉词典中第l层与其相邻层之间的相似性增量,k表示视觉词典的深度。
(6-3)基于时间连续性从多个候选闭环中确定最终的闭环,用于对步骤(4)得到的运动估计结果进行优化。
优选地,使用的非线性最小二乘法是通用图优化框架或Ceres Solver库,栅格地图法是八叉树法。
按照本发明的另一方面,提供了一种通过改进ORB-SLAM算法实现定位与构图的系统,包括:
第一模块,用于使用机器人携带的深度视觉传感器采集数据集中的连续两幅图像,以分别得到每一幅图像的彩色图像与深度图像,使用显著性区域检测算法对得到的彩色图像进行感兴趣区域初步提取,使用阈值分割法对提取到的感兴趣区域进行优化,以得到优化后的图像;
第二模块,用于在离散化尺度空间下使用ORB算法对第一模块得到的优化后的两幅图像进行特征提取,以得到两幅图像的特征描述子;
第三模块,用于对第二模块中得到的两幅图像的特征描述子进行特征匹配,以生成两幅图像特征匹配的结果;
第四模块,用于使用第三模块得到的两幅图像特征匹配的结果获得变换矩阵,并将该变换矩阵作为算法迭代初值输入到ICP算法中,以得到运动估计结果。
第五模块,用于对第二模块中得到的两幅图像的特征描述子进行特征单词化处理,基于处理后得到的单词构建视觉词典,并利用视觉词典中的单词对每一幅图像进行场景描述;
第六模块,用于对场景描述后的图像与视觉词典中出现过的每幅图像分别进行预匹配处理,对所有预匹配处理的结果进行相似度计算,根据相似度计算结果确定多个候选闭环,基于时间连续性从多个候选闭环中确定最终的闭环,用于对第四模块得到的运动估计结果进行优化;
第七模块,用于使用非线性最小二乘法对第六模块优化后的运动估计结果进行位姿图优化,以得到全局最优的机器人位姿和全局一致的运动轨迹,并利用栅格地图法对该运动轨迹进行建图。
总体而言,通过本发明所构思的以上技术方案与现有技术相比,能够取得下列有益效果:
(1)本发明能够解决现有ORB-SLAM算法中存在的会消耗大量的运算时间的技术问题:由于本发明采用了步骤(1)提取感兴趣区域,避免了对于整幅图像进行特征提取,由此缩短了处理和运算时间;此外,由于本发明采用了步骤(3)的降维处理和哈希运算,进一步缩短了处理时间。
(2)本发明能够解决ORB-SLAM算法中场景发生大尺度变化或者视角变化时会产生误差的技术问题:由于本发明步骤(3)中是在尺度空间下进行ORB特征提取,并且利用PROSAC算法对特征匹配的结果进行剔除误匹配的操作,从而提高了匹配精度。
(3)本发明能够解决ORB-SLAM算法存在的应用范围有限的技术问题:由于本发明步骤(7)中采用栅格地图法对运动轨迹进行建图,得到的地图能够应用于包括导航、避障、交互等的应用领域中。
附图说明
图1是本发明方法中步骤(1)获得的彩色图像。
图2是本发明方法中步骤(1)获得的深度图像。
图3是使用显著性区域检测算法对彩色图像进行感兴趣区域初步提取的示意图。
图4是本发明方法中步骤(1)处理后得到的优化后的图像。
图5示出现有技术的暴力匹配法得到的特征匹配结果。
图6示出本发明方法的步骤(3)的匹配过程得到的特征匹配结果。
图7(a)至(d)示出使用基准数据集中的四个RGB-D数据包得到的建图结果。
图8示出本发明的方法和RGBD-SLAM算法、ORB-SLAM算法、以及ElassticFusion算法在精度方面的性能比较。
图9示出本发明的方法和RGBD-SLAM算法、以及ORB-SLAM算法在实时性方面的性能比较。
图10是本发明通过改进ORB-SLAM算法实现定位与构图的方法的流程图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅仅用以解释本发明,并不用于限定本发明。此外,下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
如图10所示,本发明通过改进ORB-SLAM算法实现定位与构图的方法包括以下步骤:
(1)使用机器人携带的深度视觉传感器采集数据集中的连续两幅图像,以分别得到每一幅图像的彩色图像(如图1所示)与深度图像(如图2所示),使用显著性区域检测算法对得到的彩色图像进行感兴趣区域初步提取(如图3的方框所示),使用阈值分割法对提取到的感兴趣区域进行优化,以得到优化后的图像(图4中的方框表示优化后得到的感兴趣区域);
具体而言,本步骤中使用的深度视觉传感器是Kinect传感器,优选是Kinect 2.0传感器。
本发明使用的数据集是TUM RGB-D Benchmark基准数据集。
本步骤中使用的显著性区域检测算法是集合多层次卷积特征(AggregatingMulti-level Convolutional Features,简称AMULET)算法、基于区域的全卷积网络(Region-based fully convolutional networks,简称R-FCN)算法、空间加权相异性(Spatially weighted dissimilarity,简称SWD)算法等。
(2)在离散化尺度空间下使用ORB算法对步骤(1)得到的优化后的两幅图像进行特征提取,以得到两幅图像的特征描述子;
具体而言,本步骤建立离散化尺度空间是按照如下方式实现:首先构造出一个由n(其中n可以为任意正整数)个图像层li(其中i∈(0,n-1))和n个间隔层bi组成的尺度空间,其中图像层的l0表示优化后的图像,通过对优化后的图像进行1.5倍的下采样来获得第一层间隔层b0,然后分别对l0和b0逐层半采样以获取接下来每层的图像层lj和间隔层bj(其中j∈(1,n-1))。假设σ表示每层的尺度值,则图像层li的尺度值为σ(li)=2i,间隔层bi的尺度值为σ(bi)=2i×1.5。
由于机器人在室内的工作环境空间不会过大,在进行定位和构图时,场景中的物体尺度变化也不会过大,并且对于过远物体,会在图像域预处理中视为背景不进行处理,因此为了减少在构建尺度金字塔阶段消耗的时间,选取n=2,此时所构成的尺度空间包含了四个图像层。
(3)对步骤(2)中得到的两幅图像的特征描述子进行特征匹配,以生成两幅图像特征匹配的结果;
本步骤包括以下子步骤:
(3-1)对步骤(2)中得到的两幅图像的每个特征描述子分别进行降维处理;
本步骤中使用的是随机提取方法执行降维处理,降维后的维数m通常是小于等于64,优选值为32。
(3-2)根据降维处理的结果构建对应的哈希函数,并利用该哈希函数构建哈希表;
本步骤中构建哈希函数具体是采用以下公式:
H=hm-1·m+hm-2·(m-1)+...+h0·1
其中hk表示降维后的特征描述子的第k位,且有k∈(0,m-1)。
(3-3)利用步骤(3-2)构建的哈希函数对步骤(3-1)得到的两幅图像降维处理后的每个特征描述子分别进行哈希运算,如果出现本应互相匹配的两个特征描述子对应的哈希桶不相同,或者本应不互相匹配的两个特征描述子对应的哈希桶相同的情况,则设置扰动向量序列(Δ1,Δ2,…,Δp),其中p表示序列长度,其取值范围是小于64,Δ的取值为任意实数,且各不相同,然后将扰动向量序列与第二幅图像对应特征描述子哈希计算的结果相加,从而得到一系列新的哈希值,接下来,判断这一系列新的哈希值中每一个对应的哈希桶中是否存在有第一幅图像的特征描述子,如果没有,则不处理,如果有,则计算该哈希桶中两幅图像的特征描述子之间的多个汉明距离,并寻找其中最小的汉明距离,该汉明距离对应的两个特征描述子就是两幅图像特征匹配的结果。
(3-4)使用顺序抽样一致性(Progressive Sample Consensus,简称PROSAC)算法对步骤(3-3)得到的两幅图像特征匹配的结果进行剔除误匹配的操作,从而得到最终特征匹配的结果。
图5和图6分别示出现有技术的暴力匹配法和本步骤的匹配过程的特征匹配结果比较,可以看出使用优化后的匹配策略进行特征匹配不仅在结果的精确度上优于暴力匹配方法;
从下表1也可以看出,本发明的匹配过程在速度上也明显优于暴力匹配方法。
表1匹配性能对比
(4)使用步骤(3)得到的两幅图像特征匹配的结果获得变换矩阵,并将该变换矩阵作为算法迭代初值输入到迭代最近点(Iterative Closest Point,简称ICP)算法中,以得到运动估计结果。
具体而言,变换矩阵包括旋转变换矩阵R,以及位移变换矩阵T;
且有:
Y=RX+T
其中X和Y分别表示已经特征匹配好的两幅图像上的特征描述子的三维坐标。
(5)对步骤(2)中得到的两幅图像的特征描述子进行特征单词化处理,基于处理后得到的单词构建视觉词典,并利用视觉词典中的单词对每一幅图像进行场景描述;
具体而言,本步骤已经在基于视觉词典(Bag-of-Visual-Words,简称BoVW)的闭环检测方法中得以详细描述,在此不再赘述。
(6)对场景描述后的图像与视觉词典中出现过的每幅图像分别进行预匹配处理,对所有预匹配处理的结果进行相似度计算,根据相似度计算结果确定多个候选闭环,基于时间连续性从多个候选闭环中确定最终的闭环,用于对步骤(4)得到的运动估计结果进行优化;
本步骤具体包括以下子步骤:
(6-1)使用反向索引方法对场景描述后的图像与视觉词典中出现过的每幅图像分别进行预匹配处理,以得到预匹配处理结果;
具体而言,反向索引方法为:在视觉字典中标记出每个单词出现过的感兴趣区域和场景,当需要为当前时刻关键帧搜索出可能出现的闭环时,通过视觉词典找到其具有一定比例(该比例可人为确定)的公共单词的相似感兴趣区域,从而将获得的各个相似感兴趣区域所出现的可能图像作为预匹配处理结果。对于每个场景描述后的图像的感兴趣区域个数可能会不同,可以根据该条件剔除掉预匹配处理结果里感兴趣区域明显少于当前时刻图像感兴趣区域个数的图像,余下的图像继续本步骤后的处理过程。
(6-2)对所有预匹配处理的结果进行基于分层加权匹配的相似度计算,将小于等于预设阈值的相似度计算结果所对应的预匹配结果作为候选闭环;在本实施方式中,预设阈值的范围是大于等于0.6,小于等于1;
具体而言,本步骤中具体采用以下公式:
其中S(X,Y)表示场景描述后的图像与预匹配处理后视觉词典中出现过的图像之间的相似度,L表示视觉词典的层数,ηl表示视觉词典中第l层的匹配强度系数,ΔSl表示视觉词典中第l层与其相邻层之间的相似性增量,k表示视觉词典的深度。
(6-3)基于时间连续性从多个候选闭环中确定最终的闭环,用于对步骤(4)得到的运动估计结果进行优化。
(7)使用非线性最小二乘法对步骤(6)优化后的运动估计结果进行位姿图优化,以得到全局最优的机器人位姿和全局一致的运动轨迹,并利用栅格地图法对该运动轨迹进行建图。
具体而言,本步骤中使用的非线性最小二乘法是通用图优化框架(Generalframework for graph optimization,简称G2O)或Ceres Solver库,本步骤中使用的栅格地图法是八叉树法。
最终建图的结果如图7的(a)到(d)所示,其分别对应于TUM RGB-D Benchmark基准数据集中的fr1_desk、fr1_room、fr2_desk、fr2_360_hemisphere四个RGB-D数据包。
实验结果
为了验证本发明提出算法的精度和实时性,下面就这两方面将改进算法的绝对轨迹误差的均方根和算法运算时间同RGBD-SLAM算法、ElassticFusion算法和ORB-SLAM算法进行对比验证实验,如图8和9中所示。其中ElassticFusion算法在fr2_360_hemisphere数据集失败,且其需要GPU加速,因此在时间上不进行对比。
由图8可知,在精度方面,本发明方法的平均RMSE约为RGBD-SLAM算法的28%,提高了约3.5倍;约为ElassticFusion算法的76%,虽然提高不多,但鲁棒性更好;约为ORB-SLAM算法的85%,精度只提高了1倍。
由图9可知,在实时性方面,平均算法运行时间减低为RGBD-SLAM的42%,ORB-SLAM算法的48%。通过对比系统运行时间和绝对轨迹的均方根误差,可以得出本发明的方法相比于传统的RGBD-SLAM算法不仅可以更好的提高系统的精度还提高系统的运行效率;而相比于传统的ORB-SLAM算法,虽然定位的精度基本相当,但是其运算时间明显的提高了,也就是说可以在更短的时间内达到了相当的精度,这主要得益于关键帧的提取和感兴趣区域提取功能,尤其在大环境规模下长时间构图的时候优势将会更加明显。因此,很好的说明了本发明方法的有效性和可行性。
本领域的技术人员容易理解,以上所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等,均应包含在本发明的保护范围之内。