[论文笔记]Probabilistic data association for semantic slam

1. introduction

读完总结，感觉这篇文章的主要思想是，提出数据关联，并根据语义的类别结果计算数据关联的概率，然后在优化过程中，根据之前的数据关联作为概率权重，引入与目标检测位置有关的误差项，从而让目标检测来辅助位姿估计。

数据关联和识别问题是不同类别之间离散的，而传统SLAM是连续优化的。

本文的目标是利用目标检测，整合尺度(metric)信息和语义(semantic)信息，用于传感器位姿估计和地图点位置估计。具有语义标签的路标点能够用于解决传统几何SLAM的两个问题：一是数据关联问题，即匹配传感器的观测与路标。二是回环检测，即用于识别之前已经去过的地方。

传统基于位姿图的非线性优化方法，依赖于运动模型和观测模型的线性化，而对于离散的目标检测结果，这就成为了一个问题。

本文的贡献如下：

• 紧密结合了惯性传感器、几何信息、语义观测，于一个单独的优化框架之中。
• 对于联合的尺度-语义SLAM问题，将其分解为两个子问题：连续的位姿优化问题，离散的数据关联和语义标签优化问题。

2. probabilistic data association in SLAM

文章给了一个SLAM问题的定义：

拥有一组地图点：$L=\{l_m{\}}_{m=1}^M$

一组传感器观测：$Z=\{z_k{\}}_{k=1}^K$

一组相机位姿：$X=\{x_t{\}}_{t=1}^T$

一组数据关联：$D=\{({\alpha }_k,{\beta }_k){\}}_{k=1}^K$

这里数据关联表示，在第k个观测下，从位姿$x_{{\alpha }_k}$观测到路标点$l_{{\beta }_k}$。正常情况下这个从哪能观测到什么是不知道的，比如我从某个观测k的位姿上观测，可能观测到椅子的一个点，也可能观测到杯子的一个点。感觉这里把位姿和观测联想在一起会容易理解一点，观测是在不同位姿下进行的，而它所能观测到的物体是不确定的。这里数据关联和地图点坐标、相机位姿一样，也是待优化的随机变量。

传统SLAM是估计地图点的位姿和相机位姿，但没有考虑数据关联。引入数据关联后，SLAM问题就变成：

$$x,l,d=argma{x}_{x,l,d}logp(z|x,l,d)$$

文章提出了三种求解方法:

第一种是分为两步：

$$D^{\prime }=argma{x}_{D}p(D|X^0,L^0,z)$$

$$X^{\prime },L^{\prime }=argma{x}_{X,L}logp(Z|X,L,D^{\prime })$$

即先根据初始的位姿和路标以及观测，求出数据关联的最优值，再把这个求出的数据关联当作已知条件，求出路标和位姿的最优值。

比如，先根据初始的位姿和路标，确定观测到的是椅子的点还是桌子的点，从而根据椅子或桌子在地图种的位置，来估计相机位姿。

这种方法存在的问题是，如果估计错了数据关联，则对后续的位姿和路标点估计影响很大。同时，模糊的观测会被舍去，以避免错误的数据关联估计，那这些信息就完全无法发挥作用了。

第二种方法解决了第一个问题，它采用迭代的坐标下降法：

$$D^{i+1}=argma{x}_{D}p(D|x^i,L^i,Z)$$

$$x^{i+1},L^{i+1}=argma{x}_{X,L}logp(Z|X,L,D^{i+1})$$

这种方法使用上一次得到的位姿和路标点估计，来估计当前的数据关联，再用数据关联值估计当前的位姿和路标点。在位姿和路标点的估计改善后，可以重新优化数据关联。但模糊数据的问题没有解决。

第三种方法解决了以上两个问题，它使用所有数据关联的整体分布：

$$x^{i+1},L^{i+1}=argma{x}_{X,L}{E}_D(logp(Z|X,L,D)|x^i,L^i,Z)=argma{x}_{X,L}\sum _{D}p(D|X^i,L^i,Z)logp(Z|X,L,D)$$

这里使用了期望测量似然，计算了数据关联项的期望。要优化的变量是当前位姿$x^{i+1}$和路标点$L^{i+1}$，因变量是$logp(Z|X,L,D)$，即优化位姿和路标使这个log似然最大。

第二个等式表示的是，在上一帧位姿和路标的估测条件下，求数据关联期望下的最大似然。

第三个等式是其展开，$p(D|X^i,L^i,Z)$是求得这种数据关联D的概率，在这个概率下，乘以用这个D求得的似然概率，再对所有D的情况求和。

因为是平均意义下的，所以可以克服之前的问题，不需要只决定一种数据关联，而抛弃其他可能性。

这个方法还可以写成：

$$argma{x}_{X,L}\sum _D\sum _{k=1}^{K}p(D|X^i,L^i,Z)logp(z_k|x_{{\alpha }_k,l_{{\beta }_k}})=argma{x}_{X,L}\sum _{k=1}^K\sum _{j=1}^M{w}_{kj}^{i}logp(z_k|x_{{\alpha }_k},l_j)$$

$$w_{kj}^i=\sum _{D\in D(k,j)}p(D|X^i,L^i,Z)$$

第二个等式是把每一个可能的数据关联种，所有观测拆开计算。第三个等式，则是对于所有观测，每一个观测所有数据关联求和计算，其中j表示的是这个观测看到的路标的可能值。即，对于第k个观测，从这个位姿看到的路标点存在M个可能性，这个可能性就是权重w。

这个公式还可以转换成矩阵写法，以便于在算法中实现。

也可以用EM算法来对其进行求解：

• E步：计算$w_{kj}^i$，即数据关联的概率分布
• M步：根据E步的权重，求最大期望似然log的位姿和路标估计

3. semantic SLAM

设定路标点$l\in L$包含以下信息：

• 位置（3*1），三维点坐标
• 类别（1*1），类别集合中的一个离散值

为了估计路标点和相机位姿，使用了以下三种信息：

• 惯性测量
• 几何特征点
• 物体的语义观测

3.1. inertial information

假定传感器包含：一个单目摄像头、一个IMU。

每个关键帧内，传感器的信息包括：

• 传感器的6自由度位姿
• 速度
• IMU偏差值

在关键帧的间隔之间，传感器收集了一个IMU测量集合$I_t$，包括线性加速度和旋转角速度。

3.2. geometric information

使用ORB特征估计关键帧之间的转换矩阵。

假定特征点的数据关联已知。(什么意思？)

3.3. semantic information

对于每一关键帧，语义检测结果为$S_t$，包含多个目标检测结果$s_k$，每隔目标检测结果包含：

• $s_k^c$：目标检测类别
• $s_k^s$：置信度
• $s_k^b：$bounding box

对于已知的数据关联，测量的似然为：

$$p(s_k|x_{{\alpha }_k},l_{{\beta }_k})=p(s_k^c|l_{{\beta }_k}^c)p(s_k^s|l_{{\beta }_k}^c,s_k^c)p(s_k^b|x_{{\alpha }_k},l_{{\beta }_k}^p)$$

其中：

• $p(s_k|x_{{\alpha }_k},l_{{\beta }_k})$: 是在当前数据关联下，得到k检测结果的概率
• $p(s_k^c|l_{{\beta }_k}^c)$：是路标点类别为$c_1$条件下，检测结果为$c_2$的概率。（这里两个类别相等吗？）
• $p(s_k^s|l_{{\beta }_k}^c,s_k^c)$：是在路标点和观测的类别下，得到当前置信度的概率
• $p(s_k^b|x_{{\alpha }_k},l_{{\beta }_k}^p)$：是在当前数据关联下，路标点位姿为p，检测出当前bounding的概率。

4. semantic SLAM using EM

优化过程中，潜变量还有：数据关联、离散的路标类别标签。文中将连续变量和离散变量分开优化。

数据关联中，几何的数据关联（在某个位置看到哪个空间点？）根据特征点跟踪算法得到，即传统VO；语义的数据关联（在某个位置看到哪个类别的点），作为潜变量一起优化。

假设：数据关联在所有关键帧之间是相互独立，并服从均匀分布的。

优化的目标是：


这里，优化的变量是$x^{i+1}$、$l_{1:M}^{i+1}$，就是第i+1帧的位姿和路标位置。

看这个式子几层求和的含义，最内的$j$是对所有路标点（几何下），中间层是对于t帧里每一个目标检测结果，最外层是对于所有关键帧。

即，对所有关键帧中，每一帧的所有目标检测，的所有地图点下的概率求和。

再看w，w是权重。先看求和部分，内层是对所有的数据关联，外层是这个路标点j所有的类别，即某个坐标点类别下，得到这一组数据关联。求和里面的K，其分子是对于，上一个位姿和路标，及某一个数据关联下，得到这一组语义结果的概率。分母对所有的数据关联和坐标点类别求和，即归一化系数。

整体来看，优化的公式是看了所有关键帧，对于某一个关键帧st，看其中的所有检测结果。在一个检测结果下，看所有的地图点。对于某一个地图点，可以确定一个<位姿-地图点>数据关联。对于这个数据关联的所有可能情况，以及所有可能的这个路标点的类别，计算权重。

接下来用EM算法求解：

4.1. E步，Object class and data association

这一步是计算上面的权重$w_{k,j}^t$。计算对于每一个迭代i的权重，需要求和所有的数据关联（k,j）。包括每一个测量k和路标j，再对所有关键帧t求和。然后，用求得的第i时的权重，去帮助优化M步中的i+1时的路标和位姿。

同时，可以更新路标点的类别预测结果，这里是用SLAM的结果改善深度网络的语义结果（这里先不看了… 不是很重点的感觉）：

4.2. M步，Pose graph optimization

得到了权重，可以开始优化i+1时的位姿和路标点。可以解读上面那个argmin的公式了。

优化部分使用了位姿图。

• 节点：待优化的变量$x^i$，$l^i$
• 边：一些因子，与单个代价函数的成分相关联。（就是公式里的减号分开的三个部分）

每一个因子都与一个代价函数相关联，然后整体优化，它们都有这样的形式：

$$v^{\prime }=argmi{n}_v\sum _{f\in F}f(V)$$

这里的v是与代价函数有关的一组变量，这个代价函数是这个因子相对应的。就是分开每条边优化变量。

这里用来三种因子：

4.2.1. semantic factors

定义语义相关的factor:$f_{k,j}^s(X_i,l_j)$，表示测量k中，在相机位姿i下，观测地图点j，得到的语义结果s。

其loss定义为：

$$f_{k,j}^s(X_i,l_j)=-w_{kj}^{t,i}logp(s_k|x_t,l_j)$$

在语义部分我们知道：

$$p(s_k|x_{{\alpha }_k},l_{{\beta }_k})=p(s_k^c|l_{{\beta }_k}^c)p(s_k^s|l_{{\beta }_k}^c,s_k^c)p(s_k^b|x_{{\alpha }_k},l_{{\beta }_k}^p)$$

因为$l_c$也就是路标点的类别是固定的，因此，关于置信度$s^s$和类别$s^c$的部分是常量，可以省略。

$$f_{k,j}^s(X_i,l_j)=-w_{kj}^{t,i}logp(s_k^b|x_t,l_j)=||s_k^b-h_{\pi }(x_t,l_j)|{|}_{R/W}^2$$

中间等式是只保留关于bounding box的部分，后面一项中，h是把可见地图点重投影到当前帧，然后和bounding box求距离差，根据它的均值和协方差计算的范数。

就是说，语义相关的loss，是地图中重投影回来和当前帧检测结果的距离loss。所以这里，目标检测结果辅助了相机位姿估计？

4.2.2. geometric factors

这个因子是：

这里计算的是对于追踪的特征点的误差，第二个公式计算的是，三维地图点投影到当前帧，和当前帧追踪的特征点的距离差，就是重投影误差吧…。

后面还线性化了一哈。

4.2.3. inertial factors

这里是引入了IMU的测量结果，包括旋转速度位置，得到了一个因子。

至此，就得到全部图优化模型，是由上面辣个优化的argmin公式逐项转化来的。变成了一个最小二乘问题。（其实就是按照每个传感器的实际情况化简了一下?）