[论文]语义SLAM的概率数据关联解析（三）EM法求解

前言

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

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

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

IV. SEMANTIC SLAM Using EM

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

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

下面是求解步骤：

初始化：初始的位姿 $mathcal{X}^{(0)}$ 通过集成里程计数据得到。路标的初始化 $mathcal{L}^{(0)}$ 可以通过在位姿 $mathcal{X}^{(0)}$ 结合相机观测的物体相对方向，计算对应射线空间方向并初始路标得到。
求解过程

给出命题2：假如概率分布是均匀的(uniform)，而且语义测量数据关联在关键帧之间相对独立。那么有： $p(mathcal{D}|mathcal{S},mathcal{X},mathcal{L})=Pi_{t=1}^Tp(mathcal{D}_t|mathcal{S}_t,mathcal{X},mathcal{L})$

接著迭代求解两步：

1) 数据关联权重 $w_{ij}^t$ （「E」Step）

2) 连续的相机姿态和路标位置 $ell_{1:M}^p$ （「M」Step）

权重的求解公式如下：

$w_{kj}^{t,(i)}=sum_{ell^cinmathcal{C}}sum_{mathcal{D}_tinmathbb{D}_t(k,j)}mathcal{k}^{(i)}(mathcal{D}_t,ell^c) forall t,k,j ag{6}$

其中

$mathcal{k}^{(i)}(mathcal{D}_t,ell^c)=frac{p(mathcal{S}_t|mathcal{X}^{(i)},mathcal{L}^{(i)},mathcal{D}_t)}{sum_{ell^c}^{}{sum_{mathcal{D}_tinmathbb{D}_t}^{}{p(mathcal{S}_t|mathcal{X}^{(i)},mathcal{L}^{(i)},mathcal{D}_t)}}}$

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

获得权重之后需要代入这个公式中获得我们想要的两个大宝贝： $mathcal{X}^{(i+1)},ell_{1:M}^{(i+1)}$

$mathcal{X}^{(i+1)},ell_{1:M}^{(i+1)}=mathop{argmin}_{mathcal{X},ell_{1:M}^p }sum_{t=1}^{T}{}sum_{s_kin S_t}^{}{}sum_{j=1}^{M}{-w_{kj}^{t,(i)}mathop{log}p(s_k | mathbb{x}_t,ell_j)}\ -logp(mathcal{Y}|mathcal{X})- logp(mathcal{I}|mathcal{X}) ag{7}$

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

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

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

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

$hatell_{1:M}^{c} =mathop{argmax}_{ell^c }p(ell_{1:M}^c| heta,mathcal{Z}) =mathop{argmax}_{ell^c }prod_{t=1}^{T}sum_{mathcal{D}_tinmathbb{D}_t}mathcal{k}(mathcal{D}_t,ell^c)$

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

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

B. 位姿图优化（「M」Step）

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

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

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

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

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

$hat{mathcal{V}}=mathop{argmin}_mathcal{V}sum_{finmathcal{F}}f(mathcal{V})$

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

1) 语义Factors:

在相机姿态下的每一次测量都将给可见的路标 j 定义一个factors $f_{kj}^s(x_i,ell_j)$ ，其中的s代表语义；k代表第k次测量。

回忆各项目含义：

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

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

因此 $mathop{log}p(s|mathbb{x},ell)=mathop{log}p(s^b|mathbb{x},ell^p)+mathop{log}p(s^s|ell^c,s^c)p(s^c|ell^c)$ ，而且后一项可以被舍去。

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

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

下面是很神奇的部分。让 $h_pi(mathbb{x},ell^p)$ 表示路标到当前相机位姿能看到的平面图像中的投影。假定其为高斯分布，协方差为。因此，与感测器姿态t，测量k和路标j有关的相机factor: $f_{kj}^s$ 成为：

$f_{kj}^s(mathcal{X},mathcal{L})=-w_{kj}^{t,(i)}mathop{log}p(s_k^b|mathbb{x}_t,ell_j^p) ag{9}$ $=||s_k^b-h_pi(mathbb{x}_t,ell_j)||_{R_s/w_{kj}^{t,(i)}}^2 ag{10}$

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