视觉SLAM论文学习：融合点线面特征的UPLP-SLAM（精简版）

UPLP-SLAM: Unified point-line-plane feature fusion for RGB-D visual SLAM

统一点-线-面特征融合的RGB-D 视觉 SLAM

YANG H, YUAN J, GAO Y, et al. UPLP-SLAM: Unified point-line-plane feature fusion for RGB-D visual SLAM[J/OL]. Information Fusion, 2023, 96(1566-2535): 51-65. DOI:Redirecting.

今天给大家分享一篇2023年发表在一区期刊《INFORMATION FUSION》的视觉SLAM文章，论文不包含创新点理论的讲解。

关键词：RGB-D相机、结构化环境、SP模型（对称扰动模型）、视觉SLAM、特征融合、线特征、面特征、数据关联

本文介绍简介、相关工作、系统框架、实验、总结部分。跳过论文核心部分（在论文中是第三部分）：

本文其他讲解链接：（2）部分讲了论文的理论详细解读

视觉SLAM论文学习：融合点线面特征的UPLP-SLAM（1） - 知乎

视觉SLAM论文学习：融合点线面特征的UPLP-SLAM（2） - 知乎

---

1.简介/相关工作

1.1视觉SLAM分类

①前端——从特征提取和跟踪的角度，视觉SLAM分为基于特征的方法和直接法：

• 基于特征的方法：从图像中提取特征，通过特征匹配建立帧间对应关系，通过最小化特征匹配误差计算相机位姿，常用的特征包括点、线和平面。特征描述子可以稳健地描述特征，所以基于特征的方法可实现更准确的数据关联和位姿估计，用于长期跟踪。

• 直接法：主要提取光度测量信息，通过最小化光度测量误差估计相机位姿，比如 LSD-SLAM 、SVO 、DSO 和 LDSO 。由于光度信息的描述能力较低，直接方法容易出现像素或像素块的错误对应。

本论文基于点、线、面三种几何特征，属于基于特征的方法

②后端——从状态估计角度，视觉SLAM分为基于滤波和基于优化的框架：

• 基于滤波器的方法：将相机位姿和特征状态表示为系统的状态向量，该状态向量由滤波器递归估计，例如卡尔曼滤波器和粒子滤波器。如果要估计的特征数量太大，则基于滤波器的方法的实时性较低。

• 基于优化的方法：通常使用关键帧和相机位姿作为节点，用观测关系作为边来构建位姿图，以优化相机位姿和特征状态。它经常维护和更新节点之间的观测关系，从而通常获得更准确的估计结果。

本论文基于ORB-SLAM2，属于基于优化的框架

1.2 RGB-D SLAM和点特征

SLAM的视觉传感器主要包括单目、双目和RGB-D相机。和其他相机比，使用RGB-D相机可直接测量每个像素的深度信息（利用结构光技术获得深度），在环境中获得更准确的结构信息和几何特征。

除了这三种相机外，近年在视觉SLAM研究比较多的还有一种相机——事件相机（event camera），在动态场景中高度适用。 可以像IMU的使用方式一样，搭配其他三种相机混用，让SLAM在动态场景下发挥更好的性能。

具有代表性的点特征RGB-D SLAM：

• RGBD-SLAM——从彩色图像中提取关键点，用深度图像计算关键点的3D位置。随后采用RANSAC（随机样本一致性）方法估计相机运动并重建3D环境图。在图优化模块中，仅对相机位姿进行了优化，不涉及三维点的坐标。

• ORB-SLAM系列——比较流行的使用点特征的视觉SLAM。ORB-SLAM2将仅使用单目的ORB-SLAM扩展到双目和RGB-D相机，并通过特征跟踪、局部建图和全局BA实现了高定位精度。ORB-SLAM3在ORB-SLAM2和ORB-SLAM-VI（带惯性测量单元IMU的ORB-SLAM）的基础上构建，支持单目、双目、RGB-D和IMU。

• ORB-TEDM [24]——计算了ORB-SLAM2的不确定性，并通过将ORB点特征的三角测量估计与深度测量融合，实现更准确的RGB-D视觉SLAM。

[24]Jing Yuan, Shuhao Zhu, Kaitao Tang, Qinxuan Sun, ORB-TEDM: an RGB-D SLAM approach fusing ORB triangulation estimates and depth measurements, IEEE Trans. Instrum. Meas. 71 (2022) 1–15.

使用点特征的视觉SLAM缺点：容易受低纹理和相机快速移动的影响。在一些低纹理环境中只能提取少量点特征。在重复纹理或相机快速移动的情况下，经常发生特征匹配失败和特征跟踪丢失的情况。点特征不能完全描述环境的结构信息。

1.3 结构化环境中的RGB-D SLAM

结构化环境的边缘、线条和平面等高级几何特征丰富。与点特征相比，更高层次的几何特征对环境纹理和照明的变化更具鲁棒性，具有足够的结构和轮廓信息。

① 仅使用线特征或面特征的RGB-D SLAM：

• Edge-SLAM——利用边缘结构，通过正反向光流跟踪和三个视图之间的几何关系进行特征关联，实现了在低纹理环境中对边缘点的稳定跟踪。在纹理丰富的环境中，与ORB-SLAM的系统相比，它只考虑了边缘点，导致精度较低。

线条（line）和边缘（edge）是两个相关但不同的概念，但在图像（至少在SLAM）中，可以认为边缘和线条是一个东西， 因为他们大多数都是通过边缘检测算法得到的。 具体概念中，线条通常是由相邻像素点连接而成的路径，而边缘是图像中灰度变化显著的地方。线条是边缘的一种。

• CPA-SLAM——使用平面特征，相机位姿估计通过平面跟踪来实现。当环境中平面特征很少时，系统会退化。

• 本文作者之前的工作（STING-SLAM） [27]——使用统计信息网格提取平面特征，并通过平面关联图进行特征关联。针对退化情况，设计了一种平面参数空间中的扫描匹配方法，为相机位姿估计提供额外的约束信息。

[27]Qinxuan Sun, Jing Yuan, Xuebo Zhang, Fengchi Sun, RGB-D SLAM in indoor environments with STING-based plane feature extraction, IEEE/ASME Trans. Mechatronics 23 (3) (2017) 1071–1082.

② 融合多种几何特征的RGB-D SLAM：

• PinpointSLAM——融合点、面的RGB-D SLAM，使用2D和3D测量值（像素信息和深度信息），使用2D点特征形成的射线来定位3D点特征，平面特征被用于该系统中的相机位姿估计和地图构建。

• PL-SLAM——点、线融合的RGB-D SLAM，将线特征加入ORB-SLAM，在后端优化中使用协方差矩阵对点线误差进行加权。

• ManhattanSLAM[33]和[34]——同时考虑点、线、面特征。由于不涉及三种异类特征之间的约束关系，很难为位姿估计提供更充分的约束信息。此外，[33，34]中没有引入全局优化，这导致在具有挑战性的低纹理环境中定位精度较低。

[33] Raza Yunus, Yanyan Li, Federico Tombari, Manhattanslam: robust planar tracking and mapping leveraging mixture of manhattan frames, in: 2021 IEEE International Conference on Robotics and Automation (ICRA), IEEE, 2021, pp. 6687–6693, pages. [34] Yanyan Li, Raza Yunus, Nikolas Brasch, Nassir Navab, Federico Tombari, RGB-D SLAM with structural regularities, in: 2021 IEEE International Conference on Robotics and Automation (ICRA), IEEE, 2021, pp. 11581–11587, pages.

1.4 SP模型在状态滤波器的使用

统一表示和处理特征的唯一方法是对称和扰动模型（symmetry and perturbation model，SP模型）[35-37]，以往的SP模型使用扩展卡尔曼滤波器（EKF）来估计机器人位姿和特征参数。

然而，基于过滤器的框架通常假设高斯分布和马尔可夫性质，甚至需要状态变量之间的完全独立性，这在现实世界的环境中可能不成立。普遍认为，基于滤波器的框架不如基于优化的框架准确和稳健，尤其是当要融合和估计相关状态时，基于滤波器框架很可能产生不一致的估计结果。对于点、线和平面特征的融合，点的测量噪声传播到线和平面，这很容易引起不同特征的观测值之间的相关性。

• [35]中第一次提出了SP模型，它不是一个完整的SLAM系统。

• [36]和[37]中的工作仅使用2D笛卡尔平面中的SP模型，并且仅涉及点和线特征。（这两个工作用的传感器是激光雷达）

• 从特征处理的角度来看，[35-37]中独立地关联和估计了不同类型的特征，没有使用特征之间的联系。

[35] Juan D Tardos, ´ Representing partial and uncertain sensorial information using the theory of symmetries, ICRA (1992) 1799–1804, pages. [36] Jose A Castellanos, Jos´e MM Montiel, Jos´e Neira, Juan D Tardos, ´ The SPmap: a probabilistic framework for simultaneous localization and map building, IEEE Trans. Robot. Autom. 15 (5) (1999) 948–952. [37] Jos´e Neira, Juan D Tardos, ´ Joachim Horn, Günther Schmidt, Fusing range and intensity images for mobile robot localization, IEEE Trans. Robot. Autom. 15 (1) (1999) 76–84.

1.5 贡献

本文提出一种点线面融合 RGB-D 视觉 SLAM 系统，充分利用点、线、面三种几何特征，实现机器人在结构化环境中的导航。

• UPLP-SLAM通过ORB提取点特征，通过RANSAC方法提取线和平面特征。使用SP模型表示得到的三种几何特征。

• 为点、线和平面特征的数据关联设计了一种方案，不仅包括同类特征（即点-点、线-线 和 面-面）的对应关系，还包括异类特征（即点-线、点-面和线-面）的关联，实现不同特征的相互增强。

• 在位姿估计和图优化中，通过定义一种误差函数对三种特征进行联合优化，从而实现精确、紧凑的多特征融合 RGB-D 视觉 SLAM。本文的主要贡献：

1. 提出一种多特征紧耦合联合优化框架，以统一的方式表示点特征、线特征、平面特征和相机位姿，并构造误差函数来联合优化点-线-平面几何特征参数和相机位姿。UPLP-SLAM第一个在基于优化的框架中使用SP模型。

2. 提出一种点-线-面多特征相互关联方法，不仅考虑同类特征关联，还考虑了异类特征关联。在联合优化中引入关联误差，包括同类特征之间的匹配误差和异类特征之间的对应误差。

所提出的框架包含两层，从系统框架的角度看，SLAM 的所有主要组件，即特征表示、特征关联和误差函数统一处理，提高了多特征 SLAM 系统的精度和紧凑性。从特征处理的角度来看，同类特征和异类特征都被统一使用，为位姿估计提供了更多的空间约束。

---

2.系统框架

UPLP-SLAM在 ORB-SLAM2 的基础上设计：

①输入：使用 RGB-D 相机获取彩色图像和深度图像。

②跟踪线程中，从当前帧的彩色图像中提取 ORB 点特征，通过 RANSAC 方法从相机获取的三维点云中提取线和面特征。使用 SP 模型统一表示提取的点、线和面。

③在跟踪和局部建图线程中，设计了一种点-线-面相互关联方法，以关联帧间的多种几何特征。UPLP-SLAM 中的特征关联包括两种情况，即同类特征关联和异类特征关联。

④在位姿估计中构建一个新的误差函数，通过连续帧之间的特征关联关系估计当前帧中的相机位姿。

⑤插入关键帧：可避免计算复杂度的过度增长，UPLP-SLAM 中创建新关键帧的决策与 ORB-SLAM2类似，除了点特征的数量外，在当前帧中跟踪的线和平面特征的数量也被视为关键帧决策的标准。此外，在计算当前帧和前一关键帧之间关联的地图特征的重叠率时，考虑了线特征或平面特征之间的相似性。

⑥局部建图：地图中的特征首先被剔除，然后根据当前关键帧在地图中生成新的特征。地图不仅包含点特征，还包含线特征和面特征，它们构成了环境的多层次表示。然后通过关联关键帧和局部地图之间的特征，构建新的误差函数进行局部 BA ，以估计相机位姿和特征参数。

⑦回环闭合：进行回环检测和全局 BA 优化，并添加来自线和平面特征的约束条件。

框架部分总结：在ORB-SLAM2的基础上添加了线面特征，使用SP模型统一表示三种几何特征。涉及误差函数的两个部分（位姿估计PnP和图优化BA）添加了线面特征的误差，在插入关键帧、局部建图、回环闭合中增加了线面相关的部分。

---

4.实验

1. 在公共数据集上，将UPLP-SLAM与现有的SLAM方法进行了比较。

2. 分析了UPLP-SLAM的运行时间

3. 移动机器人平台上，在三个真实世界的环境中进行UPLP-SLAM和其他方法的测试，包括办公室、工业环境和走廊。

4.1在公共数据集上的比较实验

1. 数据集：从TUM RGB-D公共数据集中选择10个序列，从ICL-NUIM公共数据集选择3个序列进行实验。

2. 方法比较：UPLP-SLAM与11种最先进的视觉SLAM方法和3种自身的消融方法进行比较：

• 基于两点特征的SLAM系统——RGBD-SLAM 和ORB-SLAM2 ；

• 三个基于几何特征的SLAM系统——CPA-SLAM 、Edge-SLAM 和STING-SLAM ；

• 三个基于多特征融合的SLAM系统——Pinpoint-SLAM、PL-SLAM和ManhattanSLAM；

• 三个基于地图融合的SLAM系统——ElasticFusion 、GC-SLAM 和 PSM-SLAM ；

• 以及三种自身的消融方法——UPLP-SLAM（PL点线）、UPLP-SLAM（PP点面）和UPLP-SLAM（LP线面）。

3.实验评估标准：绝对轨迹误差（ATE），即估计的相机位姿和真实的相机位姿之间的差。然后，计算ATE的均方根误差（RMSE）。

在基于地图融合的SLAM系统中，与优化估计相机轨迹的一般SLAM方法不同，ElasticFusion、GC-SLAM和PSM-SLAM是以地图为中心的方法，更注重重建地图的准确性，重建地图是由RGB-D相机以增量方式构建的。

表1 不同SLAM方法之间ATE的RMSE比较

结论有：

①.在kt0、kt1和kt3序列中，场景中的纹理简单，点特征容易出现误匹配，而线面特征丰富。所以UPLP-SLAM在kt0、kt1和kt3序列中获得最高精度。

②.在fr1/desk、fr1/desk2和fr1/room序列中，由于有效平面的数量不足，STING-SLAM、Pinpoint SLAM和CPA-SLAM容易退化。（但是看起来这三个环境下怎么CPA-SLAM更好一点？STING-SLAM不是针对退化进行了优化吗？反而没有针对退化的CPA-SLAM指标要好一点？）

③.fr3/str_ntex_far和fr3/str_ntex_near序列来自低纹理环境，只能提取少量的点特征。ORB-SLAM2无法成功初始化跟踪过程，这导致ORB-SLAM2在这两个序列中失败。

④尽管ORB-SLAM2可以在fr1/desk和fr1/desk2序列中成功跟踪特征并实现高定位精度，但它对纹理信息敏感，鲁棒性较差。

然后是UPLP-SLAM在fr3/str_ntex_far和fr3/str_ntex_near序列中的ATE轨迹估计结果，如下图8所示。通过特征相互关联和联合优化，可以在UPLP-SLAM中充分利用环境的点-线-平面特征及其之间的约束关系。因此，在这种低纹理和杂乱的环境中实现了更高的精度和更强的鲁棒性。

下图9显示了UPLP-SLAM提取的线和平面特征在fr1/desk序列中的分布。可以看出，在室内环境中，线和平面特征是足够的，在SLAM中添加高级几何特征可以更好地利用环境信息。

下图10显示在四个序列中UPLP-SLAM和ORB-SLAM2之间的ATE轨迹估计结果的比较。可以看出，与ORB-SLAM2相比，UPLP-SLAM估计的相机轨迹与实际轨迹具有更高的重叠度，证明了UPLP-SLAM的更高精度。

4.2测试UPLP-SLAM的运行时间

对UPLP-SLAM的运行时间进行了测试。表2分别显示了跟踪线程中运行时间的平均值以及四个序列中UPLP-SLAM和ORB-SLAM2的优化时间。从表2可以看出，与ORB-SLAM2相比，UPLP-SLAM在跟踪和优化方面的运行时间略大。这是由于在UPLP-SLAM中使用了额外的高级几何特征（线和平面），这需要更长的计算时间。运行时间的增加并不显著，UPLP-SLAM仍然可以实时运行。

4.3在真实世界环境中的测试

为了验证UPLP-SLAM在真实世界环境中的有效性，配备了Kinect 1.0传感器的Pioneer 3-DX机器人，用于在三种不同类型的环境中收集数据。比较的方法包括ORB-SLAM2和ORB-TEDM，实验是要走出一个闭环路线，比较起点和终点的距离。从表3和图13中，可以清楚地看出，与其他两种SLAM方法相比，UPLP-SLAM具有更低的漂移和更高的定位精度。

我理解的真实世界环境就是，不跑数据集，在现实中实时运行SLAM，相当于使用的是自己的数据。

---

5.总结

①论文中的总结：

本文提出了一种点-线-平面多特征相互关联方法和一个统一的多特征紧耦合联合优化框架。进而实现一种统一的点-线-平面特征融合RGB-D SLAM方法，即UPLP-SLAM，以提高环境信息的利用率、定位精度和鲁棒性。

本SLAM的可拓展性：UPLP-SLAM 可以扩展到视觉-激光雷达融合 SLAM 和立体视觉 SLAM，从而进一步提高机器人的定位性能和对复杂环境的适应性。由于机器人提取了环境中的线面特征并准确估计了其参数，因此构建的地图中包含了丰富的高级结构信息。一方面，与由点特征组成的地图不同，UPLP-SLAM 构建的地图可直接用于机器人的运动规划和导航。另一方面，可以从点-线-面地图中进一步提取语义信息，用于场景理解和人机共存。

未来的工作将对 UPLP-SLAM 进行扩展，使其涉及更高层次的几何特征，如轮廓线、弯曲线和弯曲平面，以进一步增强对环境的描述能力。

②个人总结：

通过SP模型，用坐标系来统一表示点、线、面几何特征。SP模型的作用：①提供了一种几何通用的表示方法，易于拓展到更复杂的几何特征，并且易于建立各种几何特征之间的相互联系；②在处理点线面特征过程中，能够消除冗余信息，提高处理几何特征信息的效率；③运用到优化部分时，特征参数之间的误差不仅包括同类特征之间的误差，也包括异类特征之间的误差。

不过没有提到矢量运算符⊕ 和 ⊖ 的运算规则，即每个公式的具体计算方式比较模糊，不易进行进一步推导。位姿向量和约束矩阵的表示形式使用的仍然是[35]中滤波器版本的SP模型，所以在推导过程中可能会卡壳。

论文最大的创新是把SP模型用到了基于非线性优化的SLAM框架（而非以往的滤波器框架），这是一项具有开创性意义的研究工作。现在的视觉SLAM框架多数都是基于非线性优化的，所以以后的视觉SLAM研究中又多了一项可以使用的工具——SP模型。SP模型在以前感觉算是一项比较冷门的工作，过了三十年，第一篇提出SP模型的那个论文引用量好像就10个左右，而且不到一半的论文都是这个作者和他团队自己的研究工作。

以后可以使用SP模型对所有设计线面特征的SLAM进行改善，包括位姿预测、重定位。所以本文的拓展通用性是很强的：

1.多传感器融合：UPLP-SLAM 作为一个RGB-D相机为传感器的SLAM，可以扩展到视觉-激光雷达融合 SLAM 和立体视觉 SLAM。如果有可能，可以考虑一下和IMU的使用，IMU其实也有自己的一个位姿，说不定也能实现紧耦合呢？

2.从地图中进一步提取语义信息：除了语义SLAM的思路，还有以物体作为特征的物体级SLAM。几何特征和语义特征的融合也可以做文章。物体特征通常也是用几何体来表示，感兴趣的可以看看cube-SLAM以及它的相关文献，它是用长方体作为物体特征的，所以我想着可不可以用几何特征的组合来表示物体什么的？或者做数据关联的相关工作也可以？

[1]YANG S, SCHERER S. CubeSLAM: Monocular 3D Object SLAM[J/OL]. IEEE Transactions on Robotics, 2019: 925-938. CubeSLAM: Monocular 3-D Object SLAM | IEEE Journals & Magazine | IEEE Xplore. DOI:10.1109/tro.2019.2909168.

---

6.相关文献

1.SP模型的开篇之作

Juan D Tardos, Representing partial and uncertain sensorial information using the theory of symmetries, ICRA (1992) 1799–1804, pages.

描述了几何的对称变换理论，将几何位姿的对称变换子群进行了分类，表示出了点、线、面、半圆、拐角、圆洞等几何特征的位姿变换形式，提出了用EKF来优化这些特征的变换。

2.使用SP模型的论文

①用激光雷达作为传感器、滤波器作为优化的SLAM：

Jose A Castellanos, Jos´e MM Montiel, Jos´e Neira, Juan D Tardos, ´ The SPmap: a probabilistic framework for simultaneous localization and map building, IEEE Trans. Robot. Autom. 15 (5) (1999) 948–952. Jos´e Neira, Juan D Tardos, ´ Joachim Horn, Günther Schmidt, Fusing range and intensity images for mobile robot localization, IEEE Trans. Robot. Autom. 15 (1) (1999) 76–84.

②用SP模型表示新型特征：

Folkesson, J., P. Jensfelt, and H.I. Christensen. 2007. “The M-Space Feature Representation for SLAM.” IEEE Transactions on Robotics 23 (5)

③多传感器融合的滤波器优化SLAM（超声波传感器、激光测距仪和几个电荷耦合设备摄像头）：

Foresti, G.L., and C.S. Regazzoni. 2002. “Multisensor Data Fusion for Autonomous Vehicle Navigation in Risky Environments.” IEEE Transactions on Vehicular Technology 51 (5): 1165–85. doi:10.1109/tvt.2002.800629.