SLAM中边缘化与一致性

基于图的SLAM¹

使用因子图表示，节点表示状态，边表示约束，通常形式为：
$${\mathbf{z}}_{ij}={\mathbf{h}}_{ij}\left({\mathbf{x}}_i,{\mathbf{x}}_j\right)+{\mathbf{n}}_{ij}\text{\hspace{1em}(1)}$$
其中${\mathbf{h}}_{ij}$是状态到测量的映射，${\mathbf{n}}_{ij}$是零均值白噪声
$${\mathbf{n}}_{ij}\sim \mathcal{N}\left(0,{\mathbf{\Lambda }}_{ij}^{-1}\right)\text{\hspace{1em}(2)}$$
这就变成了MLE问题：
$$\hat{\mathbf{x}}=\arg \underset{\mathbf{x}}{\min }\sum _{(i,j)\in \mathrm{supp}(\mathbf{z})}{\Vert {\mathbf{z}}_{ij}-{\mathbf{h}}_{ij}\left({\mathbf{x}}_i,{\mathbf{x}}_j\right)\Vert }_{{\Lambda }_{ij}}^2\text{\hspace{1em}(3)}$$
然后就是使用马氏距离，利用迭代的方式求解这个问题。但是由于地图的增长，节点和边增加，变得耗费计算资源。

马尔科夫毯上通过边缘化移除节点

• 最好的线性化点应该是 local state estimate

为了减少代价，使用马尔科夫毯的概念，将节点分为以下几个类别：
$$\mathbf{x}={\left[\begin{array}{lll}{\mathbf{x}}_m^T& {\mathbf{x}}_b^T& {\mathbf{x}}_r^T\end{array}\right]}^T\text{\hspace{1em}(4)}$$
其中，${\mathbf{x}}_m$表示要边缘化掉的节点，${\mathbf{x}}_b$表示马尔科夫毯中与边缘化节点直接相连的节点，${\mathbf{x}}_r$表示这个图中剩余的节点。

将观测划分为：
{ z b , z r } = { z m , z c , z r } (5) \left\{\mathbf{z}_{b}, \mathbf{z}_{r}\right\}=\left\{\mathbf{z}_{m}, \mathbf{z}_{c}, \mathbf{z}_{r}\right\} \tag5 { zb​,zr​}={ zm​,zc​,zr​}(5)
其中${\mathbf{z}}_m$是直接约束${\mathbf{x}}_m$的测量，${\mathbf{z}}_b$表示马尔科夫毯中所有的约束，${\mathbf{z}}_c$表示马尔科夫毯中除了${\mathbf{z}}_m$其他的约束，${\mathbf{z}}_r$表示剩下的约束。

举例，Fig1：

x m = x 1 x b = [ x 0 T , x 2 T , x 3 T ] T x r = x 4 z m = { z 01 , z 12 , z 13 } z c = { z 0 , z 03 , z 23 } z r = { z 04 , z 34 } (6) \begin{aligned} \mathbf{x}_{m}&=\mathbf{x}_{1}\\ \mathbf{x}_{b}&=\left[\mathbf{x}_{0}^{T}, \mathbf{x}_{2}^{T}, \mathbf{x}_{3}^{T}\right]^{T}\\ \mathbf{x}_{r}&=\mathbf{x}_{4} \\ \mathbf{z}_{m}&=\left\{\mathbf{z}_{01}, \mathbf{z}_{12}, \mathbf{z}_{13}\right\}\\ \mathbf{z}_{c}&=\left\{\mathbf{z}_{0}, \mathbf{z}_{03}, \mathbf{z}_{23}\right\}\\ \mathbf{z}_{r}&=\left\{\mathbf{z}_{04}, \mathbf{z}_{34}\right\} \end{aligned} \tag{6} xm​xb​xr​zm​zc​zr​​=x1​=[x0T​,x2T​,x3T​]T=x4​={ z01​,z12​,z13​}={ z0​,z03​,z23​}={ z04​,z34​}​(6)
边缘化之后的节点会产生一个先验约束${\mathbf{x}}_b$
$$p\left({\mathbf{x}}_b|{\mathbf{z}}_m\right)={\int }_{{\mathbf{x}}_m}p\left({\mathbf{x}}_b,{\mathbf{x}}_m|{\mathbf{z}}_m\right)\mathrm{d}{\mathbf{x}}_m=:\mathcal{N}\left({\hat{\mathbf{x}}}_b,{\mathbf{\Lambda }}_t^{-1}\right)\text{\hspace{1em}(7)}$$
边缘化之后，除了由于线性化导致的误差外，所有的信息都在${\mathbf{z}}_m$中，没有损失。

边缘化后的先验分布

为了得到公式（7）的分布参数( mean and covariance)，求解一个最大后验问题：
$$\max p\left({\mathbf{x}}_b|{\mathbf{z}}_m\right)={\int }_{{\mathbf{x}}_m}\max p\left({\mathbf{x}}_b,{\mathbf{x}}_m|{\mathbf{z}}_m\right)\mathrm{d}{\mathbf{x}}_m\text{\hspace{1em}(8)}$$
对于${\mathbf{x}}_b,{\mathbf{x}}_m$一般没有先验信息，在马尔科夫毯上求解下面的 local MLE 问题：
{ x ^ b , x ^ m } = arg ⁡ max ⁡ x b , x m p ( z m ∣ x b , x m ) = arg ⁡ min ⁡ x b , x m ∑ ( i , j ) ∈ supp ⁡ ( z m ) ∥ z i j − h i j ( x i , x j ) ∥ Λ i j 2 (9) {\left\{\hat{\mathbf{x}}_{b}, \hat{\mathbf{x}}_{m}\right\}=\arg \max _{\mathbf{x}_{b}, \mathbf{x}_{m}} p\left(\mathbf{z}_{m} | \mathbf{x}_{b}, \mathbf{x}_{m}\right)=} \\ {\arg \min _{\mathbf{x}_{b}, \mathbf{x}_{m}} \sum_{(i, j) \in \operatorname{supp}\left(\mathbf{z}_{m}\right)}\left\|\mathbf{z}_{i j}-\mathbf{h}_{i j}\left(\mathbf{x}_{i}, \mathbf{x}_{j}\right)\right\|_{\Lambda_{i j}}^{2}} \tag{9} { x^b​,x^m​}=argxb​,xm​max​p(zm​∣xb​,xm​)=argxb​,xm​min​(i,j)∈supp(zm​)∑​∥zij​−hij​(xi​,xj​)∥Λij​2​(9)
公式（9）最终得到的信息矩阵为
$$\Lambda =\sum _{(i,j)\in \mathrm{supp}\left({\mathbf{z}}_m\right)}{\mathbf{H}}_{ij}^{\top }{\mathbf{\Lambda }}_{ij}{\mathbf{H}}_{ij}=:\left[\begin{array}{cc}{\mathbf{\Lambda }}_{mm}& {\mathbf{\Lambda }}_{mb}\\ {\mathbf{\Lambda }}_{bm}& {\mathbf{\Lambda }}_{bb}\end{array}\right]\text{\hspace{1em}(10)}$$
对其进行边缘化得到信息矩阵：
$${\mathbf{\Lambda }}_t={\mathbf{\Lambda }}_{bb}-{\mathbf{\Lambda }}_{bm}$$