Lin, Shiqi, Jikai Wang, Meng Xu, Hao Zhao, and Zonghai Chen. “Topology Aware Object-Level Semantic Mapping Towards More Robust Loop Closure.” IEEE Robotics and Automation Letters 6, no. 4 (October 2021): 7041–48. https://doi.org/10.1109/LRA.2021.3097242.
Abstract
SLAM 回环检测可有效消除累计误差，但是在大视角变化与环境外观变化时难以实现有效的检测，本文提出了基于物体建模object modeling与语义图匹配semantic graph matching的回环检测技术。作者使用体素与立方体cuboids对环境中的物体进行物体级别特征建模，将环境进一步表示为带有拓扑信息的语义图，在此基础上提出了一种基于edit distance的高效图匹配方法，最终通过语义地图的对齐来实现回环检测。
1 Introduction
人类通过环境中物体的外观与轮廓来识别场景，并在短时间内判断是否来过该该处，通过借鉴人类的回环检测方式作者提出了一种基于对象级拓扑感知语义匹配topology aware object-level semantic mapping的回环检测方法：
将环境表示为包含物体的语义图，通过图匹配来实现回环探测。
本文做出的贡献总结如下：
- 提出一个构建多粒度语义地图的框架，包含地图点、体素与立方体；
- 提取物体的多维特征（包括语义、尺寸、颜色分布以及拓朴结构）作为描述子来实现物体间的精确关联；
- 提出了一个基于语义图匹配的回环检测方法，对于视角与外观变化具有较强的鲁棒性；
- 提出一种基于物体对齐的回环矫正方法，可精确计算漂移误差。
2 Related Work
2.1 Object SLAM
相对于低级的几何信息，物体特征object features更稳定，且有着丰富的语义与先验信息。根据物体建模策略的不同，object SLAM 可分为specific type 与 general type：
- specific type：需要提前建立精准的3D模型数据库，然后进行实时检索匹配，从而实现物体的精确估计，但由于先验数据库的要求导致该方法的通用性有限；
- general type：可在跟踪相机位姿的过程中实现物体在线建模，有着更优的应用能力。一些方法使用通用的三维几何结构，如立方体、椭球体来建立粗粒度的物体模型。
2.2 Loop Closure
基于观测信息建立场景的鲁棒描述子是回环检测的核心问题，常见的描述子有以下几种：
- 提取图像的局部特征作为图像的抽象描述子，如SIFT、ORB 等；
- 直接提取整幅图像的全局特征实现场景描述，如GIST 等；
- 结合上述两种方法的优势，从图片的多个局部区域提取全局特征，组建场景的鲁棒表示。
以上基于外观的描述子构建方法受限于视角与场景外观的变化，而语义信息却具有更强的鲁棒性。据此，有作者提出利用环境中的物体来实现回环检测的视角不变性，然而，场景遮挡、布局改变以及多个重复物体的情况也很常见，这些因素对于基于穷尽搜索的物体匹配方法提出了挑战。
因此，本文作者提出使用物体的多维信息（包括语义、颜色与拓扑关系）作为描述子来实现物体的精准匹配，进而实现语义图匹配的回环检测。
3 Object SLAM
本文提出的架构是基于ORB-SLAM2 而设计的，使用RGB-D 数据来实现初步的位姿估计与稀疏地图构建；在此基础上，通过融合RGB-D 数据与它的实例分割结果来构建稠密点云，进而产生体素模型。基于体素模型使用离散采样与粒子群优化算法产生物体的压缩立方体模型，过程如Fig. 3所示。
3.1 Voxel Model Construction
从单张图片的RGB-D 与实例分割获取物体的稠密点云，体素被用来融合、量化多帧图片的稠密点云，根据连续观测信息更新体素被占据的概率。给定传感器测量数据$z_{1:t}$ ，体素被占据的概率可由下式计算获取：
$P(n)$ 表示体素被占据的先验概率，本文设置为0.5；在得到传感器测量值$z_t$ 后可通过下式获取后验概率：
在实验中，$l_{occ}, l_{free}$ 分别被设定为0.7，0.4。
由于传感器测量误差与实例分割误差，在物体初步建模中存在很多离散的外点，作者首先使用DBSCAN 算法进行density clustering 来去除孤立的小团点簇，然后将剩余的3D 体素投影到地面上，使用seed filling 来探测平面的连接区域，小团点簇被进一步去除。
3.2 Cuboid Generation
一个通用的立方体模型包含9个自由度：3个自由度的位置信息 $t = (t_x, t_y, t_z)$，3个自由度的旋转信息 $r = (roll, pitch, yaw)$ ，以及3个自由度的尺寸信息 $d = (l, w, h)$。因此，地图中的物体地标被表示为 $O = \{T_0, d\}$ ，其中 $T_0 \in SE(3)$ 表示6自由度的位姿参数。作者假设所有物体地标均位于水平平面，所以roll, pitch 参数设置为0。
如Fig. 4所示，首先将3D 体素投影到平面上，然后对 yaw 参数在二维投影平面上进行从0°到180°的离散采样，对于每个采样，可以获取一个完全覆盖2D 投影区域的最小bounding box，然后将这个box 设定为目标立方体的top view，由此可以获得立方体的5个参数：$\{t_x, t_y, yaw, l, w\}$ 。
然后，令该立方体包含垂直方向上的所有3D 体素，即可获取剩余两个参数：$\{t_z, h\}$ ，由此可以得到一个初步的立方体模型。在此基础上，使用粒子群优化算法进一步优化该立方体模型，从而得到最优的模型：
式中，$N_{occ}$ 表示立方体中被占据体素的体积，$N_{free}$ 表示未被占据的体积，权重参数 $\lambda$ 根据传感器噪声进行调整。
3.3 Data Association
当物体建模完成以后，我们可以获取到物体的语义与尺寸信息。当两个物体的尺寸差异小于它们平均值的1/3时，我们认为两个物体尺寸匹配成功；作者将图片转化为HSV 色域空间，计算两幅图片的颜色直方图并产生归一化特征向量，当两个特征向量的相关系数超过0.5时认为颜色匹配成功；使用random walk descriptor来提取物体周围的拓扑结构信息，本实验的 random walk descriptor 阈值设定为探索次数的0.5倍。
4 Loop Closure With Semantic Graph
4.1 Graph Representation
作者将环境中的物体作为顶点，将物体中心在世界坐标系中的坐标设定为该顶点坐标，将覆盖物体模型的最小球形区域定义为该节点的邻接空间，邻接空间有相交部分的节点使用无向边undirected edges 进行连接；此外，对于图中的每一个节点，遍历其余所有节点找到与其距离最近的一个节点，使用无向边进行连接，如Fig. 5所示。
作者使用图嵌入方法graph embedding method 来将基于图的结构信息嵌入到线性向量空间，如Fig. 6所示，首先选取一个边缘节点作为根节点，并测量根节点距离其他所有节点的距离，根据距离进行降序排列得到一个支持向量 $V = \{v_1, v_2, …, v_n\}$ 来表示图的拓扑结构，支持向量中的元素 $v_i$ 表示物体的描述子（语义、尺寸、颜色以及random walk descriptor）；通过替换根节点并重复上述操作，即可从图中获取多个支持向量。
4.2 Loop Detection
对于两个子图的支持向量 $V_a = \{v^1_a, v^2_a,…, v^m_a\}$ , $V_b = \{v^1_b, v^2_b,…, v^n_b\}$ ，编辑 $V_a$ 中的节点来转化为 $V_b$ ，将最小的编辑次数定义为两者之间的edit distance。节点编辑操作包括四种：插入、删除、单节点替换与两个邻接节点的交换。相邻节点的交换操作可以阻止支持向量元素的排序混乱问题，只允许与根节点有相似距离的两个节点进行交换操作，定义两个支持向量的编辑距离：
$sub_{a, b}(i, j)$ 表示交换 $v^i_a, v^{i-1}_a$ 之后的最小编辑距离，相应的，$nousub_{a, b}(i, j)$ 表示 $v^i_a, v^{i-1}_a$ 交换操作被禁止的最小编辑距离：
作为拓扑结构的补充，两个地图中匹配物体之间的空间距离一致性也被考虑，若两个向量包含共同的节点，但是节点间的距离（在语义图中用边连接的任意两个节点）超出阈值，那么就视为不匹配。最终，当每一对支持向量中的编辑距离满足以下约束则视为构成回环：
式中 $\lambda$ 被设为0.3。
4.3 Loop Correction
当检测到回环时，使用物体对齐来矫正偏移误差，偏移误差可通过对齐匹配物体来获取：
然后使用偏移误差来矫正相机位姿 $T_c$ ：
由于立方体的对称性，相机位姿有两个可行解，因此需要两对匹配立方体来消除模糊度。在获取了当前关键帧的初始位姿后，使用所有匹配的地标利用非线性最小二乘进行优化：
在优化了当前关键帧的位姿后，使用图优化方法来矫正整个轨迹。
5 Experiments
5.1 Datasets
使用TUM RGB-D benchmark，以及作者自采数据进行实验。
5.2 Semantic Mapping Performance
Fig. 7展示了基于本文提出的方法构建的语义地图。
5.3 Loop Detection Performance
作者在同一场景中以不同的视角、距离采集图像，理论上来讲所有的图片都可以被视为回环，作者使用SLAM 算法计算每张图片代表的相机位姿，固定一些图片作为模板，剩余图片进行匹配预测测试；将本文提出的方法与基于ORB 特征的回环检测算法进行对比，回环检测评价标准是匹配成功的数量，实验结果如Fig. 8、Fig. 9所示。
5.4 Localization Accuracy
与BAD-SLAM、ORB-SLAM2 进行对比实验，计算绝对偏移误差，实验结果如Fig. 10、Table 1（RMSE）所示，
