室内环境下深度相机V-SLAM的稠密建图

洪运志，吴怀宇，陈志环

(武汉科技大学 机器人与智能系统研究院，湖北 武汉 430081)

0 引 言

随着移动机器人行业飞速发展，视觉同步定位与建图[1](V-SLAM)问题成为了机器人领域的热点研究。由于传感器中的深度相机可以直接获取环境的彩色及深度信息，使得SLAM数据处理相对直接，所以深度相机被广泛应用于V-SLAM的地图构建[2,3]。韦兰等[4]利用深度相机采集到的数据，结合图像特征进行闭环检测和优化，得到质量较高的点云后，再进行室内稠密地图重建。还有些学者利用SFM[5]来解决场景的稠密建图问题，比如Kinect Fusion和Elastic Fusion，但其实现的平台一般需要配置强大的GPU来确保系统的实时性。Felix Endres等[6]提出的SLAM系统利用深度相机从彩色图像中提取视觉特征，并使用深度信息来确定物体的三维位置，该方案虽没要求用GPU，但使用SIFT提取特征点和所有帧来建图，导致其运算消耗较大，重建的三维点云地图冗余点较多，不够紧凑。Mur-Artal和Tardós[7]开发了ORB-SLAM2，该算法使用图优化，能够高精度地实现机器人定位，但其地图是由稀疏的环境特征点构成，应用性较差。

由以上研究现状可知：虽然深度相机V-SLAM中建图研究已经取得了一定突破，但当前针对输入数据中的深度图像的研究[8]并不多。考虑到深度图像作为V-SLAM的系统性误差将直接影响系统的定位精度，因此，本文将对深度图像去噪进行研究。另外，作为SLAM建图中的重要环节，图像关键帧[9]提取并没有具体规则，关键帧若能较好地提取，那对建图的准确性和快速性将会有着重要的意义。并且，当前深度相机V-SLAM的地图构建，大多数都是关于无序三维空间点构成的简单环境点云地图，很少涉及点云图生成之前的优化处理环节，且点云地图本身规模较大，如果应用于移动机器人导航[10]等应用，则需要更加紧凑，应用性更强的地图来呈现。

基于以上分析，本文提出一种基于改进关键帧选取和点云优化的V-SLAM稠密建图方法，实现室内环境3D OctoMap 地图快速构建。方法理论充分，实践可行，构建的稠密地图应用性强，存储代价小，满足室内环境下深度相机V-SLAM建图的实际需求。

1 本文工作

1.1 方法流程和结构

本文提出的室内环境下深度相机V-SLAM的稠密建图总体结构如图1所示，主要包括深度图像去噪与彩色图配准，SLAM位姿估计与关键帧提取，SLAM地图构建与优化3个部分，最后是实验设计与验证。

图1 深度相机V-SLAM稠密建图总体结构

1.2 深度相机模型

(1)深度相机标定

为提高深度相机数据的精度，在使用深度相机之前需要对其进行标定。以微软公司生产的Kinect为例，需要经过标定得到其内参矩阵K，如式(1)所示

(1)

式中：fx，fy为相机在X，Y两个轴上的焦距；cx，cy为相机坐标系下相机光圈中心。本文使用ROS自带的Calibration标定包对Kinect进行摄像头的标定。

(2)从二维图像到三维空间

根据针孔相机模型，设点云中的一个空间点(x，y，z)，已知其在图像中坐标为(u，v，d)，再采用式(2)进行转换，就可以确定空间点的位置

z=d/s
x=(u-cx)·z/fx
y=(v-cy)·z/fy

(2)

式中：s指深度相机的缩放因子，d为深度信息。

1.3 深度图像去噪

深度相机在拍摄物体的边缘或者颜色较深的位置时通常会产生空洞噪声(图2(a)中记号部分)，从而致使深度值缺失。

图2 深度图像去噪

文献[11]介绍了一种基于FMM算法的深度图像空洞修复方法，在深度图的一个小的邻域内，深度值是连续的，故空洞像素点p可以由其领域内的有效像素来估计，如式(3)所示

(3)

该方法虽能对空洞噪声进行去除，但其需要计算彩色图像的结构相似性(SSIM)来确定待修补空洞区域像素p(x，y)， 算法的复杂度较高，不易实现。

所以本文通过改进FMM算法，根据深度图像素阈值特点，设计一种基于反二值阈值化分割方法来确定修复区域Ω中的待修复像素p(x，y)。 改进FMM算法伪代码如下：

一种改进的FMM算法

(1)δΩ=待修复区域的边缘，p=待修补的空洞像素，Head函数中⊕为图像的膨胀

(2)while(δΩ非空)

(3)p(x，y)←Head(dΩ⊕dΩ-dΩ)

(4) for(m，n) in (x+1，y)， (x，y+1)， (x-1，y)， (x，y-1)

(5) if flag(m，n)≠know

(6) if flag(m，n)=inside

(7) inpaint(m，n)

(8) end if

(9) end if

(10) end for

(11)end while

设定图像的像素有3种：know表示δΩ外部不需要修复的像素，inside表示δΩ内部等待修复的像素。其中，dΩ表示OpenCV中函数的修复掩膜，dΩ(x，y) 是确定待修复区域掩膜的反二值阈值化函数，如式(4)

(4)

式中：根据深度图像空洞噪声的阈值范围将深度图像二值化，即灰度值不大于阈值threshhold的像素设置为255，其余设置为0，得到图像待修复区域的掩膜。然后，将其结合到FMM算法中对图像进行修复。

如图3所示，经过本文改进的FMM算法处理后，深度图的黑洞噪声被明显去除，验证了所提出方法的有效性。

图3 深度图像修复

1.4 彩色图像特征提取与匹配

在基于特征点的SLAM过程中，常用的提取算子和特征描述有SIFT、SURF、和ORB等。ORB算子计算速度较快且保持了特征子的旋转、尺度不变性，综合考虑SLAM建图对准确性和快速性需求，本文选用ORB特征进行特征提取。

特征提取完成后，为了实现较高效率的特征匹配，采用FLANN实现初步的特征匹配。同时，为了去除错误的匹配，本文还结合RANSAC算法剔除特征匹配中的误匹配。

1.5 位姿估计与优化

匹配完成后，就是求解相机的位姿变换矩阵。深度相机V-SLAM中是由3D-2D点估计法来解决位姿估计与优化问题。

(1)PnP法估计相机位姿

PnP[12]算法是求解3D-2D点估计的算法。在PnP中，已知n个点的3D位置和图像中的投影，需要估计出相机位姿。已知两帧图像F1和F2， 并且匹配到了一组对应的特征点P={p1,p2,…,pN}∈F1，Q={q1,q2,…,qN}∈F2， 其中，p和q都是已经求出的匹配好的特征点。然后需要求而出一个旋转矩阵R和位移矢量t，使得式(5)成立

∀i，pi=Rqi+t

(5)

(2)非线性优化

因为有未知的相机位姿和来自观测点的噪声，所以式(5)必然存在一个误差。为减小这个误差，需构建最小二乘问题，然后调整相机位姿都是错的是使其达到最佳位姿，从而使误差最小化，基于深度相机V-SLAM的误差函数如式(6)

(6)

式(6)被称作重投影误差，它是计算3D点位姿估计与观测值之间的误差。其目的就是找到一对 (R,t)， 使得误差函数(R,t) 取得最小值。式(6)中，pi为相机当前帧图像中的匹配点，qi为当前帧运动后一帧中图像的匹配点。

1.6 关键帧筛选

在SLAM过程中，如果图像的位移超过一定的值，就将其定义为一个关键帧，本文利用关键帧进行地图构建。为了减少建图中的点云拼接冗余、不必要的计算量以及存储浪费，需要对SLAM过程中的图像关键帧进行筛选和提取。

本文采用一种基于特征相关性筛选的关键帧选取机制，算法流程如图4所示。

图4 基于特征相关性的关键帧筛选算法

记当前帧FC，新关键帧FK，上一关键帧FK-1，首先，定义一个度量帧间运动的量D，其定义如式(7)

(7)

D是用帧间位移量r和旋转量t的范数之和表示，然后将其与一个标准量Dmax比较，当帧间运动大于标准量阈值的时候，判断此时的匹配误差过大，从而舍弃该帧，重新开始下一帧判断。当D≤Dmax时，定义一个两帧之间特征点数量占比的量E，其定义式如式(8)

(8)

式中：SFC，FK-1为当前帧与上一关键帧匹配得到的特征点数量，SFK-1为上一关键帧的特征点数量，这两者的比值表示两帧之间的运动尺度E，若当E>Emax时，说明两帧之间的运动尺度过小，不适合作为关键帧，返回到重新判断；当E≤Emax时，才判断当前帧适合作为关键帧保存。

然后，在SLAM过程中重复以上步骤，得到所有的关键帧，供点云拼接使用。

1.7 地图构建

(1)点云图拼接

点云图的拼接是由点云变换得来，这个变换是用变换矩阵T如式(9)来描述

(9)

式中：T为相机位姿的变换矩阵，包括1.5节求得的旋转矩阵R和平移向量t。

设位姿变换前某空间点为(x1，x2，x3)，变换后空间点为(y1，y2，y3)，结合式(9)，利用式(10)变换关系对空间点进行位姿变换，获得变换后的点云信息

(10)

(2)点云滤波和下采样

点云图拼接完成后，考虑到图中存在的冗余点和噪声点，需对点云加一些滤波处理，以获得更清晰紧凑的建图效果。在本文的建图方法中，基于PCL点云库，采用了去外点滤波以及点云下采样优化方法。

1)由于Kinect 的量程有限，在生成点云时，去掉深度值d在5000-8000之外的点。

2)为了去掉孤立的噪声点，利用统计滤波器方法去除地图中每个点与它最近N个点之外的点，N值通常为 40-80 左右。

另外，体素滤波能保证在某个一定大小的立方体(体素格)内仅有一个点，相当于对三维点云进行下采样，从而能节省存储空间。本文在建图上还利用体素滤波器对点云下采样。针对本文建图方法，下采样分辨率S一般取0.001-0.01。

(3)3D OctoMap转换

点云图有一些明显的缺点：点云图数据量通常很大而且不紧凑，需要大量的存储资源；点云图不能应用于机器人导航和路径规划。而3D OctoMap[13]能够更加直观地表示环境中的占用、空闲，更有利于机器人导航和路径规划。基于以上分析，本文采用一种3D栅格化处理方法，将点云图转换成应用性更强、存储代价更低的3D OctoMap地图。

2 实验与结果分析

2.1 实验平台

本实验计算机配置为：CPU为Intel Core i5 5800U处理器，8 GB内存，主频2.5 GHz，系统是Ubuntu14.04。实际环境的数据来源于JS-R移动机器人实验平台，如图5(a)所示，该移动平台搭载ROS，通过ROS采集图像数据。软件开发环境主要采用OpenCV2.4.8、Eigen库以及点云库PCL1.7。本文实际场地实验数据取自武汉科技大学如图5(b)所示。

图5 实验平台与实验室环境

进一步的，为了说明本文建图方法的有效性与一般性，实验还选取了来自德国弗莱堡大学著名的开源TUM 数据集中的FR1/XYZ室内环境。针对此数据集，将本文方法建图结果与ORBSLAM2和RGB-D SLAM v2的建图结果进行对比实验。

2.2 特征提取与配准

针对输入数据中彩色图像，提取其ORB特征点，并计算关键点和描述子，利用FLANN算法得到初次匹配如图6所示，此时产生了391个匹配特征点。

图6 初次特征匹配获取的匹配

接着，使用RANSAC算法对上述匹配结果进行进一步的筛选，得到的有效匹配数量为52，其匹配结果如图7所示。

图7 经过FLANN及RANSAC算法后的匹配

2.3 实际室内环境稠密建图

(1)位姿估计与关键帧提取

根据PnP匹配算法和最小化重投影误差函数对估计出的位姿进行优化，求解出图像帧之间的相机旋转向量r和位移t。同时，根据采用的关键帧提取方法，对机器人在图5中的室内环境1中采集到的117帧彩色、深度数据帧，进行了关键帧提取与位姿估计，设置Dmax=0.4，Emax=0.8， 得到了31帧关键帧及其位姿信息。为了使位姿估计更加直观展示，实验将相机的关键帧运动在X/Y平面上进行投影，由于实验室无法捕捉机器人运动真实值，所以本文仅将得到的位姿用MATLAB在X/Y平面上表示，如图8所示。

图8 室内环境1中机器人在X/Y平面移动轨迹

图8所表现的是，START点设置的为JS-R机器人在室内环境1中运动的起点，箭头为机器人实际的运动方向，小圆圈代表所选取机器人相机关键帧所处的位置，END点为捕捉机器人最后一帧关键帧时刻的位置点。从轨迹形状看，整体运动稳定且没有出现奇点，运动路径平稳，说明位姿估计较为准确。

(2)点云生成与滤波优化

实验数据选取的是室内环境2下拍摄获取的20帧彩色、深度图像。图9显示了武科大某室内环境2的点云地图经过滤波优化处理的过程：原始点云图显示在图9(a)、图9(b)的左侧，而经过本文方法的点云优化图如图9(c)所示。

图9 室内环境2点云图构建与优化

从图9(b)中圆圈标注部分可以看到，点云拼接后的图存在大量离群点和冗余点；而经过本文滤波方法处理后(图9(c))，这些噪声点明显大量地被去除，得到的点云图效果更平滑和清晰，说明本文滤波优化方法对点云噪声处理效果出色，实际作用明显。

(3)稠密地图构建

为验证本文建图方法在实际室内环境中的有效性，实验借助JS-R机器人移动平台，对室内环境1进行实际场地的建图实验。同时，使用3D OctoMap来实现紧凑的三维地图转换。本文将室内环境1和环境2的点云图最终转换成3D OctoMap地图形式，效果如图10所示。

图10 某室内环境1和环境2的稠密建图

图10(b)是从室内环境2构建的3D OctoMap，图10(d)和图10(e)是从图室内环境1构建的3D OctoMap。从建图效果可以看出，转换后的地图更加具体和直观，这对于SLAM应用于机器人导航和避障具有直接的现实意义，同时，所构建的稠密地图比点云图更加紧凑和准确，说明了本文稠密建图方法的准确性和实用性。

2.4 与经典开源方案的建图对比实验

针对TUM-RGB-D中的FR1/XYZ数据集，进行了建图对比实验。结果如图11所示。

图11(a)是ORB SLAM2室内环境重建图，其地图形式为稀疏的点云，对室内环境的描述效果较差，不能判断环境中的物体的类别属性。图11(b)是RGB-D SLAM v2的重建地图，其对室内环境的描述比较丰富，各个物体模样基本可以知道，但其重叠点云以及冗余性数据比较多。图11(c)是本文方法得到的点云地图，其对室内物体的描述相较于ORB SLAM2要丰富得多，与RGB-D SLAM v2对比，如图11(b)、图11(c)中圆圈部分所示，电脑上边缘处和桌面书本处等细节地方重叠和冗余明显减少，整体处理地更清晰，效果较好。图11(d)是经过3D OctoMap转换后的地图，该地图形式较于点云图，能更清楚直观地反映所处环境的具体情况，同时还能大大减少存储空间，进一步地说明了本文稠密建图方法较于一般SLAM建图方法的有效性和高效性。

图11 FR1/XYZ数据集稠密建图

2.5 结果分析

为了说明本文稠密建图质量和效率的综合表现，将1201实验室室内环境1、环境2以及FR1/XYZ数据集建图的数据进行对比。其中，一些参数设置为：深度误差去除的参数d=7000，离群点滤波参数N=50，体素滤波体素格大小为0.01。室内环境2八叉树叶子节点大小(分辨率)设为0.01，室内环境1和FR1/XYZ的叶子节点大小设为0.04，稠密建图的点云和存储空间变化见表1。

表1 稠密地图节点数和存储大小

表1中的数据显示，3种室内环境的点云图在经过本文的滤波优化以及八叉树转换后，稠密图中的稀疏异常值都得到了明显去除，地图数据质量得到了很大的改善，同时，也减少了存储消耗。图12展示了3种室内环境下稠密建图的地图数据量的平均变化率。

图12中可知，如将处理前的点云图数据点数量和存储大小都设置为100%，那么经过滤波优化处理后的点云数据点下降了接近10%，存储占用下降了68.53%。经过3D OctoMap地图转换后的地图数据点数量下降了接近97%，存储规模下降了98%左右。再次验证，根据本文的建图方法，可以构建一个更准确和紧凑的地图。

图12 室内环境地图平均数据变化(百分率)

同时，为说明本文建图效率，对TUM中的FR1_XYZ数据集，从建图的帧数和建图时间来比较RGB-D SLAM v2、ORB SLAM2以及本文方法的差异，表2为这3种方法的关键帧数量以及耗时。

表2 本文建图方法与RGB-D SLAM v2、ORBSLAM方案对比

由表2可知，ORB SLAM2的运行效率最好，平均帧率和建图耗时也是最优的，但其地图为稀疏点云图，对室内环境的描述太弱，缺乏实际应用性。

使用本文建图方法的FR1/XYZ数据集的关键帧提取数为65帧，平均帧率为8.35 Hz，帧率比RGB-D SLAM v2提高了26.1%，点云建图耗时则下降了48.7%，从实时性方面本文方法表现更好。综上所述，本文稠密建图方法能够较为快速、准确地实现室内环境三维地图构建，构建的3D OctoMap也更符合实际应用需求。

3 结束语

本文设计的室内环境下深度相机V-SLAM的稠密建图方法，该方法首先改进FMM算法并对深度图进行去噪修复，对彩色图提取ORB特征；然后利用PnP算法和最小化重投影误差估计相机位姿，并设计了一种关键帧选取机制提取关键帧；最后，采用一种结合滤波和下采样的点云图优化方法，并将优化后的点云图转换为3D OctoMap地图，最终实现三维稠密地图构建。实验2.3说明了本文建图方法的有效性以及能较好地满足实际应用性需求，实验2.4验证了方法的快速性和准确性，所提出的方法能够较为简便、高效地实现室内环境稠密地图构建，具有较好的实际应用价值。

另外，本文后续重点工作将考虑引入基于外观的回环检测部分，以减小累积误差从而使建图结果更精确。