线特征优化配置在室内RGB-D SLAM 系统中的应用

夏琳琳，崔家硕，宋梓维，胡源子，胡智棋

（东北电力大学 自动化工程学院，吉林 132012）

同时定位与建图（Simultaneous Localization and Mapping,SLAM）技术已经进入“鲁棒感知时代”[1]。如何在动态物体干扰、纹理缺失、剧烈光照变化等挑战情形下保证系统的可靠性、感知力，已成为当今视觉SLAM（Visual SLAM,V-SLAM）领域亟待解决的问题[2]。

点、线和平面是常见的几何特征，早期的MonoSLAM、PTAM 和近期的ORB-SLAM 系列等奠定了特征点法SLAM 的理论基础。近年来，通过补充线段、平面等多特征基元，研究人员在不新增传感器成本的前提下，成功开发出许多具有多观测约束的纯视觉SLAM 或视觉-惯性导航系统（Visual-Inertial Navigation System,VINS）。

Y.Li 等人[3]开发出一种面向“曼哈顿世界”的、综合利用点、线、平面特征的无漂移V-SLAM 系统，并探索突出几何结构信息、弱化纹理信息的“解耦”位姿估计方法；孙沁璇等人[4]针对平面特征不够充足而导致位姿求解的退化问题，提出平面和直线自适应融合的RGB-D 相机6 自由度（Degree of Freedom,DoF）位姿估计方法；在VINS 领域，J.Lee 等人[5]在基于点、线特征的位姿估计中，进一步集成了平行的3D 线约束，展现了线特征约束下的VINS 定位优越性。

与上述多观测约束的V-SLAM/VINS 相比，本文明确提出了基元的优化配置问题。拟开发集成点-线-平面-物体的RGB-D SLAM 系统，以丰富的线-平面特征为弱纹理区域的RGB-D 相机位姿估计提供额外约束。通过基准数据集实验，以最小的绝对位姿误差（Absolute Pose Error，APE），确定最优的线特征提取数量，使得本系统既充分利用物体边缘和平面交线的线特征，提升室内SLAM 位姿估计的鲁棒性，又剔除掉低质量线段对位姿估计在精度、效率上的不利影响。为兼顾目标检测的准确性，物体特征即路标仅用于轻量型语义地图的构建。

1 系统框架

所开发的集成点-线-平面-物体特征基元的RGB-D SLAM 系统框架如图1 所示。

图1 所开发的RGB-D SLAM 系统框架Fig.1 The framework of the developed RGB-D SLAM

继承先进的ORB-SLAM2，该系统框架可划分为两部分：前端（Front End）和后端（Back End）。

（1）前端从RGB 图像序列中提取点、线段和物体包围框等基元；从深度图像（Depth Image）中分割平面。之后，通过数据关联（Data Association）算法将当前帧观测到的全部基元联结到存在于全局地图中的地图路标。

（2）后端通过点、线和平面路标优化相机位姿，并最终输出一张融合几何、结构和语义元素的多层级导航地图。不同于文献[6,7]等，为确保目标检测的准确性，物体包围框并未参与到前端视觉里程计（Visual Odometry，VO）、后端优化及回环检测（仅依赖ORB特征点构造的视觉词袋模型）线程。

2 几何-物体特征

设计集成点-线-平面特征的VO，是提升其定位精度和鲁棒性的可行途径[8]。其中，特征提取是图像匹配、图像检索等的重要基石[9]。本文的RGB-D SLAM系统遵循ORB-SLAM2 中点特征的提取、描述、匹配及参数化过程，并通过透视 n 点定位（Perspective-n-Point，PnP）方法估计相机运动，在此不再赘述。类比点特征，本节探索线、平面及物体特征的提取、匹配（关联）和参数化方法。

2.1 线特征

2.1.1 线特征提取

经典的点-线融合SLAM 研究均采用线段检测（Line Segment Detector,LSD）算法提取线特征。LSD的设计思想是沿梯度方向寻找、合并相似的像素。与ORB 特征点相似，LSD 通过构造图像金字塔获得尺度不变性，通过计算线段支持域的主方向获得旋转不变性[10]。图2 分别给出ICL-NUIM 数据集和EuRoC 数据集上LSD 线段检测结果，前者多用于RGB-D SLAM评估，后者多用于VINS 评估。可以看出，LSD 能够正确检测出物体边缘和平面交线上的线特征。

图2 LSD 线段检测结果Fig.2 Line segment detection results by LSD

2.1.2 线特征描述与匹配

L.Zhang 等人提出的线段条带描述子（Line Band Descriptor,LBD）描述线特征的主要手段。借鉴其在文献[11]中给出的LBD 支持域示意图，定义LBD描述子为：

其中，Mj和Sj（j=1,2…m）分别为描述矩阵的均值向量和标准方差向量；变量m表示支持域中的条带数。当条带数m=9、条带宽度为7 时，LBD描述子具有最佳的匹配效果，是由72 个浮点数组成的向量。

类似于点特征，线特征的匹配方式可采用基于LBD描述符的暴力匹配[12]。对于误匹配情况，其对相机位姿估计的影响可由随机抽样一致（Random Sample Consensus,RANSAC）算法和鲁棒代价函数抑制。

2.1.3 空间线的参数化

对于图像帧中提取到的二维线段特征lm=(xstart,xend)，如果二维线段特征的两个端点都具有正确深度，那么可以直接将端点反投影为三维点(Xstart,Xend)，两个三维端点确定一条 3D 线段L=(Xstart,Xend)；如果二维线段特征的两个端点深度测量异常，那么首先反投影下那段上的全部像素点，而后采用RANSAC 算法鲁棒拟合3D 直线。

2.2 平面特征

2.2.1 平面特征提取

无纹理的平面区域几乎不包含任何显著的点、线特征，然而相关研究表明[13]：若环境包含至少两个非平行平面时，平面特征对相机位姿估计至少提供了5DoF 的约束，因此，本文将平面扩充为用于相机位姿估计的新的视觉特征。平面特征的提取借鉴一种基于连通量的有序点云高效平面分割算法：深度摄像头获得的深度图通过相机内、外参数还原为三维点云，平面分割算法计算三维点云的表面法线；与此同时，从已经计算表面法线的点云中分割出平面。

2.2.2 空间平面的参数化

对于空间平面的参数化，黑塞法线（Hessian Normal）是一种常用平面参数化形式。给定空间平面的单位法向量n=(n x,n y,nz)T和其距相机原点O的距离dp，即可定义黑塞法线形式的平面π=(nT,dp)T。由于实际场景中的平面是具有边界或边缘线的，我们假设：可由空间平面的每条边缘线导出一个假定的“假设平面”。在假设平面的约束下，黑塞法线形式表达的无限空间平面具有了确定的范围。空间平面的交线（或边界线）Ll可以用交线上的一个空间点Xl和交线的方向向量nl表示为：

图3 是对全局地图中空间平面和假设平面的可视化展示，可以看到假设平面引入了大量额外的观测平面，对后续位姿估计具有积极意义。

图3 全局地图中的空间平面和假设平面Fig.3 The space planes and supposed planes in global map

2.2.3 空间平面的关联

本文使用一种具有一定鲁棒性的参数平面关联方法，其策略是：在全局地图中寻找与观测平面距离最近的相交空间平面，而后计算它们之间的夹角，如果夹角小于设定的阈值则将两空间平面判定为关联平面；如果观测平面未能关联任何全局地图中的空间平面，则将其作为新的空间平面添加到全局地图中。

2.3 物体特征

2.3.1 物体特征提取

本文使用主流的深度学习目标检测算法PP-YOLO 训练微软MS-COCO 数据集，检测出80 个类别的物体。在物体特征提取阶段仅需要粗略的物体模型参数（这些参数在SLAM 过程中将不断被优化），故本文对检测算法设定较低的阈值，即为0.25。

2.3.2 物体特征的参数化

使用轻量型的Cuboid 模型表示物体。Cuboid 的参数化表达由(R(θ),t,d)9 个参数构成，其中，t=(tx,t y,tz)为Cuboid 到相机中心O的平移向量，R(θ)=(θr,θp,θy)∈ℝ3为Cuboid 相对相机参考系的旋转，d=(d x,d y,dz)为Cuboid 三个维度的尺寸。图4为全局地图中的参数化Cuboid 模型，其能够完整展现出场景中物体的空间位置和6DoF 姿态。

图4 全局地图中的参数化Cuboid 模型Fig.4 The parameterized Cuboid model in global map

2.3.3 物体特征的关联

本文使用基于非参数统计检验（Non-parametric Statistical Testing）的物体特征关联方法[14]，其策略是：如果点云P和点云Q是来自同一物体的点云，那么它们应该服从相同的几何分布。本文先令假设成立，再使用秩和检验判定是否可以接受原假设。具体步骤为：

首先，连接点云P和点云Q，得到点云X=[P|Q]。而后，对点云X的三个维度分别排序，并定义中间变量WP∈ℝ3×1和WQ∈ℝ3×1：

式中，R(·) 为排序后数据点xk的位次；| · |为计算点云的长度。

最后，定义曼-惠特尼统计量W=min(WP,WQ)，此统计量服从渐进高斯分布。构造W的目的是将非高斯分布的点云转化为服从高斯分布的统计量，即其均值和方差分别为：

如果W满足如下关系，意味着点云P和点云Q来自同一物体：

3 多观测约束的相机位姿估计

基于包含点、线、平面路标和相机位姿的SLAM因子图，求解相机位姿T cw，即最小化式(8)所示的非线性代价函数：

式中，fp，fl，fs，fsp，fso分别为“相机-点”因子项、“相机-线”因子项、“相机-平面”因子项、平行关系“平面-平面”因子项及垂直关系“平面-平面”因子项，其解算过程可参考文献[8]；ρ（·）为Huber 核函数，表示为：

式中，f为式(8)已定义的因子项；δ为固定阈值。

对于式(8)的求解，本文首先采用g2o 库构建因子图，而后选择Levenberg-Marquardt 算法作为梯度下降策略。为满足提升优化效率的进阶需求，以下详细推导各因子项雅可比矩阵的解析形式。

3.1 点重投影误差项

定义点特征重投影误差为ep，相机位姿及其扰动量分别为ξ和δξ，利用李代数扰动模型，推导ep关于δξ的雅可比矩阵：

式中，⊕为李代数加法，^为反对称运算符；ξcw为世界坐标系下的相机位姿；fx和fy为相机的焦距；Xw=(x,y,z)T为三维地图点，Xc=（x c,y c,zc）T为Xw在相机坐标系下的归一化坐标。

3.2 线重投影误差项

类似地，定义线特征重投影误差为el，推导el关于δξ的雅可比矩阵。本文以起始点作为中间变量，实现推导（起始或终点作为中间变量，解析形式的雅可比矩阵相同）：

3.3 平面观测误差项

定义平面观测误差为es，推导es关于δξ的解析雅可比矩阵，有：

式中，π w为空间平面；π c=T cwπ w为πw在相机坐标系下的空间平面；nc=(n cx,n cy,ncz)T为相机坐标系下空间平面单位法向量。

对于平行平面间的观测误差esp和垂直平面间的观测误差eso，仅需计算它们关于李代数形式的相机旋转扰动量δφ的雅可比矩阵，且它们的雅可比矩阵具有相同形式。以esp关于δφ的雅克比矩阵推导为例，有：

式中，φ cw为世界坐标系下的相机姿态；n w为世界坐标系下空间平面单位法向量。

4 方案实施的技术路线

在完成了集成点-线-平面-物体特征基元的RGB-D SLAM 系统框架设计之后，最佳的线特征的合理配置需通过大量数据集实验对线特征提取数量进行确定。为直观展现线特征优化配置的思想，图5 给出了方案实施的技术路线图。

图5 方案实施的技术路线图Fig.5 Technical roadmap for scheme implementation

首先进行数据处理，即在完成对原有ORB-SLAM2 框架的多个模块中添加点、线、面、物体特征的提取、匹配、参数化、优化及可视化等算法的基础上，实现对所有视觉特征的跟踪。其次分别开展用于线特征配置的数据集实验及用于位姿优化与评估的主流方法数据集对比实验。两个实验均在公开数据集ICL-NUIM RGB-D（计算机合成场景）和TUM RGB-D（真实场景）中开展，线优化配置旨在通过大量实验迭代调整线特征数量，并以在不同图像序列上最小的APE 均方根误差（Root Mean Square Error，RMSE）确定出最优的线特征提取数量。最后基于所获得的优化线特征约束，求取ICL-NUIM、TUM 中结构化场景下的位姿，并最终通过主流SLAM 方法间的数据集对比实验来实现位姿评估。

5 实验及分析

本文实验环境基于 AMD Ryzen5 3600 CPU @3.6GHz×6，未使用GPU 加速，所采用的操作系统为Ubuntu18.04LTS。使用的开源软件库包括：开源计算机视觉库OpenCV3.4.0、矩阵运算库Eigen3.2.10、图优化库g2o、可视化库Pangolin 及词袋模型库DboW2。

选取三种主流SLAM 系统，即ORB-SLAM2、点-线融合的PL-SLAM 及点-面融合的SP-SLAM，开展了数据集对比实验。APE 可以反映SLAM 过程的全局一致性，包含绝对轨迹误差（Absolute Trajectory Error，ATE）和绝对旋转误差（Absolute Rotation Error，ARE）。针对所选取的每一个子序列，经大量实验，本文通过在ICL-NUIM、TUM 数据集上设置不同线特征提取数量来求出的完整APE RMSE 结果（5 次实验的平均值），如表1-2 所示，表中数字8、16、24 等即为所提取的线特征数量。

以表1 为例，对于lr_kt0子序列，当线特征数量≥16 时，相机跟踪失败，SLAM 系统无法工作。而对于lr_kt1子序列，线特征数量＞48 时，APE 不断加大，表明数量更多且低质量的线特征对于SLAM系统的相机位姿估计几乎没有正面作用：一方面使位姿估计精度有所下降；另一方面对无用线段的提取与匹配将耗费更多的系统资源及运行时间。表中以加粗的数字对所有子序列中最小的ATE RMSE 和ARE RMSE 结果进行标注，这些标注为后续对比实验中优化线特征约束提供了重要依据。

表1 所开发的系统在ICL-NUIM 数据集上设置不同线特征提取数量时的完整APE RMSETab.1 The complete APE RMSE of developed system under different line feature extraction numbers on ICL-NUIM dataset

表2 所开发的系统在TUM 数据集上设置不同线特征提取数量时的完整APE RMSETab.2 The complete APE RMSE of developed system under different line feature extraction numbers on TUM dataset

与ORB-SLAM2、PL-SLAM 和SP-SLAM 算法的对比实验结果如表3 所示。不难发现，ORB-SLAM2几乎在所有子序列上均获得了最差的位姿估计精度；相比之下，在那些场景中包含大量高质量结构直线的子序列上（如lr_kt1、lr_kt3和of_kt3），融合额外线特征提取机制的PL-SLAM 表现出更优的性能（但仍存在of_kt1上跟踪失败的情况）。SP-SLAM 在ORB-SLAM2基础上集成了平面特征，由于ICL-NUIM数据集的8 个子序列均包含大量的理想（平行或垂直）平面结构，因此，其在几乎所有子序列上都具有超越ORB-SLAM2 的定位精度。

表3 不同算法在ICL-NUIM 和TUM 数据集上的APE RMSE 对比Tab.3 The APE RMSE comparisons under different frameworks on ICL-NUIM and TUM datasets

本文开发的系统在半数以上的子序列上都表现出最佳的定位性能。与先进的SP-SLAM 系统相比，系统在ICL-NUIM 数据集8 个子序列上的平均轨迹估计精度提升了4.33%，在TUM 数据集4 个子序列上的平均轨迹估计精度提升了21.40%，证明点-线-平面特征的相互补充有利于SLAM 系统全局定位精度的提升。

需要强调的是，TUM 数据集中的fr3/cabinet及fr3/large_cabinet为典型的弱纹理序列，图像上的物体表面特征信息较少，采集过程中相机环绕结构物体运动。在这种极端情形下，ORB-SLAM2 系统几乎不能稳定工作（如表3 所示的跟踪失败），而本文开发的系统不仅可以稳定工作，同时具有超越SP-SLAM 系统的最佳定位性能。

图6 给出本文开发的系统（图中用Ours 表示）与SP-SLAM 系统在fr3/large_cabinet子序列上的导航误差统计特性对比图。从图6(a)中可以看出，前者的ATE整体波动较小，特别是在横轴刻度为25 左右范围内，此时相机处于大角度的旋转过程，后者的ATE 出现明显升高，即SP-SLAM 系统产生较大轨迹误差，而前者ATE 未见明显升高。图6(b)为两种系统的ATE 统计直方图，不难发现，相比于后者，前者的ATE 统计直方图更接近高斯分布，轨迹误差的均值和方差相对较小。从图6(c)的全部相机位姿ATE 箱形图也可以看出，前者的ATE 上限值、下限值和中位数均小于后者，且异常值和超限量都较小，表明前者的平均轨迹误差和波动程度更小，即系统的定位精度和鲁棒性更高。

图6 所开发系统与SP-SLAM 的导航误差统计特征对比图Fig.6 The comparative diagrams of ours and SP-SLAM system on statistical properties of navigation errors

6 总结与展望

本文构建了一类新颖的室内RGB-D SLAM 系统框架，利用丰富的线、面及物体特征，优化VO、后端和建图线程。通过ICL-NUIM 和TUM 基准数据集实验，确定最优的线特征提取数量，提升系统在弱纹理区域的定位鲁棒性，并增强结果的可解释性。分别与ORB-SLAM2、PL-SLAM 及SP-SLAM 等主流系统构成线-平面、平面、线段特征基元”消去”的消融实验，验证了本文开发的系统可取得最优的全局定位结果。

本文通过紧耦合方式估计相机位姿，后续将研究无旋转漂移的“解耦估计”方法。此外，考虑将平面间的垂直和平行关系建模推广至更多结构关系建模，以满足不同场景需求。