SLAM中边缘化与一致性

文章目录

• 基于图的SLAM[^1]
• 马尔科夫毯上通过边缘化移除节点
• 边缘化后的先验分布
• 相对MLE公式
• 备注
• 边的稀疏化
• 在线确定稀疏拓扑
• 构建一致稀疏因子
• 解耦稀疏化和边缘化
• VO中的滑窗介绍[^2]
• 滑窗VO
• BA
• 旧状态边缘化
• 估计的一致性
• 物理解释
• 性能改进办法
• 举例
• SLAM中边缘化与高斯推断的边缘概率区别
• Reference

基于图的SLAM¹

使用因子图表示，节点表示状态，边表示约束，通常形式为：
$\mathbf{z}_{i j}=\mathbf{h}_{i j}\left(\mathbf{x}_{i}, \mathbf{x}_{j}\right)+\mathbf{n}_{i j} \tag1$zij​=hij​(xi​,xj​)+nij​(1)
其中${\bf h}_{ij}$hij​是状态到测量的映射，${\bf n}_{ij}$nij​是零均值白噪声
$\mathbf{n}_{i j} \sim \mathcal{N}\left(\mathbf{0}, \mathbf{\Lambda}_{i j}^{-1}\right) \tag2$nij​∼N(0,Λij−1​)(2)
这就变成了MLE问题：
$\hat{\mathbf{x}}=\arg \min _{\mathbf{x}} \sum_{(i, j) \in \operatorname{supp}(\mathbf{z})}\left\|\mathbf{z}_{i j}-\mathbf{h}_{i j}\left(\mathbf{x}_{i}, \mathbf{x}_{j}\right)\right\|_{\Lambda_{i j}}^{2} \tag3$x^=argxmin​(i,j)∈supp(z)∑​∥zij​−hij​(xi​,xj​)∥Λij​2​(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} \tag{4}$x=[xmT​​xbT​​xrT​​]T(4)
其中，${\bf x}_m$xm​表示要边缘化掉的节点，${\bf x}_b$xb​表示马尔科夫毯中与边缘化节点直接相连的节点，${\bf x}_r$xr​表示这个图中剩余的节点。

将观测划分为：
$\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)
其中${\bf z}_m$zm​是直接约束${\bf x}_m$xm​的测量，${\bf z}_b$zb​表示马尔科夫毯中所有的约束，${\bf z}_c$zc​表示马尔科夫毯中除了${\bf z}_m$zm​其他的约束，${\bf z}_r$zr​表示剩下的约束。

举例，Fig1：

$\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)
边缘化之后的节点会产生一个先验约束${\bf x}_b$xb​
$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) \tag{7}$p(xb​∣zm​)=∫xm​​p(xb​,xm​∣zm​)dxm​=:N(x^b​,Λt−1​)(7)
边缘化之后，除了由于线性化导致的误差外，所有的信息都在${\bf z}_m$zm​中，没有损失。

边缘化后的先验分布

为了得到公式（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} \tag8$maxp(xb​∣zm​)=∫xm​​maxp(xb​,xm​∣zm​)dxm​(8)
对于${\bf x}_b, {\bf x}_m$xb​,xm​一般没有先验信息，在马尔科夫毯上求解下面的 local MLE 问题：
${\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 \operatorname{supp}\left(\mathbf{z}_{m}\right)} \mathbf{H}_{i j}^{\top} \mathbf{\Lambda}_{i j} \mathbf{H}_{i j}=: \left[ \begin{array}{cc}{\boldsymbol{\Lambda}_{m m}} & {\boldsymbol{\Lambda}_{m b}} \\ {\boldsymbol{\Lambda}_{b m}} & {\boldsymbol{\Lambda}_{b b}}\end{array}\right] \tag{10}$Λ=(i,j)∈supp(zm​)∑​Hij⊤​Λij​Hij​=:[Λmm​Λbm​​Λmb​Λbb​​](10)
对其进行边缘化得到信息矩阵：
$\mathbf{\Lambda}_{t}=\mathbf{\Lambda}_{b b}-\boldsymbol{\Lambda}_{b m} \mathbf{\Lambda}_{m m}^{-1} \mathbf{\Lambda}_{b m}^{\top} \tag{11}$Λt​=Λbb​−Λbm​Λmm−1​Λbm⊤​(11)
先验信息确定之后，MLE 问题就可以转化为最小二乘问题：
$\hat{\mathbf{x}}=\arg \min _{\mathbf{x}}\left\|\hat{\mathbf{x}}_{b}-\mathbf{x}_{b}\right\|_{\Lambda_{t}}^{2}+\sum_{(i, j) \in \operatorname{supp}\left(\mathbf{z}_{r}, \mathbf{z}_{c}\right)}\left\|\mathbf{z}_{i j}-\mathbf{h}_{i j}\left(\mathbf{x}_{i}, \mathbf{x}_{j}\right)\right\|_{\Lambda_{i j}}^{2} \tag{12}$x^=argxmin​∥x^b​−xb​∥Λt​2​+(i,j)∈supp(zr​,zc​)∑​∥zij​−hij​(xi​,xj​)∥Λij​2​(12)
到这为止的理解：可以分为两步考虑

• 第一，只有节点${\bf x}_m$xm​和${\bf x}_b$xb​节点，约束是他们之间的${\bf z}_m$zm​，进行公式（9）的求解。这时得到了均值$\left\{\hat{\mathbf{x}}_{b}, \hat{\mathbf{x}}_{m}\right\}${x^b​,x^m​}，信息矩阵公式（10）。对${\bf x}_m$xm​进行边缘化，这样就得到了${\bf x}_b$xb​的先验。
• 第二，这时节点${\bf x}_r$xr​加入，进行公式（12）的优化，将之前优化得到的结果作为先验，利用剩余的约束进行优化。

*Lemma 4.1：*如果公式（10）中测量方程雅克比，以及公式（11）中的信息矩阵是 local MLE 估计的结果。那么公式（12）是原来边缘化结果的二阶最优近似。

对Lemma 4.1的证明：

可以将节点划分为：
$c(\mathbf{x})=c_{m}\left(\mathbf{x}_{m}, \mathbf{x}_{b}\right)+c_{r}\left(\mathbf{x}_{b}, \mathbf{x}_{r}\right) \tag{13}$c(x)=cm​(xm​,xb​)+cr​(xb​,xr​)(13)
因此有：
$\min _{\mathbf{x}} c(\mathbf{x})=\min _{\mathbf{x}_{b}, \mathbf{x}_{r}}\left(\min _{\mathbf{x}_{m}} c\left(\mathbf{x}_{m}, \mathbf{x}_{b}, \mathbf{x}_{n}\right)\right)=\min _{\mathbf{x}_{b}, \mathbf{x}_{r}}\left(c_{r}\left(\mathbf{x}_{b}, \mathbf{x}_{r}\right)+\min _{\mathbf{x}_{m}} c_{m}\left(\mathbf{x}_{m}, \mathbf{x}_{b}\right)\right) \tag{14}$xmin​c(x)=xb​,xr​min​(xm​min​c(xm​,xb​,xn​))=xb​,xr​min​(cr​(xb​,xr​)+xm​min​cm​(xm​,xb​))(14)
将$c_m$cm​进行二阶展开得到：
$c_{m} \simeq c_{m}\left(\hat{\mathbf{x}}_{m}, \hat{\mathbf{x}}_{b}\right)+\mathbf{g}^{T} \left[ \begin{array}{c}{\mathbf{x}_{m}-\hat{\mathbf{x}}_{m}} \\ {\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}}\end{array}\right]+\frac{1}{2} \left[ \begin{array}{c}{\mathbf{x}_{m}-\hat{\mathbf{x}}_{m}} \\ {\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}}\end{array}\right]^{T} \boldsymbol{\Lambda} \left[ \begin{array}{c}{\mathbf{x}_{m}-\hat{\mathbf{x}}_{m}} \\ {\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}}\end{array}\right] \tag{15}$cm​≃cm​(x^m​,x^b​)+gT[xm​−x^m​xb​−x^b​​]+21​[xm​−x^m​xb​−x^b​​]TΛ[xm​−x^m​xb​−x^b​​](15)
其中$\mathbf{g}=\left[ \begin{array}{c}{\mathbf{g}_{m m}} \\ {\mathbf{g}_{m b}}\end{array}\right]$g=[gmm​gmb​​]，$
\boldsymbol{\Lambda}=\left[ \begin{array}{ll}{\boldsymbol{\Lambda}{m m}} & {\boldsymbol{\Lambda}{m b}} \ {\boldsymbol{\Lambda}{b m}} & {\boldsymbol{\Lambda}{b b}}\end{array}\right]
$，线性化点是$，线性化点是[\hat{\bf x}_m, \hat{\bf x}_b]$。

对公式（15）求关于${\bf x}_m$xm​的最小值得到：
$\mathbf{x}_{m}=\hat{\mathbf{x}}_{m}-\boldsymbol{\Lambda}_{m m}^{-1}\left(\mathbf{g}_{m m}+\mathbf{\Lambda}_{m b}\left(\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}\right)\right) \tag{16}$xm​=x^m​−Λmm−1​(gmm​+Λmb​(xb​−x^b​))(16)
把公式（16）带入公式（15）得到，使用二次函数最低点公式：
$\min _{\mathbf{x}_{m}} c_{m}\left(\mathbf{x}_{m}, \mathbf{x}_{b}\right) \simeq \zeta+\mathbf{g}_{t}^{T}\left(\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}\right)+\frac{1}{2}\left(\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}\right)^{T} \boldsymbol{\Lambda}_{t}\left(\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}\right) \tag{17}$xm​min​cm​(xm​,xb​)≃ζ+gtT​(xb​−x^b​)+21​(xb​−x^b​)TΛt​(xb​−x^b​)(17)
其中$\zeta$ζ是常数项，其中：
$\begin{aligned} \mathbf{g}_{t} &=\mathbf{g}_{m b}-\boldsymbol{\Lambda}_{b m} \mathbf{\Lambda}_{m m}^{-1} \mathbf{g}_{m m} \\ \mathbf{\Lambda}_{t} &=\mathbf{\Lambda}_{b b}-\mathbf{\Lambda}_{b m} \mathbf{\Lambda}_{m m}^{-1} \mathbf{\Lambda}_{m b} \end{aligned} \tag{18}$gt​Λt​​=gmb​−Λbm​Λmm−1​gmm​=Λbb​−Λbm​Λmm−1​Λmb​​(18)
将公式（17）代入公式（14）之中就可以得到，与原问题等价的：
$\begin{aligned} c_{r}^{\prime}\left(\mathbf{x}_{b}, \mathbf{x}_{r}\right) &=\mathbf{g}_{t}^{T}\left(\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}\right)+\frac{1}{2}\left(\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}\right)^{T} \mathbf{\Lambda}_{t}\left(\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}\right) \\ &+\frac{1}{2} \sum_{(i, j) \in \operatorname{supp}\left(\mathbf{z}_{r}, \mathbf{z}_{c}\right)}\left\|\mathbf{z}_{i j}-\mathbf{h}_{i j}\left(\mathbf{x}_{i}, \mathbf{x}_{j}\right)\right\|_{\Lambda_{i j}}^{2} \end{aligned} \tag{19}$cr′​(xb​,xr​)​=gtT​(xb​−x^b​)+21​(xb​−x^b​)TΛt​(xb​−x^b​)+21​(i,j)∈supp(zr​,zc​)∑​∥zij​−hij​(xi​,xj​)∥Λij​2​​(19)
如果线性展开点是局部最小值，这样公式（19）的一阶梯度项就消失了，因为极值点处的导数为0！因此这也公式（19）就变成了公式（12）。

得证。

相对MLE公式

马尔科夫毯的测量提供的只是关于节点的相关信息，如果想得到全局状态估计，局部 MLE 问题即公式（9）是缺少约束的。为了得到完全约束，我们把相对状态估计进行重新参数化，如同得到先验一样。就是把MLE问题转移到某个节点的参考帧上进行计算。

如图Fig1所示，我们就是把该问题公式（9）转换到为相对于节点${\rm x}_0$x0​的局部参考帧上：
$\left\{^{0} \hat{\mathbf{x}}_{i}\right\}_{i=1}^{3}=\arg \max _{\left\{^{0} \mathbf{x}_{i}\right\}_{i=1}^{3}} p\left(\left.\mathbf{z}_{m}\right|^{0} \mathbf{x}_{1},^{0} \mathbf{x}_{2},^{0} \mathbf{x}_{3}\right) \tag{20}${0x^i​}i=13​=arg{0xi​}i=13​max​p(zm​∣0x1​,0x2​,0x3​)(20)
边缘化掉${\rm x}_1$x1​之后，这部分以公式（21）的形式对公式（12）的第一项有贡献。
$\left\|\left[ \begin{array}{c}{^0 \hat{\mathbf{x}}_{2}} \\ {^0 \hat{\mathbf{x}}_{3}}\end{array}\right]-\left[ \begin{array}{cc}{\mathbf{x}_{2}} {\ominus \mathbf{x}_{0}} \\ {\mathbf{x}_{3}} {\ominus \mathbf{x}_{0}}\end{array}\right]\right\|^{2}_{^{0}\Lambda_t}\tag{21}$∥∥∥∥​[0x^2​0x^3​​]−[x2​⊖x0​x3​⊖x0​​]∥∥∥∥​0Λt​2​(21)
也就是从global变换到了local帧。这个测量就是在local帧上线性化，global上非线性化。

备注

GLC方法中将参考帧转移到任意一个节点，使用的是节点的全局估计，根据Lemma4.1，说明这个不是最优的近似，导致了不一致的结果。NFR 使用局部线性化点，通过固定节点的估计找到全局帧下局部线性化点。

注意的是，边缘化子图的选择只使用与被边缘化节点直接相连的因子（约束）就可以，不需要${\rm z}_b$zb​，但是GLC和NFR为了边缘化过程有更好的稀疏性，包含了这些节点，并且在后续的优化中不进行重新的线性化。

在代码中注意，这几种的处理方式！！

边的稀疏化

边缘化破坏了稀疏性，提出了在线的稀疏拓扑求解，使用稀疏因子代替稠密测量。

在线确定稀疏拓扑

使用$\mathcal l 1$l1正则化KLD来求解稀疏信息矩阵。使用的是局部，转移到参考帧的信息矩阵$^L\Lambda _t$LΛt​。

推断测量增加到具有非零条目的节点间，另外局部先验测量也被增加到每个节点。如下图

构建一致稀疏因子

稀疏因子不仅要符合稀疏拓扑，还要和原局部目标信息一致。

推断因子有以下形式：
$\breve{\mathbf{z}}_{i j}=\breve{\mathbf{h}}_{i j}\left(\mathbf{x}_{i}, \mathbf{x}_{j}\right)+\breve{\mathbf{n}}_{i j} \tag{22}$z˘ij​=h˘ij​(xi​,xj​)+n˘ij​(22)
其中噪声的信息矩阵需要确定。

首先，构建公式（22）的期望值和相应的雅克比，使用$^L\hat {\bf x}$Lx^作为线性化点。即
$\hat{\breve {\bf z}}_{ij} = \breve{\bf h}_{ij}(^L\hat{\rm x}_i, ^L\hat{\rm x}_j),\ \text and \ \breve {\bf H}_{ij}=\frac{\partial \breve{\bf h}_{ij}}{\partial {\bf x}}|_{{\bf x}=^L\hat{\bf x}}\tag{23}$z˘^ij​=h˘ij​(Lx^i​,Lx^j​), and H˘ij​=∂x∂h˘ij​​∣x=Lx^​(23)
我们希望新的测量不比原因子包含额外的信息，是一致的。使用最小化KLD来求解。使得稀疏化之后，不会变得过度自信（协方差变小）。

解耦稀疏化和边缘化

边缘化之后推迟稀疏化，先边缘化再稀疏化。

• 直到稀疏化之前，剩余节点的信息都包含在稠密因子中，可以重用。
• 稀疏化之前可以重新线性化内部的非线性化测量，保证精度。
• 交错边缘化和稀疏化，分配计算量。

总结，首先在马尔科夫毯上进行局部相对优化，并计算边缘分布。然后进行边的稀疏化，找到新的稀疏因子代替稠密的内部因子。

---

VO中的滑窗介绍²

滑窗的方式在计算量和精度进行了折中，在边缘化之后如何对丢弃状态的不确定性进行建模是精度的关键。标准边缘化导致了不一致性，同一状态的不同估计被用来了计算雅克比矩阵。这导致了把不是测量实际提供的信息计算出来的状态空间方向加入到了状态中，就是沿着不可观测方向前进了。这进而导致了估计的协方差被错误估计，变得不一致，精度不准确。

提出一种方法，保证计算雅克比时，只有一种估计被利用。

一致性： A recursive estimator is termed consistent when the state estimation errors are zero mean, and their covariance equals the one reported by the estimator

滑窗VO

BA

相机相对于全局参考帧的姿态：
$\mathbf{c}_{i}=\left[ \begin{array}{c}{\mathbf{q}_{C_{i}}} \\ {\mathbf{p}_{C_{i}}}\end{array}\right] \tag{24}$ci​=[qCi​​pCi​​​](24)
观测模型：
$\mathbf{z}_{i j}=\mathbf{h}\left(\mathbf{C}\left(\mathbf{q}_{C_{i}}\right)\left(\mathbf{p}_{L_{j}}-\mathbf{p}_{C_{i}}\right)\right)+\mathbf{n}_{i j} \tag{25}$zij​=h(C(qCi​​)(pLj​​−pCi​​))+nij​(25)
其中$\mathbf{n}_{i j} \sim \mathcal{N}\left(\mathbf{0}_{2 \times 1}, \mathbf{R}_{i j}\right)$nij​∼N(02×1​,Rij​).

优化的概率密度函数为：
$p\left(\mathbf{x}_{k} | \mathbf{z}_{0 : k}\right)=p\left(\mathbf{x}_{k}\right) \prod_{(i, j) \in \mathcal{S}_{a}(k)} p\left(\mathbf{z}_{i j} | \mathbf{c}_{i}, \mathbf{p}_{L_{j}}\right) \tag{26}$p(xk​∣z0:k​)=p(xk​)(i,j)∈Sa​(k)∏​p(zij​∣ci​,pLj​​)(26)
其中$p({\bf x }_k)$p(xk​)是状态先验信息的，例如第一帧添加全局尺度信息。建模为：$\mathbf{f}\left(\mathbf{x}_{k}\right) \sim \mathcal{N}\left(\hat{\mathbf{f}}, \mathbf{R}_{p}\right)$f(xk​)∼N(f^,Rp​)

公式（26）变成最小化cost function
$c\left(\mathbf{x}_{k}\right)=\frac{1}{2}\left\|\mathbf{f}\left(\mathbf{x}_{k}\right)-\hat{\mathbf{f}}\right\|_{\mathbf{R}_{p}}+\frac{1}{2} \sum_{(i, j) \in \mathcal{S}_{a}(k)}\left\|\mathbf{z}_{i j}-\mathbf{h}\left(\mathbf{c}_{i}, \mathbf{p}_{L_{j}}\right)\right\|_{\mathbf{R}_{i j}} \tag{27}$c(xk​)=21​∥∥∥​f(xk​)−f^∥∥∥​Rp​​+21​(i,j)∈Sa​(k)∑​∥∥​zij​−h(ci​,pLj​​)∥∥​Rij​​(27)
求解：
$\mathbf{A}^{(\ell)} \Delta \mathbf{x}^{(\ell)}=-\mathbf{b}^{(\ell)} \tag{28}$A(ℓ)Δx(ℓ)=−b(ℓ)(28)
其中
$\mathbf{A}^{(\ell)}=\mathbf{F}^{T} \mathbf{R}_{p}^{-1} \mathbf{F}+\sum_{(i, j) \in \mathcal{S}_{a}(k)} \mathbf{H}_{i j}^{(\ell) T} \mathbf{R}_{i j}^{-1} \mathbf{H}_{i j}^{(\ell)}\tag{29}$A(ℓ)=FTRp−1​F+(i,j)∈Sa​(k)∑​Hij(ℓ)T​Rij−1​Hij(ℓ)​(29)

$\mathbf{b}^{(\ell)}=\mathbf{F}^{T} \mathbf{R}_{p}^{-1}\left(\mathbf{f}\left(\mathbf{x}_{k}^{(\ell)}\right)-\hat{\mathbf{f}}\right)-\sum_{(i, j) \in S_{a}(k)} \mathbf{H}_{i j}^{(\ell) T} \mathbf{R}_{i j}^{-1}\left(\mathbf{z}_{i j}-\mathbf{h}\left(\mathbf{c}_{i}^{(\ell)}, \mathbf{p}_{L_{j}}^{(\ell)}\right)\right) \tag{30}$b(ℓ)=FTRp−1​(f(xk(ℓ)​)−f^)−(i,j)∈Sa​(k)∑​Hij(ℓ)T​Rij−1​(zij​−h(ci(ℓ)​,pLj​(ℓ)​))(30)

雅克比矩阵为
${\bf H}^{(\ell)}_{ij}=\left[ \begin{array}{c} 0 &... &{\bf H}_{L_{ij}}({\bf x^{(\ell)}_k}) &... &{\bf H}_{C_{ij}}({\bf x^{(\ell)}_k}) &... & 0 \end{array}\right]\tag{31}$Hij(ℓ)​=[0​...​HLij​​(xk(ℓ)​)​...​HCij​​(xk(ℓ)​)​...​0​](31)
其中对相机位姿和点位置的雅克比:
$\begin{aligned} \mathbf{H}_{C_{i j}}\left(\mathbf{x}_{k}\right)&=\mathbf{\Gamma}_{i j} \mathbf{C}\left(\mathbf{q}_{C_{i}}\right)\left[\left\lfloor\left(\mathbf{p}_{L_{j}}-\mathbf{p}_{C_{i}}\right) \times\right\rfloor \mathbf{C}^{T}\left(\mathbf{q}_{C_{i}}\right) \quad-\mathbf{I}_{3}\right] \\ \mathbf{H}_{L_{i j}}\left(\mathbf{x}_{k}\right)&=\mathbf{\Gamma}_{i j} \mathbf{C}\left(\mathbf{q}_{C_{i}}\right) \\ \mathbf{\Gamma}_{i j}&=\left.\frac{\partial \mathbf{h}(\mathbf{p})}{\partial \mathbf{p}}\right|_{\mathbf{p}=\mathbf{C}\left(\mathbf{q}_{C_{i}}\right)\left(\mathbf{p}_{L_{j}}-\mathbf{p}_{C_{i}}\right)} \end{aligned} \tag{32}$HCij​​(xk​)HLij​​(xk​)Γij​​=Γij​C(qCi​​)[⌊(pLj​​−pCi​​)×⌋CT(qCi​​)−I3​]=Γij​C(qCi​​)=∂p∂h(p)​∣∣∣∣​p=C(qCi​​)(pLj​​−pCi​​)​​(32)

这里求导需要会,使用了$(Rp)^\wedge =Rp^\wedge R^T$(Rp)∧=Rp∧RT

旧状态边缘化

要边缘化的旧的状态:
$\mathbf{x}_{\mathbf{m}}=\left\{\mathbf{c}_{0}, \dots, \mathbf{c}_{m-1}, \mathbf{p}_{L_{1}}, \dots, \mathbf{p}_{L_{q}}\right\} \tag{33}$xm​={c0​,…,cm−1​,pL1​​,…,pLq​​}(33)
滑窗中的活动状态:
$\mathbf{x}_{\mathbf{r}}=\left\{\mathbf{c}_{m}, \dots, \mathbf{c}_{k}, \mathbf{p}_{L_{q+1}}, \cdots, \mathbf{p}_{L_{n}}\right\} \tag{34}$xr​={cm​,…,ck​,pLq+1​​,⋯,pLn​​}(34)
边缘化:
$\mathbf{b}_{\mathbf{p}}(k)=\mathbf{b}_{\mathbf{m} \mathbf{r}}(k)-\mathbf{A}_{\mathbf{r m}}(k) \mathbf{A}_{\mathbf{m} \mathbf{m}}(k)^{-1} \mathbf{b}_{\mathbf{m} \mathbf{m}}(k) \tag{35}$bp​(k)=bmr​(k)−Arm​(k)Amm​(k)−1bmm​(k)(35)

$\mathbf{A}_{\mathbf{p}}(k)=\mathbf{A}_{\mathbf{r r}}(k)-\mathbf{A}_{\mathbf{r m}}(k) \mathbf{A}_{\mathbf{m} \mathbf{m}}(k)^{-1} \mathbf{A}_{\mathbf{m} \mathbf{r}}(k) \tag{36}$Ap​(k)=Arr​(k)−Arm​(k)Amm​(k)−1Amr​(k)(36)

这里
$\begin{aligned} \mathbf{b}_{\mathbf{m}}(k) &=\left[ \begin{array}{c}{\mathbf{b}_{\mathbf{m m}}(k)} \\ {\mathbf{b}_{\mathbf{m r}}(k)}\end{array}\right] \\=& \mathbf{F}^{T} \mathbf{R}_{p}^{-1}\left(\mathbf{f}\left(\hat{\mathbf{x}}_{k}(k)\right)-\hat{\mathbf{f}}\right) \\ &-\sum_{(i, j) \in \mathcal{S}_{m}} \mathbf{H}_{i j}^{T}(k) \mathbf{R}_{i j}^{-1}\left(\mathbf{z}_{i j}-\mathbf{h}\left(\hat{\mathbf{c}}_{i}(k), \hat{\mathbf{p}}_{L_{j}}(k)\right)\right) \end{aligned} \tag{37}$bm​(k)=​=[bmm​(k)bmr​(k)​]FTRp−1​(f(x^k​(k))−f^)−(i,j)∈Sm​∑​HijT​(k)Rij−1​(zij​−h(c^i​(k),p^​Lj​​(k)))​(37)

$\begin{aligned} \mathbf{A}_{\mathbf{m}}(k) &=\left[ \begin{array}{cc}{\mathbf{A}_{\mathbf{m} \mathbf{m}}(k)} & {\mathbf{A}_{\operatorname{mr}}(k)} \\ {\mathbf{A}_{\mathbf{r m}}(k)} & {\mathbf{A}_{\mathbf{r r}}(k)}\end{array}\right] \\ &=\mathbf{F}^{T} \mathbf{R}_{p}^{-1} \mathbf{F}+\sum_{(i, j) \in \mathcal{S}_{m}} \mathbf{H}_{i j}(k) \mathbf{R}_{i j}^{-1} \mathbf{H}_{i j}(k) \end{aligned} \tag{38}$Am​(k)​=[Amm​(k)Arm​(k)​Amr​(k)Arr​(k)​]=FTRp−1​F+(i,j)∈Sm​∑​Hij​(k)Rij−1​Hij​(k)​(38)

注意上面的矩阵都是针对要被边缘化的状态$\mathcal S_m$Sm​，因为有共视关系所以有$r$r状态。

${\bf A_p}(k)$Ap​(k)代表了提供给估计${\bf x_r}$xr​的所有先验和被抛弃的测量信息。

之后继续运行到$k'$k′时刻，包含了$\bf x_r, x_n$xr​,xn​状态，进行优化第$\ell$ℓ次迭代：
${\mathbf{b}^{(\ell)}=\mathbf{\Pi}_{\mathbf{r}}^{T} \mathbf{b}_{\mathbf{p}}(k)+\mathbf{\Pi}_{\mathbf{r}}^{T} \mathbf{A}_{\mathbf{p}}(k)\left(\mathbf{x}_{\mathbf{r}}^{(\ell)}-\hat{\mathbf{x}}_{\mathbf{r}}(k)\right)} \\ {-\sum_{(i, j) \in \mathcal{S}_{a}\left(k^{\prime}\right)} \mathbf{H}_{i j}^{(\ell) T} \mathbf{R}_{i j}^{-1}\left(\mathbf{z}_{i j}-\mathbf{h}\left(\mathbf{c}_{i}^{(\ell)}, \mathbf{p}_{L_{j}}^{(\ell)}\right)\right)} \tag{39}$b(ℓ)=ΠrT​bp​(k)+ΠrT​Ap​(k)(xr(ℓ)​−x^r​(k))−(i,j)∈Sa​(k′)∑​Hij(ℓ)T​Rij−1​(zij​−h(ci(ℓ)​,pLj​(ℓ)​))(39)

$\mathbf{A}^{(\ell)}=\Pi_{\mathbf{r}}^{T} \mathbf{A}_{\mathbf{p}}(k) \Pi_{\mathbf{r}}+\sum_{(i, j) \in \mathcal{S}_{a}\left(k^{\prime}\right)} \mathbf{H}_{i j}^{(\ell) T} \mathbf{R}_{i j}^{-1} \mathbf{H}_{i j}^{(\ell)} \tag{40}$A(ℓ)=ΠrT​Ap​(k)Πr​+(i,j)∈Sa​(k′)∑​Hij(ℓ)T​Rij−1​Hij(ℓ)​(40)

公式（39）（40）优化的就是状态$\bf x_r, x_n$xr​,xn​，其中$\boldsymbol{\Pi}_{\mathbf{r}}=\left[\mathbf{I}_{\operatorname{dim} \mathbf{x}_{\mathbf{r}}} \mathbf{0}\right]$Πr​=[Idimxr​​0]

然后重复之前的边缘化步骤。

估计的一致性

证明了特征测量的信息矩阵的秩错误的增加了。

普通的BA中在$[0,k']$[0,k′]内的信息矩阵
$\mathbf{J}_{\mathrm{ba}}\left(k^{\prime}\right)=\sum_{(i, j) \in \mathcal{S}} \mathbf{H}_{i j}^{T}\left(k^{\prime}\right) \mathbf{R}_{i j}^{-1} \mathbf{H}_{i j}\left(k^{\prime}\right) \tag{41}$Jba​(k′)=(i,j)∈S∑​HijT​(k′)Rij−1​Hij​(k′)(41)
这里的雅克比线性展开点在$K'$K′

滑窗算法中的信息矩阵，要包含与公式（41）相同的项，使用相同的测量。
$\mathbf{J}_{\operatorname{mar}}\left(k^{\prime}\right)=\sum_{(i, j) \in \mathcal{S}_{m}} \mathbf{H}_{i j}^{T}(k) \mathbf{R}_{i j}^{-1} \mathbf{H}_{i j}(k)+\sum_{(i, j) \in \mathcal{S}_{a}\left(k^{\prime}\right)} \mathbf{H}_{i j}^{T}\left(k^{\prime}\right) \mathbf{R}_{i j}^{-1} \mathbf{H}_{i j}\left(k^{\prime}\right) \tag{42}$Jmar​(k′)=(i,j)∈Sm​∑​HijT​(k)Rij−1​Hij​(k)+(i,j)∈Sa​(k′)∑​HijT​(k′)Rij−1​Hij​(k′)(42)
这里的雅克比线性展开点，前半部分在$k$k，后半部分在$k'$k′。这两个不同状态使得某些可以消去的项变得不行。因此两个矩阵的秩变得不一样。
$\operatorname{rank}\left(\mathbf{J}_{\operatorname{mar}}\left(k^{\prime}\right)\right)=\operatorname{rank}\left(\mathbf{J}_{\mathrm{ba}}\left(k^{\prime}\right)\right)+3 \tag{43}$rank(Jmar​(k′))=rank(Jba​(k′))+3(43)
这使得信息矩阵包含了更多的信息（沿着状态空间的更多方向），因此低估了状态的不确定性。使得该问题变得不一致且是次优的。其它方式的边缘化也有这个问题。

公式（43）证明，参考论文²中的3.1使用双目作为例子。

物理解释

正常的零空间（不可观的状态）是6 个维度，是全局旋转和全局平移。但是边缘化发生后，零空间缺少了3个维度，是全局的角度。这使得全局方向变得可观。

性能改进办法

出现这个问题的主要原因是，在测量雅克比中出现了两个不同的某些状态估计。特别是某些边缘化掉的测量和当前剩余状态$\bf x_r$xr​相连。例如，相机位姿在边缘化后留在了滑窗中，但是其观测的某个点在$k$k时刻被边缘化掉。这样它的信息矩阵就会出现在公式（36）中，而当我们在$k'$k′时刻对该相机位姿进行优化使用的是$k'$k′的状态计算雅克比。

为了解决这个问题，只能改变用于计算雅克比的状态估计。**在当前状态和边缘化状态相连时，使用边缘化时刻的状态来计算后面的雅克比。**这样保证了信息矩阵只有一个状态。注意，旧的状态只用来计算雅克比。

诚然，这样导致了线性展开的误差，因此使用时还是要保证在较好的状态下。

比较有意思的一个公式：
$\operatorname{rank}(\mathbf{A B})=\operatorname{rank}(\mathbf{B})-\operatorname{dim}(\mathcal{N}(\mathbf{A}) \bigcap \mathcal{R}(\mathbf{B})) \tag{44}$rank(AB)=rank(B)−dim(N(A)⋂R(B))(44)

举例

对应的信息矩阵变化为：

第1个矩阵是原始矩阵，对应第一张因子图。将要边缘化的相机状态移动到左上角之后，将其边缘化，对应着第二张因子图。矩阵有2变3了，可以发现它使得矩阵变得稠密了，有3行都出现fill-in了。之后边缘化掉点m1，对应第三张因子图。矩阵由3变成4了。通过这次变换，发现矩阵没有变得稠密。

因此，边缘化要尽量边缘化掉那些不被其它帧观测到的点，观测到的点不边缘化或者直接丢弃。那么问题来了，没观测到的点提供的先验信息不就很少了吗？

利用这张图³可以对公式（42）进行解释，第一个是原始的两个非线性函数组成的能量函数图，深蓝色部分是他的零空间。第二三个是在两个不同的展开点分别进行线性展开。最后加在一起得到第四个，发现原来的零空间变小了，不确定变得确定了，不可观变得可观了。和之前的解释一样。

SLAM中边缘化与高斯推断的边缘概率区别

高斯推断中边缘概率求解，可以发现，是先对协方差矩阵进行LDU分解，然后再求逆的。

而对于SLAM中的边缘化，从公式（11）（36）都可以看出来是直接对信息矩阵进行的分解消元，也就是先求逆再分解。结果是不一样的。它们都使用了舒尔补，具体有啥不同呢，还得再研究。

参考PRML的2.3.1和2.3.2可以明显的发现，边缘化和求条件概率是不一样的！！！

再说一句，边缘化是把另一个变量当做变量积分掉，求条件概率是把另一个变量当做常数忽略掉。

Reference

---

1. Decoupled, Consistent Node Removal and Edge Sparsification for Graph-based SLAM ↩︎

2. Consistency Analysis for Sliding-Window Visual Odometry ↩︎ ↩︎

3. https://blog.csdn.net/heyijia0327/article/details/52822104 ↩︎