PW_SLAM (Semantic SLAM in Workshop)

对论文 Three-dimensional dynamic uncertainty semantic SLAM method for a production workshop (Engineering Applications of Artificial Intelligence, 2022) 的阅读整理。

PW-SLAM (Semantic SLAM in Workshop)

论文情况

• 标题：Three-dimensional dynamic uncertainty semantic SLAM method for a production workshop
• Rongsong Gou, Guangzhu Chen, Chengliang Yan, Xin Pu, Yuanyuan Wu, Yuan Tang
• 期刊：Engineering Applications of Artificial Intelligence, 2022
• 源码：未开源

1 Introduction

生产车间环境的复杂性所产生的问题：

• 传统语义分割网络可能无法正确分割物体；
• 现有网络实时性的问题；
• 现有方法难以准确定位地图上的动态物体。

提出 PW_SLAM，结构如 Fig.1，基于 ORB-SLAM3[1] 进行设计：

1. 使用轻量级语义分割网络 PWNet[2]，在车间数据集 SOP[3] 上进行训练；
2. 动态不确定特征点分类器（DUKC）用于在前端过滤动态特征点；
3. 动态物体过滤器用于过滤地图上的动态物体。

主要贡献：

• 提出 DUKC 来消除 SLAM 系统中的动态关键点。DUKC 由基于环境语义信息的语义模块和基于关键点几何信息的不确定性几何模块组成；
• 构建生产车间三维语义八叉树地图，设计动态对象地图过滤器，消除地图中的动态对象
• 提出 PW_SLAM，在 TUM RGB-D 数据集和真实生产车间环境上评估了 PW_SLAM 的有效性。

2 System Description

在 ORB-SLAM3 的跟踪、局部建图、回环检测和建图的基础上，新增了语义分割线程和语义八叉树线程。同时，在前端跟踪线程加入 DUKC。

• 图像输入后，在语义分割线程中，利用 PWnet 为跟踪线程和语义八叉树线程提供语义信息。
• 在跟踪线程中：
  • 首先，从 RGB 图像中提取关键点；
  • 其次，利用 DUKC 消除动态关键点；
  • 然后，利用剩余的静态关键点计算姿态 $T_{wc}$。
• 语义八叉树线程融合 RGB 信息、深度信息、语义信息、姿态信息 $𝑇_{𝑤𝑐}$ 生成三维稠密八叉树地图，使用动态物体过滤器剔除地图中的动态物体，生成静态地图。

2.1 PWNet for Semantic Segmentation

PWNet 输入为原始 RGB 图像，输出为物体掩码。PWNet 包含编码器（MobileNet V2）和解码器（平均池化 AP，注意力 AM，分类器），如 Fig.3。

AP 模块：

• 使用 double average pooling，融合编码器产生的低层特征和高层特征的多尺度信息：

  $F_i = \operatorname{Avg}^j (X_i) \tag{1}$

  $\operatorname{Avg}^j (\cdot)$ 为池化操作，$F_i$ 为不同尺度的特征图，$j = \{ 2, 3, 4, 6 \}$ 为池化因子，$X_i$ 表示编码器第 $i$ 的特征，其作为 AP 的输入；$X_i \in \mathbb{R}^{w \times h \times c}(i = \{ n, n-1, ..., 2, 1 \})$，$w,h,c$ 表示特征图的宽、高、通道数，$n$ 为编码器的最后一层。通过拼接通道融合低层特征图，消除高层特征图与低层特征图在语义信息和结构细节上的差异。

AM 模块：

• 利用注意力机制将多尺度融合的特征信息处理为具有相同权重的不同尺度特征信息。
• AM 模块采用 3 种卷积核（$7 \times 7$、$5 \times 5$、$3 \times 3$）对 AP 模块得到的特征图进行特征提取。利用这些特征预测 3 个尺度上对应像素位置的语义标签，建立多尺度特征金字塔。多尺度特征金字塔结构准确融合不同尺度信息，减少网络参数量。

分类器模块：

• 输出分割结果。

2.2 DUKC

2.2.1 Semantic Module

语义模块中的语义约束使用 PWNet 输出的掩码对每个图像进行预处理：

• 如果掩码对应的对象是动态的，则对这些掩码所在的图像区域进行二值化，二值化后的区域即为动态区域。

图像预处理后，采用 ORB 关键点提取方法，利用图像中的像素差值提取关键点。Fig.5 中的蓝色点为关键点，白色区域为动态区域。这个提取动态区域从而过滤关键点的方法为语义约束

仅依靠语义模块来区分动态关键点存在以下问题：

1. 动态区域的生成效果依赖于 PWNet 网络输出的掩码，有时语义分割网络不能提供精确的对象分割结果，使得生成的动态区域不完整。
2. 存在一些原本靠近动态区域边缘的关键点（Fig.5 中圈 A 内的关键点），和一些由于像素值的急剧变化而新生成的边缘关键点（Fig.5 中圈 B 内的点），这些边缘关键点无法通过动态区域直接判断。
3. 语义分割网络只能分割有限数量的类别，如果出现新的类别，则需要重新训练。

2.2.2 Uncertainty Geometry Module

为了在语义模块中准确区分关键点的状态并矫正错误分类的关键点，基于关键点的几何信息设计了一个多约束的不确定性几何模块，包括 5 部分：边缘约束 $M_1$、极线约束 $𝑀_2$、位置约束 $𝑀_3$、先验概率、概率融合。

(1) Edge Constraint

记从包含动态区域的图像上提取的关键点为：

$K_s = \{ k_{pt}^{(1)}, ..., k_{pt}^{(i)}, ..., k_{pt}^{(n)} \}, k_{pt}^{(i)} \notin S_d^{\circ} \tag{2}$

$K_s$ 为从一张图像提取的关键点集；$n$ 为关键点数量；$k_{pt}^{(i)}$ 为第 $i$ 个关键点；$S_d^{\circ}$ 为动态区域。

在边缘约束中，利用动态区域边缘到每个关键点的距离度量关键点的动态概率。取 $𝑃_{𝑐𝑜𝑛𝑡𝑜𝑢𝑟𝑠}=\{𝑝_1，...，𝑝_𝑚\}$ 表示提取的动态区域的边缘，$𝑚$ 是 $P_{contours}$ 的总数。关键点到动态区域边缘的距离可以表示为：

$\begin{aligned} D_E (k_{pt}^{(i)}, P_{contours}) &= \min_{j \in m} \{ ((k_{pt}^{(i)} (x) - P_{contours}^{(j)}(x))^2 \\ &+ (k_{pt}^{(i)} (y) - P_{contours}^{(j)}(y))^2)^{\frac{1}{2}} + w_{G1} \} \end{aligned} \tag{3}$

其中 $(k_{pt}^{(i)} (x), k_{pt}^{(i)} (y))$ 为 $k_{pt}^{(i)}$ 的像素坐标；$(P_{contours}^{(j)}(x), P_{contours}^{(j)}(y))$ 为 $p_j$ 的像素坐标；$w_G \sim N(0, \sigma_k^2)$ 为噪声。$k_{pt}^{(i)}$ 在边缘约束 $M_1$ 下的动态概率为：

$P(k_{pt}^{(i)} | M_1) = \begin{cases} 1, &k_{pt}^{(i)} \in S_d^{\circ} \\ \left(1 + \exp \left( -\frac{D_E (k_{pt}^{(i)}, P_{contours})}{k_1} \right) \right)^{-1}, &k_{pt}^{(i)} \notin S_d^{\circ} \end{cases} \tag{4}$

$k_1$ 为尺度因子。注意，关键点提取器不提取动态区域的关键点 $S_d^{\circ}$。但为了表达方便，假设关键点存在于动态区域 $S_d^{\circ}$，其动态概率值设置为 1。

(2) Epipolar Constraint

对极约束基于对极几何，其原理如 Fig.6。根据对极几何，对于一个 3D 点 $D$，它在前一帧中的投影是 $P_H$，因此它在当前帧中的投影 $P_{H^{\prime}}$ 应该位于右极线 $L_{Right}$。但是，如果 $D$ 是动态的，因此 $P_{H^{\prime}}$ 将是动态的，不会严格定位极线。因此，关键点到极线的距离与关键点是动态的概率正相关。但是考虑到关键点运动过程中存在大量的环境噪声，设置一个阈值，如果距离大于阈值，则认为关键点是动态的。

在当前图像中获得关键点集 $K_s$ 后，将使用这些关键点与上一幅图像中的关键点进行匹配。如果有一个关键点 $k_{pt}^{(i)}$ 与上一幅图像中的一个关键点成功匹配，则认为存在这两个关键点对应的 3D 点 $D$，这三点遵循对极几何的原则。定义：

$P = [u,v], \quad P_H = [u, v, f] \tag{5}$

$P^{\prime} = [u_1, v_1], P^{\prime}_H = [u_1, v_1, f_1] \tag{6}$

$P$ 为 $k_{pt}^{(i)}$ 在前一帧的像素坐标，$P^{\prime}$ 为与 $k_{pt}^{(i)}$ 匹配的关键点在当前帧的像素坐标，$P_H$ 和 $P_H^{\prime}$ 为对应的齐次坐标；$f$ 和 $f_1$ 为齐次参数。右极线表示为：

$L_{Right} = [x, y, z] = F \times P_H^T \tag{7}$

$F$ 为通过 RANSAC 计算得到的基础矩阵。$P_H$ 是三维点 $D$ 在前帧的投影，$D$ 在当前帧的投影 $P_H^{\prime}$ 应该位于极线 $L_{Right}$。但是，由于基础矩阵 $F$ 的计算误差和环境噪声的影响，特别是当关键点是动态的，关键点会偏离极线。根据这个偏移距离，极线约束 $M_2$ 可以表示为：

$D_{epoline} (k_{pt}^{(i)}, L_{Right}) = \frac{|P_H^{\prime} F P_H^T|}{\sqrt{x^2 + y^2}} + w_{G2} \tag{8}$

$w_{G2}$ 为高斯噪声。

关键点在 $M_2$ 约束下的动态概率为：

$P(k_{pt}^{(i)} | M_2) = \begin{cases} \left(1 + \exp \left( -\frac{D_{epoline} (k_{pt}^{(i)}, L_{Right})}{k_2} \right) \right)^{-1}, &D_{epoline} \ge w_1 \\ 0.5, &D_{epoline} < w_1 \end{cases} \tag{9}$

$k_2$ 为尺度因子，$w_1$ 为影戏那个阈值，用于去除基础矩阵 $F$ 的噪声。

(3) Position Constraint

位置约束利用了图像之间关键点的运动关系。在位置约束中，用一个描述子向量描述图像中每个关键点的位置。在连续的两幅图像中，如果两个关键点匹配，则它们的描述子向量不能有太大的变化，否则认为这些关键点是动态的。

定义关键点 $k_{pt}^{(i)}$ 和匹配关键点 $k_{pt}^{\prime(i)}$ 的描述子为 $desc(k_{pt}^{(i)})$ 和 $desc(k_{pt}^{\prime(i)})$，描述子间的差异可以表述为：

$D_{descriptors}(k_{pt}^{(i)}, k_{pt}^{\prime(i)}) = |desc(k_{pt}^{(i)}) - desc(k_{pt}^{\prime(i)})| + w_{G3} \tag{10}$

使用汉明距离计算描述子差异，$w_{G3}$ 为高斯噪声。

在约束 $M_3$ 下的动态概率定义为：

$P(k_{pt}^{(i)} | M_3) = \left(1 + \exp \left( -\frac{D_{descriptors} (k_{pt}^{(i)}, k_{pt}^{\prime(i)})}{k_3} \right) \right)^{-1} \tag{11}$

$k_3$ 为尺度因子。

(4) Probability Constraint

概率融合是在不同约束条件下得到的动态概率，并基于朴素贝叶斯定理将其与过去的图像信息（先验概率）相结合，从而得到关键点的最终状态。定义 $S_t(k_{pt}^{(i)})$ 为 $t$ 时刻 $k_{pt}^{(i)}$ 的状态：

• 如果 $S_t(k_{pt}^{(i)}) = 1$，则关键点是动态的；否则，是静态的。

因此，最终 $k_{pt}^{(i)}$ 的动态概率可以表示为：

$\begin{aligned} &P(S_t(k_{pt}^{(i)}) = 1 | M_1, M_2, M_3) \\ = &\frac{\eta \prod_{j=1}^3 P(k_{pt}^{(i)} | M_j)}{\eta \prod_{j=1}^3 P(k_{pt}^{(i)} | M_j) + (1 - \eta) \prod_{j=1}^3 (1 - P(k_{pt}^{(i)} | M_j))} \end{aligned} \tag{12}$

$\begin{aligned} &P(S_t(k_{pt}^{(i)}) = 1 | M_1, M_2, M_3) \\ = &\frac{(1 - \eta) \prod_{j=1}^3 (1 - P(k_{pt}^{(i)} | M_j))}{\eta \prod_{j=1}^3 P(k_{pt}^{(i)} | M_j) + (1 - \eta) \prod_{j=1}^3 (1 - P(k_{pt}^{(i)} | M_j))} \end{aligned} \tag{13}$

(12) 和 (13) 分别表示关键点 $k_{pt}^{(i)}$ 是动态点和静态点的概率。$\eta$ 为先验概率：

$\eta = \frac{T_d}{T_d + T_s} \tag{14}$

其中 $T_d$ 表示在前一帧中动态关键点的数量，$T_s$ 则表示前一帧中静态点的数量。最终 $k_{pt}^{(i)}$ 的状态表示为：

$S_t\left(k_{p t}{ }^{(i)}\right)=\left\{\begin{array}{l} 1, P\left(S_t\left(k_{p t}^{(i)}\right)=1 \mid M_1, M_2, M_3\right)>P\left(S_t\left(k_{p t}^{(i)}\right)=0 \mid M_1, M_2, M_3\right) \\ 0, P\left(S_t\left(k_{p t}^{(i)}\right)=1 \mid M_1, M_2, M_3\right)<P\left(S_t\left(k_{p t}^{(i)}\right)=0 \mid M_1, M_2, M_3\right) \end{array}\right. \tag{16}$

对于关键点 $k_{pt}^{(i)}$，(15) 是 DUKC 通过比较其动态概率和静态概率输出其状态的。DUKC融合了关键点的语义信息和运动信息：

1. 首先，计算关键点在 3 种约束条件下的动态概率；
2. 然后，设计概率融合部分以获得关键点的最终状态：如果 $S_t(k_{pt}^{(i)}) = 1$，则将关键点放入集合 $M$；
3. 最后，$M$ 中的所有关键点被删除。

算法 1 给出了 DUKC 的伪代码：

Fig.7 对 DUKC 的性能进行了定性评价。从连续的图像帧中提取关键点后的灰度关键点图如 Fig.7(a) 和 Fig.7(b) 所示，其中从动态对象中提取了许多动态关键点。语义模块过滤后的关键点如 Fig.7© 和 Fig.7(d) 所示，从 Fig.7© 和 Fig.7(d) 中可以看出，虽然位于动态对象上的大部分关键点被剔除，但在动态对象的边缘仍有许多状态未知的关键点。DUKC 在过滤连续图像帧的动态关键点方面的效果如 Fig.7(e) 和 Fig.7(f)，与图7©和图7(d)相比，动态关键点被剔除得更加彻底。

2.3 Production Workshop Semantic Octree Map

地图过滤器过滤地图中的动态对象。同时，地图以八叉树格式存储，且语义信息被添加到生成的地图中，以便它可以用于在生产车间中执行其他任务。

PW_SLAM 中的语义八叉树地图主要在语义八叉树线程中生成，其流程图如 Fig8 所示：

1. 首先，语义八叉树线程将跟踪线程生成的相机位姿信息 $T_{wc}$ 与 RGB-D 相机收集的 RGB 图像信息和深度信息相结合生成点云图；
2. 然后，利用最大融合概率将点云图转换为语义八叉树图[4]；
3. 然而，此时的语义八叉树地图包含环境中的动态对象，因此，设计动态物体过滤器，在地图更新过程中过滤这些动态物体。动态物体过滤器包括回调过滤器 A 和回调过滤器 B。

设一帧 RGB 图像上的一个像素的齐次坐标为 $P_i = [u, v, 1]^T$，像素对应的相机坐标系下的 3D 坐标为 $P_{i,c}$：

$P_{i,c} = [X_c, Y_c, Z_c]^T = K^{-1} \cdot Z_c P_i \tag{16}$

$K$ 为相机内参，$Z_c$ 为点的深度值。在获得优化后的相机位姿后，同样的，可以得到像素点 $P_i$ 在世界坐标中对应的 3D 坐标：

$P_{i, w} = [X_w, Y_w, Z_w, 1]^T = T_{wc} \cdot P_{i,c} \tag{17}$

最终点云的生成可以表达为：

$Cloud = \bigcup_{i=1}^n P_{i,w} \tag{18}$

八叉树的初始化主要描述了其节点的占用情况。由于八叉树节点的占用率不稳定，将八叉树节点的占用率转化为概率形式。定义节点占用率的概率函数如下：

$y = \operatorname{logit} (x) = \log (\frac{x}{1 - x}) \tag{19}$

$x$ 为节点被观测的次数：如果一个节点被观测到占用，则 $x$ 增加，否则减少。通过 $x$ 的观测值，可以得到节点被占用的概率 $y$。同时，八叉树节点的更新方法为：

$L(n | z_{1:T+1}) = L(n | z_{1:T-1}) + L(n | z_{1:T}) \tag{20}$

$L(n | z_{1:T+1})$ 表示节点从开始到时间 $T+1$ 的占用概率的对数，同理于另外两个变量。

八叉树生成后，地图中仍然有一些动态物体。因此使用包含过滤器 A 和 B 的动态物体过滤器。过滤器 A 是一个转移过滤器（passthrough filter）[5]，记为 $F_{radius}(\cdot)$：

$Cloud_{static} = F_{radius} (r, k, Cloud) \tag{21}$

其中 $Cloud$ 是未经处理生成的点云，$Cloud_{static}$ 是 $Cloud$ 过滤离散点云后的点云。$r(=0.02)$ 为扫描半径，如果 $r$ 范围内存在的点数量小于 $k (= 2)$，则认为该点云为离散点云。过滤器 B 是一个八叉树节点过滤器 $F_{map}(\cdot)$，可以表示为：

$Oct_{map} = F_{map} (Oct_{dynamic}, Semantic) \tag{22}$

$F_{map}$ 使用语义信息过滤八叉树节点 $Oct_{dynamic}$ 的动态对象，该节点由 $Cloud_{static}$ 得到；$Semantic$ 是由 PWNet 网络提取的动态对象的语义信息。

在八叉树节点更新过程中，两个过滤器共同工作，不断过滤八叉树图中的离散点云和动态节点，直到所有节点更新完毕，最后生成静态语义八叉树地图。带有语义信息的静态八叉树地图快速、完整地保存了环境信息，可直接用于车间导航等更高层次的任务。

3 Experimental Results

3.1 Evaluation of PWNet

• 设备：NVIDIA Jetson NANO

3.2 Evaluation on TUM RGB-D Public Datasets

• 设备：GeForce 2080，CUDA 10.1

improvements 值的是 PW_SLAM 相对 ORB-SLAM3 的提升。

3.3 Real Workshop Environment Experiment

• 设备：NVIDIA Jetson NANO，Intel RealSense D455 RGB-D

Fig.11(a) 表示 ORB-SLAM3 的关键点检测，Fig.11(b)© 为使用 DUKC 前后的关键点检测。Fig.11(d) 为 PW_SLAM 建立的不含动态物体的八叉树地图，Fig.11(e)(f) 为 PW_SLAM 和 ORB-SLAM3 建立的八叉树地图，蓝色为动态物体（人），紫色和绿色和车床和 pedals。

下式表示了静态车间场景下的地图精度结果，其中 $True$ 为生产车间内物体的真实尺寸，$Estimate$ 为 PW_SLAM 估计的模型尺寸。$Accuracy$ 是地图精度，计算公式如下：

$Accuracy = \left| 1 - \frac{Estimate}{True} \right| \times 100\tag{23}$

Table-6 中 Pedal 1 - Pedal 4 表示 Fig.11(d) 中的车间中的 pedals，Dp1 - Dp4 表示 pedals 之间的距离：

参考

• [1] Campos, Carlos, et al. “Orb-slam3: An accurate open-source library for visual, visual–inertial, and multimap slam.” IEEE Transactions on Robotics 37.6 (2021): 1874-1890.
• [2] Liu, Junmin, et al. “Pwnet: An adaptive weight network for the fusion of panchromatic and multispectral images.” Remote Sensing 12.17 (2020): 2804.
• [3] Tang, Zaizuo, et al. “Bi-stage multi-modal 3D instance segmentation method for production workshop scene.” Engineering Applications of Artificial Intelligence 112 (2022): 104858.
• [4] Tian, Yong Chen Senyuan, and Bingnan Sun. “Maximum Joint Probability Fusion Algorithm for Multi-Resolution Damage Detection Results.” Journal of Zhejiang University (Engineering Edition) 1 (2007): 126-133.
• [5] Li, Ruixue, and Jiwei Zou. “Research on point cloud filtering algorithm based on PCL library.” Satellite TV & IP Multimedia 13 (2020): 3.