Liu, Yang, Chi Guo, and Yingli Wang. “Object-Aware Data Association for the Semantically Constrained Visual SLAM.” Intelligent Service Robotics 16, no. 2 (April 1, 2023): 155–76. https://doi.org/10.1007/s11370-023-00455-9.
1 Introduction
作者认为，将物体级语义信息和传统vSLAM 融合需要考虑两个关键问题：
- 第一个问题是构建连续帧之间的数据关联问题。作者认为物体级vSLAM 需要充分利用相机运动以及3D 几何信息，最终产生一个带有全局一致性物体信息的3D 地图。现有的物体级vSLAM 大多依赖于点云的低级几何特征匹配(Yang 和 Scherer, 2019)、聚类或者统计分析进行数据关联，而忽略了物体整个的外观信息；
- 第二个问题是怎么利用物体级信息进行位姿估计。CubeSLAM(Yang 和 Scherer, 2019)、QuadricSLAM (Nicholson 等, 2019) 都是将物体建模为可观测的几何参数在位姿估计中建立约束；但是作者认为，根据物体级约束来提高传统数据关联的准确性和鲁棒性也可以提高位姿估计精度。
总结来看，本文的贡献如下：
- 提出一个基于物体外观和特征点路标的物体级数据关联框架，产生一个带有物体信息的语义地图；
- 提出一个语义重投影误差项并将其整合至位姿优化中，该重投影误差项结合了语义与特征点观测作为相机位姿的联合约束；
- 在公开数据集上进行了测试，证明了本方法在物体级数据关联中实现了较高的准确性，并超越了位姿估计基线方法。
3 Methods
3.1 System overview
系统框架如Fig. 1所示，本系统基于ORB-SLAM2 并使用双目/RGB-D 图片作为输入。
3.2 Build object-level features and landmarks
3.2.1 Build 2D-objects
2D 物体语义信息用来构建新的3D 物体、更新已有的3D 物体并为位姿优化提供约束。作者使用YOLACT 实例分割网络来获取2D 物体语义信息，实例级语义信息包含物体掩码、类别标签以及置信度，2D 物体集合表示为 $S = \{s_j\}$ ，其中，每个2D 物体 $s_j = \{l_j, q_j, m_j, b_j, f_j, Z_j\}$ ：
- $l_j$ 表示物体类别标签
- $q_j$ 表示分类置信度
- $m_j$ 表示物体二值掩码图像
- $b_j$ 表示物体的2D bbox
- $f_j$ 表示物体掩码区域的HSV (Hue-Saturation-Value) 直方图向量，从RGB 转换为HSV，向量维度为94，来描述物体外观
- $Z_j$ 物体掩码区域内的ORB 特征点集合
3.2.2 Build and update 3D-objects
3D 物体作为物体级地标存储了物体的外观和几何信息，$o = \{l, P, B, F, C\}$ :
- $l$ 表示类别标签
- $P$ 表示物体的稀疏路标点云
- $B$ 表示3D bbox
- $F$ 表示之前所有帧中该2D 物体的HSV 特征向量集合
- $C$ 表示存储当前与之前物体点云中心点的集合
3.2.3 Outliers elimination
外点指的是那些位于本物体的特征点云中，但是不属于本物体的特征点。造成外点的原因包括：
- 双目相机估计的深度误差或者RGB-D 相机的深度测量误差；
- 错误的实例分割结果导致包含背景区域或其他物体；
- 在物体边界处的特征点难以区分类别。
作者综合利用三种手段进行外点剔除：统计检验、聚类以及概率分析。
- 统计检验：作者假设一个物体的点云分布较为紧凑且近似符合高斯分布，点云中心作为其均值，统计所有点云计算标准差；
- 聚类：作者假设物体点云应该是相连的，一小部分外点会被主体点云所孤立。作者使用DBSCAN 对物体点云进行聚类，将点云分为几个独立的聚类点云，计算各个聚类中点云所占的比例。如果某个点所属的聚类占总体点云比例较小且该点距离点云中心的距离超过3倍标准差，则将该点视为外点。值得注意的是，DBSCAN 算法消耗很大的计算资源，若某个物体的点云数量超过一定阈值则不使用聚类检测手段，只使用剩余两个方法；
- 概率分析：利用语义一致性进行检测，考虑到由于模糊分割造成的外点其分类一般具有随机性，很难长期保持一个稳定的类别标签，因此，作者统计每个特征点的分类概率，若该点属于对应物体的概率值较低，则可判定该点为外点。
3.3 Object-level data association
作者考虑多种度量方式来检查物体的外观和几何一致性，以此进行短期与长期的物体级数据关联。
3.3.1 Data association metrics
Appearance metric
本文的物体级数据关联核心度量手段是物体的外观相似度检查。基于从物体掩码区域提取的HSV 特征向量进行外观检测，考虑了物体的整体外观特征，且不需要物体具有突出的几何特征。外观相似度的计算过程如下所示，表征两个归一化HSV 向量 $f_1, f_2$ 的cosine 相似度：
2D IoU metric
计算两个2D 物体的bbox IoU，被广泛应用于短期物体跟踪。
3D overlap ratio metric
由于相机在单一视角下只能观测到物体的一部分，因此，作者不直接计算3D IoU，而是使用重叠部分占两个视角3D 区域的最大比例：
Object center metric
3D 物体的点云是随着观测数据的加入而增量式更新的，相应的点云中心坐标也会随着新的观测数据加入而改变；针对新观测的物体点云，作者计算该点云中心坐标与物体之前所有点云中心坐标的最近距离作为一个度量参数：
3.3.2 Data association scheme
本数据关联框架包含两种物体数据关联以及冗余的物体检查。
Short-term data association
作者在连续两帧中利用2D IoU 和外观检测来进行物体级的短期数据关联。
查看作者源代码发现，作者利用两帧的2D物体匹配更新当前帧的3D物体，判断2D 物体匹配成功的准则为：两者外观相似度大于0.8，且2D IoU不小于0.5。
Long-term data association
当物体无法利用该短期数据关联完成时，作者利用前端的特征点匹配得到相机的初始位姿估计，基于此将物体的特征点映射至空间中以产生临时的3D 点云，结合外观检测、3D 重叠以及物体中心度量来实现图片中2D 物体与地图中3D 物体的关联。
查看作者源代码发现，作者没有使用3D 重叠来建立长期数据关联，而是符合以下两种情况之一即建立数据关联：
- 外观相似度大于0.8，且该物体中心与3D 物体历史观测中心的平均距离小于阈值 $\mu_c$ （室内场景为0.3，室外场景为5.0）；
- 该物体中心与3D 物体历史观测中心的最小距离小于0.1。
完整的数据关联如下所示：
3.4 Pose optimization constrained by semantic information
本文的语义重投影误差采用与文章(Lianos 等, 2018) VSO 相似的方法，主要不同之处在于本文建立物体对应关系的语义约束，此外还建立了一个新的损失函数以及优化策略。
理论上来讲，只要一个物体出现在视角中，那么该物体点云的投影像素应该位于相应的掩码区域内，如Fig. 4所示，物体级语义信息对于视角变换和尺度变换具有鲁棒性。
对于一个新观测帧，获取到一组匹配点对 $\mathcal{M}_p = \{(z_i^g,p_i)\}$ ，其中，$z_i^g$ 为图片中的ORB 点，$p_i$ 为地图中的特征点地标；进行物体级数据关联之后，得到一组物体匹配对 $\mathcal{M}_o = \{(s_j, o_j)\}$ ，其中，$s_j$ 表示图片中的2D 物体，$o_j$ 表示地图中的3D 物体。利用物体匹配 $\mathcal{M}_o$ 来检查匹配点对 $\mathcal{M}_p$ 的语义一致性：
其中，$\emptyset$ 表示背景类，示例如Fig. 5所示。其中，$\mathcal{M}_p^{base}$ 中的点对表示低等级几何一致性和物体级语义一致性；特别地，式（6）中 $\mathcal{M}_p^{base}$ 中属于不同语义类别的两种情况：
- 一个平面语义特征点与一个背景点地标匹配时（上式第二行），该背景点地标可视为该物体的备选元素；
- 一个背景特征点与一个语义点地标匹配时（上式第三行），该3D 物体没能与2D 物体匹配上，可能是由于数据关联失败或者实例分割误差的存在。
$\mathcal{M}_p^{joint}$ 中的匹配点对不满足语义一致性。
整个优化过程分为两步：初始位姿优化和语义约束优化。
在初始位姿优化中，利用 $\mathcal{M}_p^{base}$ 中匹配点对计算传统的重投影误差：
而对于 $\mathcal{M}_p^{joint}$ 中的匹配点对不具有语义一致性，因此需要更多的约束来进行位姿优化，此处，作者添加一个语义重投影误差项来作为额外约束，定义重投影像素与所属2D 物体最近的距离作为语义残差项，如Fig. 5（a）所示。
式（9）中的 $h(z, m)$ 表示在 m 中搜寻距离 z 最近的255数值点（即2D 物体的掩码区域）；$\hat{T}$ 表示相机的初始位姿。
疑问：式8中是否还需要第一项，是否只有第二项时会有更优的结果？？？
作者将上述两个误差项在初始优化中进行结合：
在初始位姿优化后，作者为优化增加额外的语义约束。基于初始位姿优化得到的位姿，作者将3D 物体中没有被匹配的地图点投影至图像中，并搜寻2D 图像中对应最近的像素作为语义观测项，剔除掉那些距离过远的点，如Fig. 5（b）所示，由此构建新的误差项：
从而构建整个位姿优化的误差函数：
上述优化策略可应用于对相机位姿和特征点地标的同时优化中，但是会大幅增加计算复杂度。在作者的实验中，仅利用其对相机位姿进行优化（match local map）来限制约束数量以及计算负载。作者还利用Kd-tree 来加速最邻近点的搜寻。