论文解析 - Probabilistic Data Association for Semantic SLAM（二）语义数学模型与EM求解

前言

上文宏观梳理了SLAM问题的数学框架，接下来到论文的第三部分，站在语义信息处理的角度上分析如何用识别到的物体来辅助求解相机姿态和路标位置。

 

III. SEMANTIC SLAM

论文接下来将专注于特别的SLAM问题公式，除了传感器和路标姿态外，还引入路标类别（landmark classes）（比如门、椅子、桌子），以及物体检测形式的语义测量（semantic measurements）两个变量。

论文使用三个数据源：惯性、几何点特征和语义物体检测来估计路标姿态和传感器轨迹。

关键帧中的ORB特征（绿色点）和物体检测结果。分别代表着几何特征点和语义观测。

A. 惯性信息

使用惯性传感器IMU获得一个测量集合  ，包含线性加速度和转动角速度数据。

 

B. 几何信息（Geometric information）

使用ORB特征。通过最小化ORB描述子的距离来匹配下一帧获得相机变换矩阵（即李代数了）。

 

前面AB两者合起来作为惯性视觉里程计VIO获得里程计信息。

详情可以了解VIO和ORB-SLAM，在此不多做叙述。

 

C. 语义信息

首先我们要用非常美的数学模型来描述我们观测到的香蕉：香蕉在图像的哪，香蕉属于哪个类，这个香蕉有多可信呢。

定义从关键帧t中提取的一组物体检测结果： ，其中一个检测结果 中包含三个部分：

•  : 检测置信度 score
•  : 矩形框 bounding box，即图像中的物体位置
•  ：检测分类结果，是香蕉还是苹果还是梨子

本论文使用的object detect方法为DPM( deformable parts model ) 可以在CPU上实时运行。同时列出了一些其他描述子，可以参考。

当语义测量的数据关联是已知时（即我们知道现在的观测结果是在A点看的梨子的时候），测量的似然可以分解成下列几个部分：

各部分说明如下，上标s b c的含义如上所述。

b 矩形框估计: 物体方框的似然用正态分布表示，其均值等于物体到图像平面内的投影中心，而协方差与被检测框的规模成正比。也就是说认为中心就是准的，而知道肯定有误差，这个误差用最显然的大小来凑就好了。

c 类别估计和s 置信度计算：与物体检测的confusion matrix有关。实际上，如果用神经网络来识别物体，可以代表神经网络中对图像矩阵的计算过程。也就是说这个统一的大模型甚至将物体检测部分也整合到了一起，可以连续运算。（其实就是将物体检测部分的概率融合到了计算中而已，实际还是分开的，只是表述起来浑然一体）

 

该节的最后给出完整语义SLAM的数学定义，非常简洁漂亮，可以Mark一下以后在论文综述中引。

总结来说，惯性和几何测量用来跟踪局部传感器轨迹。（这里有一句话不是很理解，similar to a visual odometry approach, the geometric structure is not recovered. 这个recover是回环检测还是什么意思？）而语义SLAM则可以建立物体地图，并用于建立回环检测，因而在不清楚的测量结果下更加鲁棒高效。而SLAM的过程则需要完整的几何结构才能实现同样效果。

---

总结

本章对引入语义信息之后的SLAM问题做了数学模型的介绍，下一节将重点介绍如何应用期望最大化法（EM）求解这个最优估计问题。

 

前言

很不容易大家能看到这一章。在前面的章节对语义SLAM的数学模型做了详细说明，那么我们如何求解并得到所要的相机、路标的估计姿态呢。

在看论文的过程中发现底层的概率学理论、非线性优化方法实际都能在各领域通用，比如在机器学习领域，神经网络的训练也在不断应用非线性优化。

所以虽然理论枯燥，一旦打通便能达到视野广阔的境界。

 

IV. SEMANTIC SLAM Using EM

论文的求解过程，除了数据关联外，我们还把离散路标的类别也当作优化中的潜在变量，使得整体清晰高效地分割了离散和连续变量[resulting in a clean and efficient separation between discrete and continuous variables.]。（什么叫清晰分割?)

在论文III部分中所说（见解析二），几何测量的数据关联由特征跟踪算法提供，交给了前端算法不用我们管。所以我们需要考虑的重点只剩下语义测量的数据关联  和物体类别  。

下面是求解步骤：

1. 初始化：初始的位姿  通过集成里程计数据得到。路标的初始化  可以通过在位姿  结合相机观测的物体相对方向，计算对应射线空间方向并初始路标得到。
2. 求解过程

给出命题2：假如概率分布  是均匀的(uniform)，而且语义测量数据关联在关键帧之间相对独立。那么有：

接着迭代求解两步：

1) 数据关联权重  （“E”Step）

2) 连续的相机姿态和路标位置（“M”Step）

权重的求解公式如下：

其中

这部分就是个权重而已，分母部分是所有概率的和，可以当成归一化系数。

获得权重之后需要代入这个公式中获得我们想要的两个大宝贝：

该公式整体是一个优化问题，我们需要求得让整体值最小的两个  作为i+1帧状态的  。先别被这一大坨公式吓坏，我们将在下面的B. 图优化小节中慢慢拆解各部分所代表的含义。

 

A. 物体类别和数据关联（E Step）

单个帧的权重需要对所有可能数据关联取和，然而，由于帧与帧之间的独立数据关联假设，以及仅仅很少的物体可以在传感器视野内被看到，所以有可能全部遍历，因此对于所有帧t，测量k，路标j的可以被很高效地实际计算。

一旦  被计算出来，将被用于连续的传感器姿态和路标位置最优化中。此外，最大化似然的路标类估计  也可以从被计算的  值中被获得。（这里SLAM反过来帮助语义了）

脑海中一直有思路说既然SLAM综合了环境中这么多信息，如何帮助原本只靠一张图片的图像检测来识别物体呢。这个数学公式一句话就解决了这个问题。确实感觉到数学简洁之美。

我需要求解得到的路标类别，是能够让该式子值取最大时的解。该公式的证明论文中单独附出了一个Appendix II. 有兴趣的同学我将单开一节叙述这个公式。

 

B. 位姿图优化（“M”Step）

该部分就是教如何求解公式（7），我们把它再搬过来。

直观理解，我们开始搜索这个最优值，这个式子的值在随着相机姿态、路标位置和路标类别疯狂变化的状态下值在空间中变化，直到发现一个全局最优值能让整体值最小。那么我们就找到了这样的最优解组合。

我们选择用图优化的方式来求解，位姿图是反映有清晰物理结构和稀疏约束的优化问题的一种简便方式。图由一群顶点V组成，每一个顶点与一个待优化变量相关联，一组因子F在顶点之间，与单个代价函数的成分相关联。我们的图对每一个传感器姿态和每一个路标位置都有一个顶点，他们是待优化的变量。同时图的求解有很多现成的库，非常方便直接调用。

比起先前工作，传感器姿态和路标之间的连接是硬的数据关联，论文的工作则用软的语义数据关联累乘因子。而软关系的体现则用因子图来表示。最后的总表达式将是各个Factor所表示的概率分布函数的乘积。而单个Factor式子中涉及到的变量，作为因子图中的节点会通过边与对应的f相连接。

一个factor  与一个代价函数相关联，以如下形式整体求解：

公式（7）中的Factor式子分为以下几个部分：

1) 语义Factors:

在相机姿态  下的每一次测量都将给可见的路标 j 定义一个factors  ，其中的s代表语义；k代表第k次测量。

回忆各项目含义：

•  : 检测置信度 score
•  : 矩形框 bounding box，即图像中的物体位置
•  ：检测分类结果，是香蕉还是苹果还是梨子

由于  在公式（7）中是固定的（在E Step中算完了置信度和类别，M Step不改变），则  和  是常量。注意这里是地图中存放的路标，则是观测。

因此  ，而且后一项可以被舍去。

这个公式分别包含了：已知地标种类，观测是该种类的测量概率（仅与前端有关），已知地标种类和观测种类的置信度（仅与前端有关），最后的是已知相机姿态和地标位置求观测中矩形框的位置概率（这个涉及到相机和地标的位置估计）。

在实际计算中将该表达式输入到因子图求解的库中，直接获得结果。

 

下面是很神奇的部分。让  表示路标  到当前相机位姿能看到的平面图像中的投影。假定其为高斯分布，协方差为  。因此，与传感器姿态t，测量k和路标j有关的相机factor: 成为：

 

实际上反映了地图中物体投影到当前相机观测中的位置的距离（即YouTube视频中的蓝色虚拟车辆），与实际观测的物体矩形框位置之间的像素距离。该距离结合了范数，右下角  代表一个与其权重和协方差有关的范数，以及右上角的平方，整体作用产生一个衡量匹配关系的数值。

由于重新看到了之前记录的路标产生的语义factors将成为我们回环检测约束的根据。

 

2）几何Factors:

即公式（7）中的  。

其中  是清晰跟踪到的特征总数量，如观测到的几何路标的总数量。

让  成为通过测量  生成的全局框架下路标的3D位置，并如之前般假设投影有高斯像素噪声  ，于是得到：

该公式中测量与投影都是特征点的位置，如ORB特征点。其中条件  代表所有能让“：”后面成立的k。将所有地图上的特征点投影到当前图像中计算距离。

对于单个观测：

由于使用迭代方式来最优化完整姿态图，我们需要线性化上述公式。得到：

详细的线性化过程在此就不论述了。

 

3）惯性Factors:

该论文基于引文[40]提出的模型来考虑帧间接收的多个IMU测量数据，具体不多论述，公式如下：

 

到此，可以看到完整的姿态图优化模型了，就是个非线性最小二乘问题，包含三项：语义观测项（10），几何观测项（15）和惯性项（17）。

---

到此，论文核心数学模型已经介绍完毕。总体来讲本论文提出了一种框架，而很多部分实际上组合的是已经现成的模型：如特征检测ORB、物体检测deformable parts model detection algorithm [38]、还有因子图的求解框架等等。

感觉很像神经网络中提出了一种新的网络模型。

不过按照作者的大量实验，确实能够做到语义辅助定位，所以在这个框架确实能成为当今语义SLAM的基本问题：如何融合语义信息和几何信息的一个解决方案。接下来语义SLAM还有很多路要走，比如作者在论文最后提出了一个方向，即将检测物体的姿态（朝向）引入到模型中，而非现在一样只有物体的位置。

笔者特意搜索了作者2017年以来文章，并没有发现该方向的发表。所以要么作者还在做要么就卡住了。

 

感谢大家读到这里。知乎的公式编辑器真好用，只是没法可视化改