后端-1

概述

状态估计的概率介绍

SLAM过程可由运动方程和观测方程来描述。那么，假设在t=0到t=N的时间内，有$x_0$到$x_N$那么多个位姿，并且有$y_1,...,y_M$那么多个路标。运动和观测方程为：

$$\begin{cases} x_k=f(x_{k-1},u_k)+w_k\\ z_{k,j}=h(y_j,x_k)+v_{k,j} \end{cases} \quad\quad k=1,...,N,j=1,...,M$$

注意：

1. 观测方程中，只有当$x_k$看到$y_j$时，才会产生观测数据，否则就没有。事实上，在一个位置上通常只能观测到一小部分路标。而且，由于视觉SLAM特征点众多，所以实际当中观测方程数量远远大于运动方程数量。
2. 可能没有测量运动的装置，所以也有可能没有运动方程。在这种情况下，有若干种处理方式：认为确实没有运动方程，或假设相机不动，或者假设相机匀速运动。这几种方式都是可行的。在没有运动运动方程的情况下，整个优化问题就只由许多观测方程组成。这种就非常类似于SfM(structure from motion)问题，就相当于我们通过一组图像恢复运动和结构。与SfM不同的是，SLAM中的图像有时间上的先后顺序，而SfM允许使用完全无关的图像。
   由于位姿和路标点都是待估计的变量，改变一下记号，令$x_k$为k时刻所有的未知量。它包含当前时刻的相机位姿和m个路标点。在这种记号的意义下，写成：
   $$x_k \triangleq \{x_k,y_1,\cdots,y_m \}$$

同时，把k时刻的所有观测基座$z_k$。于是，运动方程与观测方程的形式更加简洁。

$$\begin{cases} x_k=f(x_{k-1},u_k)+w_k\\ z_{k}=h(x_k)+v_{k,j} \end{cases}$$
现在考虑第k时刻的情况。我们希望通过0到k时刻中的数据，来估计现在的状态分布：
$$P(x_k|x_0,u_{1:k},z_{1:k})$$
下表0:k表示0时刻到k时刻的所有数据。按照Bayes法则，把 $z_k$与 $x_k$交换位置，有：
$$P(x_k|x_0,u_{1:k},z_{1:k}) \propto P(z_k|x_k))P(x_k|x_0,u_{1:k},z_{1:k-1})$$
第一项成为似然，第二项称为先验。似然由观测方程给出，而先验部分， $x_k$是基于所有过去状态估计得来的。至少它会受 $x_{k-1}$影响，于是按照 $x_{k-1}$时刻条件概率展开：
$$P(x_k|x_0,u_{1:k},z_{1:k−1})=\int{P(x_k|x_{k-1},u_{1:k},z_{1:k−1})P(x_{k-1}|x_{0},u_{1:k},z_{1:k−1})}dx_{k-1}$$
如果我们考虑更久之前的状态，也可以继续对此式进行展开，但现在我们只关心k时刻和k-1时刻的情况。后续处理，存在若干种选择：其一是假设马尔可夫性，简单的一阶马氏性认为，k时刻状态只与k-1时刻状态有关，而与再之前的无关。如果做出这样的假设，我们就会得到以扩展卡尔曼滤波(EKF)为代表的滤波器方法。在滤波方法中，我们会从某时刻的状态估计，推导到下一时刻。另一种方法是依然考虑k时刻状态与之前所有状态的关系，此时将得到非线性优化为主题的优化框架。

线性系统和KF

当我们假设了马尔可夫性，从数学角度会发生哪些变化呢？

$${P(x_k|x_{k-1},u_{1:k},z_{1:k−1})}={P(x_k|x_{k-1},u_{k})}$$
$$P(x_{k-1}|x_{0},u_{1:k},z_{1:k−1})=P(x_{k-1}|x_{0},u_{1:k-1},z_{1:k−1})$$

我们从形式最简单的线性高斯系统开始，最后会得到卡尔曼滤波器。线性高斯系统是说，运动方程和观测方程可以由线性方程来描述：

$$\begin{cases} x_k=A_kx_{k-1}+u_k+w_k\\ z_{k}=C_kx_k+v_{k} \end{cases} \quad k=1,\cdots,N$$
并假设所有的状态和噪声均满足高斯分布。记这里的噪声服从零均值高斯分布：
$$w_k \sim N(0,R).\quad v_k \sim N(0,Q)$$

现在，利用马尔可夫性，假设我们知道了k -1时刻的后验（在 k - 1 时刻看来）状态估计：$\hat x_{k-1}$和它的协方差 $\hat P_{k-1}$，现在要根据 k 时刻的输入和观测数据，确定$x_k$ 的后验分布。为区分推导中的先验和后验，我们在记号上作一点区别：以尖帽子$\hat x_k$表示后验，以横线$\overline x_k$ 表示先验分布。
卡尔曼滤波器的第一步，通过运动方程确定$x_k$ 的先验分布。根据高斯分布的性质，显然有：

$$P(x_k|x_0,u_{1:k},z_{1:k−1})=N(A_k\hat x_{k-1}+u_k,A_k\hat P_{k-1}A^T_k+R)$$
这一步成为预测，它显示了如何从上一时刻的状态，根据输入信息，推测当前时刻的状态的分布。这个分布也就是先验。记这里的：
$$\overline x_k=A_k\hat x_{k-1}+u_k\quad \overline P_k=A_k\hat P_{k-1}A^T_k+R$$

另一方面，由观测方程，我们可以计算在某个状态下，应该产生怎样的观测数据：

$$P(z_k|x_k)=N(C_kx_k,Q)$$
为了得到后验概率，我们想要计算它们的乘积。虽然我们知道最后得到一个关于 $x_k$的高斯分布，但是计算上很麻烦，我们先把结果设为 $x_k \sim N(\hat x_k,\hat P_k)$，那么：
$$N(\hat x_k,\hat P_k)=N(C_kx_k,Q).N(\overline x_k,\overline P_k)$$
这里我们稍微用点讨巧的方法。既然我们已经知道等式两侧都是高斯分布，那就只需比较指数部分即可，而无须理会高斯分布前面的因子部分。指数部分很像是一个二次型的配方，我们来推导一下。首先把指数部分展开，有：
$$(x_k-\hat x_k)^T\hat P^{-1}_k(x_k-\hat x_k)=(z_k-C_kx_k)^TQ^{-1}(z_k-C_kx_k)+(x_k-\overline x_k)^TP^{-1}_k(x_k-\overline x_k)$$
为了求左侧的 $\hat x_k$ 和 $\hat P_k$，我们把两边展开，并比较 xk 的二次和一次系数。对于二次系数，有：
$$\hat P_k^{-1}= C_k^TQ^{-1}C_k+\overline P_k^{-1}$$
该式给出了协方差的计算过程。为了便于后边列写式子，定义一个中间变量：
$$K=\hat P_kC_k^TQ^{-1}$$
则
$$\hat P_k\hat P_k^{-1}=I=\hat P_kC_k^TQ^{-1}C_k+\hat P_k\overline P_k^{-1}=KC_k+\hat P_k\overline P_k^{-1}$$
于是有：
$$\hat P_k=(I-KC_k)\overline P_k$$
然后再比较一次项的系数，有：
$$-2\hat x_k^T\hat P_k^{-1}x_k=-2z_k^TQ^{-1}C_kx_k-2\overline x_k^T\overline P_k^{-1}x_k$$
整理得：
$$\hat P_k^{-1}\hat x_k=C_k^TQ^{-1}z_k+\overline P_k^{-1}\overline x_k$$
两侧乘以 $\hat P_k$得：
$$\hat x_k=\hat P_kC_k^TQ^{-1}z_k+\hat P_k\overline P_k^{-1}\overline x_k\\ =Kz_k+(I-KC_k)\overline x_k\\ =\overline x_k+K(z_k-C_k\overline x_k)$$
总结为预测和更新两步：

1. 预测： $$\overline x_k=A_k\hat x_{k-1}+u_k\quad \overline P_k=A_k\hat P_{k-1}A^T_k+R$$
2. 更新：先计算K，它又称为卡尔曼增益： $$K=\overline P_kC_k^T(C_k\overline P_kC_k^T+Q)^{-1}$$ 然后就算后验概率的分布： $$\hat x_k=\overline x_k+K(z_k-C_k\overline x_k)$$ $$\hat P_k=(I-KC_k)\overline P_k$$

卡尔曼滤波器构成了线性系统的最优无偏估计。

非线性系统和EKF

SLAM中的运动方程和观测方程通常是非线性函数，尤其是视觉中的相机模型，需要使用相机内参模型以及李代数表示位姿，更不可能是一个线性系统。一个高斯分布，经过非线性变换后，往往不再是高斯分布，所以在非线性系统中，我们必须取得一定的近似，将一个非高斯的分布近似成一个高斯分布。把卡尔曼滤波器的结果拓展到非线性系统中来，称为扩展卡尔曼滤波器（Extended Kalman Filter，EKF）。通常的做法是，在某个点附近考虑运动方程和观测方程的一阶泰勒展开，只保留一阶项，即线性部分，然后按照线性系统进行推导。令k-1时刻的均值和协方差矩阵为$\hat x_{k-1},\hat P_{k-1}$。在k时刻，我们把运动方程和观测方程，在$\hat x_{k-1},\hat P_{k-1}$处进行线性化（相当于一阶泰勒展开），有：

$$x_k\approx f(\hat x_{k-1},u_k)+\frac{\partial f}{\partial x_{k-1}}|_{\hat x_{k-1}}(x_{k-1}-\hat x_{k-1})+w_k$$
记这里的偏导数：
$$F=\frac{\partial f}{\partial x_{k-1}}|_{\hat x_{k-1}}$$
同样的，对于观测方程：
$$z_k\approx h(\overline x_k)+\frac {\partial h}{\partial x_k}|_{\overline x_k}+n_k$$
记这里的偏导数
$$H=\frac {\partial h}{\partial x_k}|_{\overline x_k}$$
预测步骤中，根据运动方程有
$$P(x_k|x_0,u_{1:k},z_{0:k-1})=N(f(\hat x_{k-1},u_k),F\hat P_{k-1}F^T+R_k)$$
这里的先验和协方差为
$$\overline x_k=f(\hat x_{k-1},u_k), \quad \overline P_k=F\hat P_{k-1}F^T+R_k$$
考虑观测中，我们有
$$P(z_k|x_k)=N(h(\overline x_k)+H(x_k-\overline x_k),Q_k)$$
根据Bayes展开式，可以推导 $x_k$的后验概率形式。
我们先定义一个卡尔曼增益 $K_k$：
$$K_k=\overline P_kH^T(H\overline P_kH^T+Q_k)^{-1}$$
在卡尔曼增益的基础上，后验概率的形式为：
$$\hat x_k=\overline x_k+K_k(z_k-h(\overline x_k)),\quad \hat P_k=(I-K_kH)\overline P_k$$
在线性系统和高斯噪声情况下，卡尔曼滤波给出了无偏最优估计。而在SLAM这种非线性的情况下，他给出了单次线性近似下最大后验估计(MAP)。

EKF的讨论

非线性优化比滤波器占有明显的优势，但是在计算资源受限，带估计量比较简单的场合，EKF仍不失为一种有效的方式。
EKF有哪些局限性？

1. 首先，滤波器方法在一定程度上假设了马尔可夫性，也就是k时刻的状态只与k-1时刻相关，而与k-1之前的状态与观测无关（或者和前几个有限时间的状态相关）。这里有点类似于视觉里程计，只考虑相邻两帧关系一样。如果当前帧真的与很久之前的数据相关，那么滤波器很难处理这种情况。而非线性优化则倾向于使用所有的历史数据。它不光考虑临近时刻的特征点与轨迹关系，更会吧考虑很久之前的状态考虑进来，称为全体时间上的SLAM（FULL SLAM）。在这种意义下，非线性优化算法使用更多的信息，当然也需要更多的计算。
2. EKF滤波器仅在$\hat x_{k-1}$出做了一次线性化，存在非线性误差。
3. 从程序实现上来说，EKF需要存储状态量的均值和方差，并对他们进行维护和更新。如果把路标也放进状态的话，由于视觉SLAM中路标数量很大，这个数量是相当大的，且与状态呈平方增长。因此EKF SLAM被普遍认为不可适用于大型场景。

BA与图优化

Bundle Adjustment，是指从视觉重建中提取最优的3D模型和相机参数（内参数和外参数）。从每一个特征点反射出来的技术光线，在我们把相机姿态和特征点空间位置做出最优的调整后，最后收束到相机光心的过程，我们简称BA。
在以图优化框架的视觉SLAM算法中，BA起到了核心作用。它类似于求解只有观测方程的SLAM问题。

投影模型和BA代价函数

从一个世界坐标系中的点$p$出发，把相机的内外参数和畸变都考虑进来，最后投影成像素坐标，一共需要以下几步：

1. 首先，把世界坐标转换到相机坐标，这里将用到相机外参数$(R,t)$：
   $$P^{'}=Rp+t=[X^{'},Y^{'},Z^{'}]^T.$$
   • 然后将$P^{'}$投至归一化平面，得到归一化坐标： $$P_c=[u_c,v_c,1]^T=[X^{'}/Z^{'},Y^{'}/Z^{'},1]^T$$
   • 对归一化坐标去畸变，得到几畸变后坐标。这里暂时只考虑镜像畸变： $$\begin {cases} u_c^{'}=u_c(1+k_1r_c^2+k_2r_c^4)\\v_c^{'}=v_c(1+k_1r_c^2+k_2r_c^4) \end{cases}$$
   • 最后，根据内参模型，计算相机坐标： $$\begin{cases} u_s=f_xu_c^{'}+c_x\\v_s=f_yv_c^{'}+c_y\end{cases}$$
     这一系列过程的流程图，如下：
     
     这个过程也就是前面讲的观测方程，之前把他抽象的记成：
     $$z=h(x,y)$$
     现在给出他的详细参数化过程。具体的说，这里的x指代此时相机的位姿，即外参$R,t$,它对应的李代数为$\xi$.路标$y$即这里的三维点$p$,而观测数据则是像素坐标$z \triangleq [u_s,v_s]^T$。以最小二乘的角度来考虑，可以列写此次观测的误差： $$e=z-h(\xi,p)$$
     然后，把其他时刻的观测量也考虑进来，给误差添加一个下标。设$z_{ij}$为位姿$\xi_i$处观察路标$p_j$产生的数据，那么整体的代价函数（Cost Function）为：
     $$\frac 12 \sum_{i=1}^m\sum_{j=1}^n||z_{ij}-h(\xi_i,p_j)||^2.$$
     对这个最小二乘进行求解，相当于对位姿和路标同时作了调整，也就是所谓的BA。

   • BA的求解

     观察观测模型$h(\xi,p)$,其不是线性函数，所以我们通过一些非线性手段来优化他。根据非线性优化的思想，我们应该从某个初始值开始，不断的寻找下降方向$\triangle x$来找到目标函数的最优解，即不断地求解增量方程中的增量$\triangle x$。尽管误差项都是针对单个位姿和路标点的，但在整体BA目标函数上必须把自变量定义成所有待优化的变量：
     

     $$x=[\xi_1,\cdots,\xi_m,p_1,\cdots,p_n]^T.$$
     相应的，增量方程中的 $\triangle x$则是对整体自变量的增量。在这个意义下，当我们给出自变量的一个增量时，目标函数变为：
     $$\frac12||f(x+\triangle x)||^2\approx \frac12\sum_{i=1}^m\sum_{j=1}^n||e_{ij}+F_{ij}\triangle\xi_i+E_{ij}\triangle p_j||^2$$
     其中， $F_{ij}$表示整个代价函数在当前状态下对相机姿态的偏导数，而 $E_{ij}$标识该函数对路标点位置的偏导。
     把相机位姿点放在一起：
     $$x_c=[\xi_1,\xi_2,\cdots,\xi_m]^T\in R^{6m},$$
     把空间点的变量也放在一起：
     $$x_p=[p_1,p_2,\cdots,p_n]^T \in R^{3n}$$
     那么可以得到如下
     $$\frac12||f(x+\triangle x)||^2= \frac12||e+F\triangle x_c+E\triangle x_p||^2$$
     需要注意的是，该式从一个由很多小型二次型之和，变成了一个更整体的样子。这里的雅克比矩阵 $E$和$F$必须是整体目标函数对整体变量的导数，它将是一个很大块的矩阵，而里头的每个小分块，需要由每个误差项的导数 $F_{ij}$和 $E_{ij}$“拼凑起来”，然后无论我们使用 $G-N$还是 $L-M$方法，最后都将面对增量线性方程：
     $$H\triangle x=g$$
     我们知道 $G-N$和 $L-M$的主要区别在于，这里的 $H$是取$J^TJ$还是 $J^TJ+\lambda I$的形式。由于我们把变量归类成位姿和空间点两种，所以雅克比矩阵可以分块为：
     $$J=[F\quad E]$$
     那么以 $G-N$为例，则 $H$矩阵为：
     $$H=J^TJ=\begin{bmatrix} F^TF&F^TE\\ E^TF&E^TE \end{bmatrix}$$
     当然 $L-M$中我们也需要计算这个矩阵，不难发现，因为考虑了所有的优化变量，这个线性方程的维数将非常大，包含了所有的相机位姿和路标点。尤其是在视觉SLAM中，一个图像就会提取数百个特征点，大大增加了这个线性方程的规模。如果直接对 $H$求逆来计算增量方程，由于矩阵求逆是复杂度为$O(n^3)$的操作，这是非常消耗计算资源的。幸运的是，这里的 $H$矩阵是有一定特殊结构的。利用这个特殊结构我们可以加速求解过程。

     稀疏性和边缘化

     $H$矩阵的稀疏性是由雅克比 $J(x)$引起的。考虑这些代价函数中的其中一个 $e_{ij}$。注意到这个误差项只描述了在 $\xi_i$看到 $p_j$这件事，只涉及到第 $i$个相机位姿和第$j$个路标点，对其余变量的导入都为0。所以该误差对应的雅克比矩阵有下面的形式：
     

     $$J_{ij}(x)=(0_{2\times6},\cdots 0_{2\times6},\frac {\partial e_{ij}}{\partial \xi_i},0_{2\times6},\cdots 0_{2\times3},\cdots0_{2\times3},\frac {\partial e{ij}}{\partial p_j},0_{2\times3},\cdots0_{2\times3})$$
     其中 $0_{2\times6}$表示为纬度为 $2\times6$的 $0$矩阵，同理$0_{2\times3}$也一样。该误差对相机姿态的偏导 ${\partial e_{ij}}/{\partial \xi_i}$的维度为 $2\times6$对路标点的偏导 ${\partial e{ij}}/{\partial p_j}$的维度为 $2\times3$。这个误差项的雅克比矩阵，除了这两处为非零块之外，其余地方都为零。这体现了该误差项与其他路标与轨迹无关的特性。那么，他对增量方程有什么影响呢？ $H$矩阵为什么会产生稀疏性呢？
     

     当某个误差项$J$具有稀疏性时，它对 $H$的贡献也具有稀疏形式

     设$J_{ij}$只在 $i,j$处有非零块，那么它对 $H$的贡献为$J_{ij}^TJ_{ij}$，具有示意图上所显示的稀疏形式。这个 $J_{ij}^TJ_{ij}$仅有四个非零块，位于 $(i,i),(i,j),(j,i),(j,j)$。对于整体 $H$，由于：
     

     $$H=\sum_{i,j}J_{ij}^TJ_{ij}$$
     请注意 $i$在所有的相机的位姿中取值，$j$在左右路标点中取值。我们把 $H$进行分块：
     $$H=\begin{bmatrix} H_{11}&H_{12}\\ H_{21}&H_{22} \end{bmatrix}$$

     这里的$H_{11}$只和位姿点有关，$H_{22}$只和路标点有关。当我们遍历$i,j$时，下列事实总是成立的：

     1. 不管$i,j$怎么变，$H_{11}$都是对角阵，只有$H_{ii}$处有非零块。
     2. 同理，$H_{22}$都是对角阵，$H_{jj}$处有非零块。
     3. 对于$H_{12}$和$H_{21}$,有可能稀疏有可能稠密，视具体的情况而定。

     这显示了矩阵的稀疏结构，之后对线性方程的求解，也是利用它的稀疏结构。
     我们这里简单举例说明。假设一个场景里面有2个相机位姿$(C_1,C_2)$和6个路标$(P_1,P_2,P_3,P_4,P_5,P_6)$。相机位姿和路标点所对应的变量分别为$\xi_i,i=1,2$,$P_j,j=1,\cdots,6$。$C_1$观测到路标点$P_1,P_2,P_3,P_4$,$C_2$观测到路标点$P_3,P_4,P_5,P_6$。

     
     可以推测BA的目标函数为：
     

     $$\frac12(||e_{11}||^2+||e_{12}||^2+||e_{13}||^2+||e_{14}||^2+||e_{23}||^2+||e_{24}||^2+||e_{25}||^2+||e_{26}||^2)$$
     这里的 $e_{ij}$使用之前定义过的代价函数。以 $e_{11}$为例，它描述在 $C_1$看到 $P_1$这件事，与其它的相机位姿和路标无关。记 $J_{11}$为 $e_{11}$对应的雅克比矩阵，不难看出 $e_{11}$对 $\xi_2$和 $P_2,\cdots,P_6$的偏导数都为0。我们把所有变量以 $(\xi_1,\xi_2,p_1,\cdots,p_6)^T$的顺序摆放，则有：
     $$J_{11}=\frac {\partial e_{11}}{\partial x}=(\frac {\partial e_{11}}{\partial x_{11}},0_{2\times6},\frac {\partial e_{11}}{\partial p_{1}},0_{2\times3},0_{2\times3},0_{2\times3},0_{2\times3},0_{2\times3})$$

     为例方便表示稀疏性，使用带有颜色的方块表示矩阵在该方块内有数值，其余没有颜色的区域表示矩阵在该处数值都为0，那么$J_{11}$可以表示成下面的图案：
     

     为了得到该目标函数对应的雅可比矩阵，我们可以将这些$J_{ij}$ 按照一定顺序列为向量，那么整体雅可比矩阵以及相应的H 矩阵的稀疏情况就是图所展示的那样。
     
     也许你已经注意到了，邻接矩阵（Adjacency Matrix）和上图中的$H$矩阵，除了对角元素以外的其余部分有着完全一致的结构。事实上的确如此。上面的$H$ 矩阵一共有$8\times 8$的矩阵块，对于$H$矩阵中处于非对角线的矩阵来说，如果该矩阵块非零，则在其位置所在的变量之间存在一条边。如下图所示。所以，H矩阵中的非对角阵中的非零矩阵块可以理解为对应的两个变量之间存在联系，或者称之为约束，我们发现图优化结构和增量方程的稀疏性存在明显的联系。
     
     现在考虑更一般的情况，假设有m个位姿点和m个路标点。由于通常路标数量远远比位姿数量多得多，即$n>>m$。由上面的矩阵推理过程，可以知道H矩阵大致的样子。它的左上角块非常小，而右下角对角块占据大量的地方。除此之外，非对角部分，分布着算乱的观测数据。由于它的形状很像箭头，又称箭头形矩阵。
     对于具有这种矩阵结构$H$,线性方程组$H\triangle x=g$的求解有何不同?现实当中存在多种利用$H$的稀疏性加速计算的方法。这里介绍SLAM中一种常用的手段Schur消元，又叫边缘化。
     观察下图，我们不难发现，该矩阵可以分成4块，左上角为对角矩阵，每个对较块的维度与相机位姿的维度相等，且是一个对角块矩阵。右下角也是对角矩阵，每个对角块维度是路标的维度。非对角块的结构与具体的观测数据有关，我们首先划分区域$B,E,C$:
     
     
     于是对应的线性方程组由$H\triangle x=g$变为如下形式：
     

     $$\begin{bmatrix} B&E\\ E^T&C \end{bmatrix}\begin{bmatrix} \triangle x_c\\ \triangle x_p \end{bmatrix}=\begin{bmatrix} v\\ w\end{bmatrix}$$
     其中B为对角块矩阵，每个对角块的维数与相机参数相同，对角块的个数是相机变量的个数。由于路标的个数要远远大于相机位姿的个数，所以C要远远大于B。三维空间中每个路标点位3维，所以C矩阵为对角块矩阵，每个对角块为 $3\times 3$。对角块矩阵逆的求解难度远远小于一般矩阵的求逆难度，因为我们只需要对矩阵块分别求逆。考虑这个特性，我们考虑对上面的方程进行高斯消元，目标是消去右上角的非对角元素。得：
     $$\begin{bmatrix} I&-EC^{-1}\\ 0&I \end{bmatrix}\begin{bmatrix} B&E\\ E^T&C \end{bmatrix}\begin{bmatrix} \triangle x_c\\ \triangle x_p \end{bmatrix}=\begin{bmatrix} I&-EC^{-1}\\ 0&I \end{bmatrix}\begin{bmatrix} v\\ w\end{bmatrix}$$
     整理得：
     $$\begin{bmatrix} B-EC^{-1}E^T&0\\ E^T&C \end{bmatrix}\begin{bmatrix} \triangle x_c\\ \triangle x_p \end{bmatrix}=\begin{bmatrix} v-EC^{-1}w\\ w\end{bmatrix}$$

     经过一次消元之后，第一个方程组变成了和$\triangle x_p$无关的项。单独把它拿出来得到了关于位姿部分的增量方程：
     

     $$\begin{bmatrix} B-EC^{-1}E^T \end{bmatrix} \triangle x_c= v-EC^{-1}w$$
     这个线性方程组的维度和 $B$矩阵一样。我们的做法是先解出这个方程组，求得$\triangle x_c$，然后再讲 $\triangle x_c$带入原方程求得 $\triangle x_p$。 这个过程称称作Marginalization 或者称作Schur消元(Schur Elimination)。相比于直接求解线性方程组，它的优势在于:
     1. 在消元过程中，因为$C$为对角块，所以$C^{-1}$容易求得。
     2. 求解$\triangle x_c$之后，路标部分的增方程由$\triangle x_p=C^{-1}(w-E^T\triangle x_c)$，也会利用到$C^{-1}$易于求逆。

     边缘化的主要计算量在于上式上， 它紧是一个普通的线性方程，没有特殊的结构可以利用。系数记为S，它的稀疏性是不规则的。
     
     前面说过，非对角元素代表着相机与路标之间的关联。那么进行消元之后的S矩阵是否具有物理意义。答案是肯定的。S矩阵非对角线上的非零矩阵块表示该处对应的两个相机变量存在共同观测路标点，有时称为共视。反之，如果该块为零，则表示两个相机没有共同观测。
     
     于是，S矩阵的稀疏性结构取决于实际观测的结果，我们无法提前预知。在实践当中，例如ORB_SLAM中的Local Mapping环节，在做BA的时候可以选择那些具有共同观测的帧作为关键帧，在这种情况下Schur消元后得到的矩阵就是稠密矩阵。不过，这种做法也不是实时进行，所以这种做法也是可以接受的。但在另一些方法里面，例如DSO、
     ，OKVIS等，他们采用了滑动窗口方法。这类方法对每一帧都要求做一次BA来防止误差的累积，因此他们也必须采取一定的技巧来保持S矩阵的稀疏性。
     从概率角度来说，我们称这一步为边缘化，是因为我们实际上把求($\triangle x_c,\triangle x_p$)的问题，转化成先求$\triangle x_c$,再求$\triangle x_p$的过程。这一步相当于做了条件概率展开：
     

     $$P( x_c, x_p)=P(x_c).P(x_p|x_c)$$
     结果是求出了关于 $x_c$的边缘分布，故称边缘化。在上面的边缘化过程中，我们实际上把所有的路标点都给边缘化了。在实际情况下，我们也能选择一部分进行边缘化。同时，Schur消元只是实现边缘化的一种方式，同样也可以选择Cholesky分解进行边缘化。

     鲁棒核函数

     在前面的BA问题中，我们最小化误差的二范数平方和，作为目标函数。这种做法虽然很直观，但存在一个严重的问题：如果出现无匹配等原因，某个误差项给的数据是错误的，会发生什么呢？我们把一条原本不应该加到图中的边给加进去了，然而优化算法并没能辨别出这是个错误数据，他会把所有数据都当作误差来处理，这时，算法会看到一条误差很大的边，他的梯度也很大，意味着调整与他相关的变量会使目标函数下降更多。所以算法将试图调整这条边所连接节点的估计值，使他们顺应这条边的无理要求由于这条边的误差真的很大，往往会抹平了其他正确边的影响，使优化算法专注于调整一个错误的值。这当然不是我们希望看到的。
     出现这种问题的原因是，当误差很大时，二范数增长得太快了。于是有了核函数的存在。核函数保证每条边的误差不会打的没边，掩盖掉其他的边。具体的方式是，把原先误差的二范数度量，替换成一个增长没那么快的函数，同时保证自己的光滑性质。因为它们是的整个优化结果更为鲁棒，因此叫做鲁棒核函数（Robust Kernel）。
     鲁棒核函数有许多种，例如最常用的Huber核：
     

     $$H(e)= \begin{cases} \frac12e^2 \quad\quad\quad\quad if |e|\leq \delta\\ \delta(|e|-\frac12\delta)\quad otherwise \end{cases}$$
     我们看到，当误差 $e$大于某个预知$\delta$后，函数增长形势由二次形势变为一次形式，相当于限制了梯度的最大值。同时，Huber核函数又是光滑的，可以很方便的求导。下图显示了HUber核函数与二次函数的对比，课件误差较大时Huber核函数增长明显低于二次函数。
     
     除了Huber核之外，还有Cauchy核，Turkey核等等，读者可以参考g2o核Ceres都提供了哪些核函数。