室内场景下基于曼哈顿约束的多重特征视觉SLAM 方法

孙新柱，龚光强，陈孟元，程 浩，郭行荣

（1.安徽工程大学 电气工程学院，芜湖 241000；2.高端装备先进感知与智能控制教育部重点实验室，芜湖 241000）

同步定位与地图构建（Simultaneous Localization and Mapping,SLAM）是估计智能机器人旋转和平移六自由度运动、同时构建周围未知环境地图的技术。视觉SLAM（Visual SLAM,V-SLAM）是移动机器人领域研究的热点和难点。V-SLAM 算法被提出以来，纹理稀疏环境下的同步定位与建图一直是研究的热点[1]。为解决V-SLAM因点特征不足导致定位精度低的问题，除了融合其它传感器外，场景中的其它特征逐渐被研究者关注，例如线特征、平面特征和语义特征。现代建筑的室内环境普遍表现出特殊的规律性，具有统一的场景结构模式。室内的结构化场景表现出具有较多平面、纹理信息不丰富或区分度不明显的特点。在VSLAM 技术中，面是比点和线拥有更多约束的几何特征，室内的地板、墙面和天花板相互垂直或平行，导致平面特征的法线矢量集中在三维空间中的三个主要正交方向，具有这种结构特点的场景可以被抽象为曼哈顿世界（Manhattan Word,MW）[2]，沿三个主要正交方向定义的坐标系称为曼哈顿坐标系（Manhattan Frame,MF）。对具有MW 结构特点的环境，可以将其形式化为MW 假设模型，目前该方法已应用于多种领域，例如相机校准、三维场景重建和SLAM 技术[3]。

目前，室内环境下的定位与导航是V-SLAM 研究的重点，早期基于优化的V-SLAM大多仅利用点特征，早在2015 年，ORB（Oriented FAST and Rotated BRIEF）点特征[4]就已经被用于单目定位算法。后续ORBSLAM2[5]增加了对双目相机和RGB-D 相机的支持，可以适用于更多场景；ORB-SLAM3[6]是将相机与惯性测量装置（Inertial measurement unit,IMU）融合，使精度、鲁棒性和实时性都有了显著提高，成为了SLAM领域中广泛使用的算法之一。但在纹理稀疏的室内结构化场景下，仅依赖点特征的SLAM 方法易造成跟踪丢失；而无需借助其他传感器，通过加入线特征进行跟踪，这样可以弥补点特征的退化问题[7]。2019 年，Gomez-Ojeda 等人开源了双目PL-SLAM[8]，采用快速直线检测算法[9]（Line Segment Detector,LSD），融合了点和线特征以提高地图的准确性和鲁棒性，并且使用点线特征的描述子构建词袋用于闭环检测，建立了包含点和线的地图，提高了V-SLAM 在点特征稀疏或图像特征分布不均场景的定位精度。但是该系统简单地将点和线特征检测结果相加，会将线特征匹配的高误差引入系统中，从而降低数据关联的准确性。

近些年，平面特征被广泛应用于SLAM 算法。王雅仪等人[10]通过激光雷达传感器直接获得点云信息，对原始点云进行聚类分割，再从点云中提取面特征。为了将MW 中涉及的结构特征融合到SLAM 中，孙沁璇等人[11]针对利用平面特征位姿求解的退化问题，提出平面与平面、平面与直线的多特征关联方法，并且使用自适应加权方法保证了平面在位姿计算的主导作用；2018 年，Kim 等人提出了一种结合线和平面的视觉里程计LPVO[12]，将线和平面结合起来，利用均值偏移算法跟踪MF 估计旋转运动，这使SLAM 系统的位姿解耦估计成为可能。2019 年，Yang S 等人[13]将面特征加入统一的SLAM 框架进行优化，联合推断三维对象和布局平面。2020 年，Li 等人提出了一种用于室内场景的Structure-SLAM[14]，利用卷积神经网络从RGB 图像中估计平面法向量，并用当前帧的平面特征和线特征计算出MW 矩阵，从而估算出当前姿态；Structure-SLAM 的优点是能够利用平面特征提高场景重建的精度和鲁棒性，同时能够实现实时性能和轻量级计算。2021 年，Li 等人的团队开源了基于RGB-D的Planar-SLAM[15]，提出了一种基于多重特征的解耦细化方法，通过联合平面上的点、线特征和基于曼哈顿约束的位姿优化过程来提升定位准确性。除了通过解耦策略估计旋转和平移之外，Planar-SLAM 也采用平行或垂直的结构规律约束姿态。但解耦方案需要对每一帧进行MW 假设，因此在不严格符合MW 假设的室内环境中是非常有限的，导致性能下降甚至跟踪失败。为了克服这个问题，2021 年，Yunus 等人提出基于 RGB-D 融合点、线和面特征的 Manhattan-SLAM[16]，该算法从线和面特征提取MW 矩阵，建立基于MW 假设的室内场景模型，并采用MF 跟踪和点线特征跟踪两种策略，取得了较高的精度和鲁棒性；但所用的聚集层次聚类点云分割算法[17]受深度信息噪声影响较大，会导致MF 提取出现误差。

综上所述，在纹理稀疏场景下，平面特征较点、线特征能表现出更强的稳定性。将面特征融入VSLAM，不仅能改善特征退化，还可以通过平面关系确定MF，利用MF 估计相机旋转以减少累积误差，提高机器人在室内纹理稀疏场景中的定位精度。

1 系统总体框架

针对传统RGB-D SLAM 在室内纹理稀疏场景中易出现定位精度差的问题，提出一种基于曼哈顿约束的点线面多特征V-SLAM 方法。在特征处理阶段，针对传统面特征提取算法存在平面漏检测和错分割的问题，提出特征增强模块对PlaneRecNet 网络[18]进行改进，实现更准确的图像面特征和参数提取。在位姿跟踪阶段，采用方向引导的方法筛选正交主平面，提高MF 估计的准确性；针对单纯利用MW 假设进行相机位姿估计的应用场景受限问题，检查平面间的正交关系，分别采用曼哈顿跟踪和点线特征跟踪两种策略。对于MW 场景，利用MW 假设结合球面均值偏移算法获得旋转矩阵，降低SLAM 过程因旋转累积误差带来的轨迹漂移；再固定旋转，构建点、线重投影误差函数求解三自由度的平移运动，系统结构框架如图1所示。

图1 系统结构框图Fig.1 System architecture block diagram

2 平面处理

传统平面特征提取采用图像中的几何形状，将特征聚类分组以检测平面，或从3D 点云中提取平面[19]。但是在镜像表面和非平面中很难检测其几何性质，在SLAM 中实际效果不佳。鉴于卷积神经网络在目标检测和语义分割领域的成功应用，本文采用改进的PlaneRecNet 网络来提高结构化场景下平面特征提取的精度。PlaneRecNet 是一种具有单阶段实例分割分支的多任务卷积神经网络，具有平面优先注意的深度解码器和用于分段平面区域估计与重建的共享主干，通过引入深度梯度分割损失函数和平面表面法线损失函数来加强分段平面分割。特征金字塔网络[20]（Feature Pyramid Network,FPN）作为大多数物体检测器的基本组成部分，可以有效提取多尺度特征。FPN 在进行特征融合之前，不同层次的特征会独立进行1×1 卷积，将特征通道维数减少至最低常数（256），而骨干网络在每个时间段提取的原始特征往往包含比这个常数更丰富的信息（如512、1024、2048）。这种横向连接直接将通道维数降低了几倍，损失了部分特征图的语义信息。由于高层特征包含更多通道，因此通道减少带来的信息损失在高层特征中最为严重。针对PlaneRecNet 在纹理稀疏且相似的环境中因信息丢失造成的平面漏检测和错分割问题，提出了特征增强模块（Feature Enhancement Module,FEM），如图2 所示。

图2 特征增强模块Fig.2 Feature enhancement module

FEM 利用具有更大感受野的C5层上下文信息弥补特征金字塔顶层横向连接的信息丢失，有助于在纹理稀疏场景提取更丰富的特征。FEM 的关键思想是融合大量局部和全局上下文信息以生成更具辨别力的特征。原始图像在ResNet101 骨干网络中通过一系列下采样得到的特征图有{C2,C3,C4,C5}。首先，FEM 对来自ResNet101 最高层C5的信息行执行三个不同尺度的池化与卷积操作：1）在C5上应用3×3 卷积来提取局部信息；2）输入特征通过3×3 最大池化下采样到w×h，并经过1×1 卷积层以扩展通道维度；3）对C5进行全局平均池化以获取通道关联信息。通过扩展三个尺度的特征表示，有效扩大了C5的感受野，并细化了表示能力。然后，将三个具有不同比例的上下文特征Fi(1≤i≤ 3)通过双线性插值上采样到C5的尺度，以进行后续concat 融合得到。最后，经过卷积并进行逐元素求和得到Mi，Mi与融合进行张量对齐得到，再将得到的添加到FPN。

其中，σ为sigmoid 函数；w1和w2是卷积层参数。

3 基于曼哈顿约束的位姿解耦估计

ORB 算法和LSD 算法是计算机视觉领域中常用的特征提取算法，分别用于对场景中的点特征和线特征进行提取。使用LBD（Line Band Descriptor）描述符将线特征参数化表示。在提取图像帧的二维特征后，点特征二维坐标用p表示，得到归一化线函数l记为：

符合MW 假设要求场景中同时具有三个相互正交的平面，但实际上两个正交的平面也可满足MW 假设。如果平面角差满足正交阈值（至少86°），则认为由两个平面确定的主导方向是正交的，第三个方向可以通过二者的向量积得到，任何三个相互正交的主导方向都可构成MF。本文算法使用两种不同的策略估计相机位姿：对于非MW 场景，直接跟踪点线特征估计六自由度的相机位姿；对于MW 场景，首先比较跟踪平面结构的正交性进行三自由度旋转估计，然后构建点、线重投影误差函数求解三自由度平移运动。

3.1 方向引导的MF 估计

曼哈顿场景平面法向量主要集中在三个正交主方向上，但每帧被探测的平面一般位置和面积都不一样，除了正交优势面外，还有很多小平面和非正交面区域。这些小平面和非正交面区域也可能存在平行或垂直关系，这会导致MF 估计的结果出现误差。因此，如何在众多具有一致法线方向的平面中选取合适的平面，成为确定最优MF 的关键问题。在三维欧式空间R3中，平面法向量n在单位高斯球面上具有一定的规律性，高斯球是以相机光学中心为球心的虚拟单位球体，是在相机固有参数已知的情况下表示直线、平面法向量等几何元素的一种简便方法，如图3 所示。

图3 平面法向量在高斯球中的分布Fig.3 Distribution of plane normal vectors in a Gaussian sphere

图4 切平面上的均值偏移Fig.4 Mean shift in the tangent plane

图5 各坐标之间的转换Fig.5 Conversion between coordinates

对于满足MW 假设的室内场景，当前帧的平面集合可以表示为H={h1,h2…hi}，其中hi表示第i个平面（i2≥ ）。任意两个平面hj和hk之间的关系可以用它们之间的夹角θ表示，θ越接近90°，hj和hk越接近垂直。曼哈顿场景的大部分平面都相互垂直或平行，H中各平面的面积和方向关系可以表示为：

其中，S(hj)为H中提取平面的面积；F⊥(hk)可以表示为除hk之外H中其它平面在垂直于hk的平面上的投影面积之和。H中与hk近似垂直的平面数量越多，则面积越大，F⊥(hk)越大，反之F⊥(hk)值越小。本文算法结合场景中提取平面之间的方向关系和面积选取确定MF 的优势平面，采用具有最大F⊥(hk)值的平面来确定主导方向，如式(4)所示。计算H中所有平面的F⊥(hk)值，取F⊥(hk)最大的两个相互正交的平面作为主导平面，求二者向量积得到第三个主导方向。

3.2 跟踪曼哈顿帧

跟踪曼哈顿帧的核心是比较和跟踪平面结构的正交性。使用{Rcw,Tcw}表示六自由度的相机位姿，其中Rcw∊SO(3)和Tcw∊R3分别表示从世界坐标系到相机坐标系的旋转和平移。MF 相对于相机坐标系的旋转矩阵表示为Rcm=[rxryrz]∊SO(3)，其中rx、ry、rz分别表示MF 的x、y、z轴对应的方向向量。将当前相机坐标系的平面法向量n变换为在MF 的表示：

相机在曼哈顿场景中运动，相当于交替地将当前帧检测到的平面法向量投影到上一帧MF 的x、y和z轴对应的高斯球切平面上，用表示与MF 的x轴平行的平面相关法向量。

然后，使用具有高斯内核的均值偏移算法计算法向量在切平面上的均值偏移。均值偏移是一种通过迭代运算找到目标位置实现目标区域跟踪的统计迭代算法，如式(7)所示。

其中，c是内核的宽度，根据法向量在切平面分散的稀疏程度来确定。

最后，用黎曼指数映射进行归一化，将均值偏移从切平面变换回高斯球面上，得到更新后的法向量sx，并将sx相对于相机坐标系进行转换，获得相机坐标系中下一帧的方向向量，如式(8)所示。

其中，[∙]代表向量的归一化。

得到旋转矩阵之后就可以固定旋转，构建点、线重投影误差函数求解三自由度的平移运动，满足SLAM 系统在低纹理场景中稳定运行所需特征。平移运动估计的核心是将上一帧的三维点线重新投影到当前帧，定义相机测量的重投影误差函数如式(10)所示。

4 实验结果与分析

本文实验平台配置为：CPU 为Inter Core i5-12400F，6 核12 线程，主频3.2 GHz，内存16 GB；GPU 为RTX3060 显卡，显存12 GB；算法运行软件平台为Ubuntu20.04。为了验证本文算法有效性，面特征提取网络和定位精度测试在合成数据集ICL-NUIM 与真实世界数据集TUM RGB-D 上进行。ICL-NUIM 是合成数据集，可模拟在人工环境（客厅和办公室）捕获的RGB 和深度图像数据中的真实传感器噪声，包含缺少纹理的墙壁和天花板。TUM RGB-D 数据集是由慕尼黑工业大学制作的VO/V-SLAM 评估数据集，在室内环境中通过微软Kinect RGB-D 相机采集的图像组成，用运动捕捉系统收集相机的真实运动轨迹。

4.1 平面分割实验分析

在室内结构化场景中验证本文所提的改进算法平面分割效果，为满足本实验所需条件，选取ICL-NUIM和TUM 数据集的室内场景序列进行测试。PlaneRCNN、PlaneRecNet 网络改进前和改进后的分割效果对比如图6 所示。PlaneRCNN 算法基于两阶段分割方法和ROI 内的局部掩模分割，对小平面表现出更高的检测能力，但也会检测出很多假性小平面，如图6(b)所示。图6(f)中的门框被错误识别为平面（箭头所指），并且在海报上分割出了小平面。

图6 平面分割对比图Fig.6 Comparison of planar divisions

大尺寸平面的高质量分割可以显著提高室内V-SLAM 定位精度，PlaneRecNet 网络主要针对大平面区域进行分割提取，因此本文在PlaneRecNet 基础上进行改进，但纹理相似和目标区域的语义信息丢失会导致平面检测精度降低。PlaneRecNet 在ICL-NUIM 数据集office1 序列上存在严重错分割的情况，办公桌正面和部分地面被识别为同一平面，而地面没有被识别，如图 6(c)所示。PlaneRecNet 在 TUM 数据集fre3/s_t_near 序列场景上存在错分割的情况，墙面海报本身是同一平面，但在海报下方多分割出了一个平面（箭头所指黄色区域），同时还出现了过度分割，如图6(g)所示。通过PlaneRecNet 改进前后的分割效果对比可知，本文算法通过减少FPN 高层图像语义信息过度损失，改善了纹理稀疏的结构化场景中PlaneRecNet网络的分割效果，如图6(d)和图6(h)所示。

本文使用具有不同交并比（Intersection over Union,IoU）的平均精准率（Average Precision,AP）作为衡量指标，PlaneRCNN、PlaneRecNet 和PlaneRecNet+FEM三种算法的AP、AP50（IoU>50%）、AP75（IoU>75%）对比结果如表 1 所示。由表 1 数据可知，PlaneRecNet+FEM 的平均精准率比PlaneRCNN 提高了11.89%，与改进前的PlaneRecNet 相比提高了5.47%。

表1 三种算法AP 值对比Tab.1 Comparison of AP values for the three algorithms

4.2 合成数据集定位结果与分析

为验证算法的整体效果，使用ICL-NUIM 数据集进行评估。为比较ORB-SLAM3、Planar-SLAM、Manhattan-SLAM 与本文算法（图、表中用Our 表示）的误差大小，本文使用绝对轨迹和相对旋转的均方根误差（Root Mean Squared Error,RMSE）来衡量定位精度。绝对轨迹误差（Absolute Trajectory Error,ATE）适合评估SLAM 算法估计轨迹与相机真实运动轨迹的整体一致性，而相对旋转误差更适合评估轨迹漂移程度，因此通过比较各算法的绝对轨迹误差和相对旋转误差进一步验证。

不同算法在ICL-NUIM 数据集部分序列轨迹对比结果如图7 所示：其中黑色线条代表序列的真实轨迹，蓝色线条为算法估计的轨迹，红色线条是偏移误差。从图7(a)-(d)可以看出，lr-kt1 序列由于点特征缺失加上阳台光照影响导致ORB-SLAM3 与Planar-SLAM 误差较大；图7(e)-(h)中of-kt0 序列由于纹理稀疏加之弯道角域过大，对SLAM 系统提出了较大挑战，Planar-SLAM 仅利用结构规律性估计直线和平面上的旋转运动，面对室内场景存在局限性，导致精度较差。综合图7 可以看出，本文算法的相机运动轨迹与序列真实轨迹吻合度较高。

四种算法在ICL-NUIM 数据集所有序列上运行的绝对轨迹误差和相对旋转误差RMSE 结果，如表2 所示。ICL-NUIM 数据集包含大量缺少纹理的地板、天花板和墙壁，能提取到的点特征稀少，导致基于点特征的 ORB-SLAM3 精度较差；Planar-SLAM 和Manhattan-SLAM 均是将位姿估计解耦，在符合MW假设的情况下联合线、面特征估计旋转运动，其中Manhattan-SLAM 为了应对室内场景复杂性，加入了MW 场景和非MW 场景判断策略，但二者的面特征提取方法都是传统聚集层次聚类算法，并且主平面未经筛选，导致误差较大；本文采用深度学习的方法，在MW 场景中改善了面特征提取的数量和质量，通过方向引导的MF 估计策略选取场景优势平面来估计最优MF，从而提高了曼哈顿帧跟踪效果。在ICL-NUIM 数据集的测试结果如表 2 所示，本文算法相对于Manhattan-SLAM、Planar-SLAM 和ORB-SLAM3 的绝对轨迹误差 RMSE 分别减少了10.82%、20.46%和34.77%，相对旋转误差RMSE 分别减少了10.63%、15.11%和55.76%。

4.3 真实世界数据集定位结果与分析

本文算法面向室内环境，因此需在TUM RGB-D数据集上进行评估。在图8 中，对本文算法和其他三种算法估计的相机运动轨迹进行可视化，并与相机真实运动轨迹进行对比。对于具有丰富纹理的fr2/desk序列，本文算法性能与其他算法基本相当，如图8(a)所示，Planar-SLAM 虽然可以追踪整个序列，但其估计出的相机轨迹误差较大。在fr3/l_cabinet 等弱纹理序列中，ORB-SLAM3 初始阶段表现良好，但随着场景纹理变少，定位精度逐渐下降，在fr3/s_t_near 等序列上点特征提取失败，算法失效，无法跟踪完整序列，如图8(b)和图8(c)所示。

图8 部分TUM 序列估计的轨迹对比Fig.8 Comparison of trajectories estimated for partial TUM sequences

四种算法在TUM RGB-D 数据集fr2/desk 序列和部分纹理稀疏室内场景序列测试的绝对轨迹误差和相对旋转误差RMSE 结果，如表3 所示。ORB-SLAM3在所选取的大部分序列中误差较大，在fr3/s_nt_far 等挑战性较高的序列上出现了跟踪丢失的现象，无法获得完整运行轨迹。在纹理丰富且不满足MW 假设的fr2/desk 序列上，Planar-SLAM 仅依靠MW 假设进行位姿估计，导致旋转误差较大，定位精度反而下降；本文采用曼哈顿跟踪与点线特征跟踪两种策略，避免了算法对曼哈顿场景的过度依赖，使得定位精度高于Planar-SLAM，略低于ORB-SLAM3。fr3/cabinet 和fr3/l_cabinet 序列的结果表明，基于点特征的方法难以有效减少旋转估计带来的累计漂移，基于MW 假设的旋转估计可以有效减少旋转误差；而本文算法的相对旋转误差普遍低于其他三种算法。本文算法相对于Manhattan-SLAM、Planar-SLAM 和ORB-SLAM3 的绝对轨迹误差RMSE 分别减少了15.92%、31.12%和43.23%，相对旋转误差RMSE 分别减少了16.44%、29.53%和32.89%。

表3 各算法在TUM RGB-D 数据集轨迹误差对比Tab.3 Comparison of trajectory errors of various algorithms with the TUM RGB-D dataset

4.4 真实场景实验

为了在真实环境中全面验证本文方法，使用固定在地面机器人上的RealSense D435i 摄像头采集了实验场景数据集并且进行算法测试。场景一为含有较多小平面和非正交面，且符合MW 假设的室内实验室，规模为7.8 m×7.2 m，如图10(a)所示。在场景一的平面布局图中，A 和B 位置分别指示机器人的开始和终止，途经1、2 和3 位置的折线路径用于衡量算法运行轨迹的漂移。场景二为纹理稀疏的走廊和楼梯间，用走廊到楼梯间的蓝色路径衡量算法运行轨迹，走廊部分长8 m，楼梯间部分长4 m，如图10(b)所示。

图10 真实场景设置Fig.10 Real scene settings

在场景一中，ORB-SLAM3 由于特征稀疏和弯道角域过大出现了跟踪丢失的现象，导致轨迹不完整。本文算法与Manhattan-SLAM 的定位轨迹对比，如图11(a)所示：Manhattan-SLAM 的MF 受到小平面和非正交面的干扰，导致轨迹漂移较大；而本文算法利用方向引导的方法进行主平面筛选，减少了MF 估计的误差，利用曼哈顿约束实现了更精确的旋转估计。在场景二中，ORB-SLAM3 由于点特征过少导致跟踪丢失。本文算法与Manhattan-SLAM 的定位轨迹对比，如图11(b)所示：由于稀疏且相似的纹理会影响面特征提取的准确性，导致Manhattan-SLAM 系统定位精度下降；而本文算法因为改进了面特征的提取方式，一定程度上避免了面特征提取的错误，使算法在室内弱纹理场景有更高的定位精度。真实场景实验的绝对轨迹误差RMSE 对比如表4 所示：本文算法在场景一中误差比Manhattan-SLAM 降低了26.58%，在场景二中误差比Manhattan-SLAM 降低了43.22%。

表4 真实场景绝对轨迹误差RMSE 对比（单位：米）Tab.4 Comparison of absolute trajectory error RMSE in real scenes (Unit: m)

图11 真实场景轨迹对比图Fig.11 Trajectory comparison of real scene

5 总结

本文针对传统V-SLAM在纹理稀疏的室内环境下出现的定位精度不高甚至失效的问题，提出了一种鲁棒的V-SLAM 系统，该系统充分利用室内场景的面特征结构规律实现了更为精确的旋转估计。针对传统面特征提取算法存在的平面漏检测和错分割的问题，使用特征增强模块改进了PlaneRecNet 网络，使面特征及参数提取更精确。针对单纯利用曼哈顿假设的应用场景受限以及MF 估计不准确的问题，通过方向引导的方法进行主导平面筛选，减少了小平面和非平面噪声对MF 估计的影响，分别采用曼哈顿跟踪与点线特征跟踪两种策略以降低对曼哈顿场景的依赖。公开数据集上的实验结果表明，本文算法的平均绝对轨迹误差比Manhattan-SLAM 减少了13.37%，相较于ORBSLAM3 减少了39%。然而，深度学习网络使算法整体实时性受到限制，面特征的提取占用了大部分时间。在接下来的研究中，需要进一步优化算法，提高系统时效性。曼哈顿约束目前的限制在于要求当前帧有两个及以上正交的主方向，后续可以利用线特征的方向或加入IMU 信息作为主方向来构建MW 假设，并可能将其扩展到室外环境。