vSLAM学习笔记——边缘化

BA求解

$BA$求解时对目标函数做线性化处理：
$$\frac{1}{2}||f(x+\Delta x)|{|}^2\approx \frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||e_{ij}+F_{ij}\Delta {\xi }_i+E_{ij}\Delta p_j|{|}^2\text{\hspace{1em}(1)}$$
$F_{ij}$表示在当前状态下对相机姿态的偏导数，$E_{ij}$表示该函数对路标点位置的偏导。
将相机为自变量和空间点变量放在一起可将上式简化为：
$$\frac{1}{2}||f(x+\Delta x)|{|}^2=\frac{1}{2}||e+F\Delta x_c+E\Delta x_p|{|}^2\text{\hspace{1em}(2)}$$
由于把变量归类成了位姿和空间点两种，所以雅克比矩阵可以分块为
$$J=\left[\begin{array}{cc}F& E\end{array}\right]\text{\hspace{1em}(3)}$$
以高斯牛顿法为例，$H$矩阵为：
$$H=J^{T}J=\left[\begin{array}{cc}{F}^{T}F& F^{T}E\\ & \\ E^{T}F& E^{T}E\end{array}\right]\text{\hspace{1em}(4)}$$
因为考虑了所有的优化变量，所以这个线性方程的维度非常大，如果直接对$H$求逆来计算增量方程将会非常消耗计算资源。

稀疏性

以上得到的$H$矩阵具有稀疏结构的特点，该结构可以自然、显式地用地图优化来表示。
$H$矩阵的稀疏性是由雅克比矩阵$J(x)$引起的，因为部分误差项与其他路标和轨迹无关，这使得$H$中存在很多零块。对于具有这种稀疏结构的$H$，线性方程$H\Delta x=g$的求解可以用若干种加速计算的方法。其中$Schur$消元是SLAM中最常用的手段，也被成为$Marginalization$。

边缘化

$H$矩阵可以分成$4$个块。左上角为对角块矩阵，每个对角块元素的维度与相机位姿的维度相同，且是一个对角块矩阵。右下角也是对角块矩阵，每个对角块的维度是路标的维度。这$4$个区域正好对应了$(4)$式中的$4$个矩阵块。将这个四个块记为$B，E，E^T，C$。
于是对应的线性方程组也可以由$H\Delta x=g$变为如下形式：
$$\left[\begin{array}{cc}B& E\\ & \\ E^T& C\end{array}\right]\left[\begin{array}{c}\Delta x_c\\ \\ \Delta x_p\end{array}\right]=\left[\begin{array}{c}v\\ \\ w\end{array}\right]$$
其中$B$是对角块矩阵，每个对角块的维度和相机参数的维度相同，对角块的个数是相机变量的个数。由于路标数量会远远大于相机变量个数，所以$C$往往也远大于$B$。三维空间中每个路标点为三维，于是$C$矩阵为对角块矩阵，每个块为$3\times 3$矩阵。
对角块矩阵求逆的难度远小于对一般矩阵的求逆难度，因为只需要对那些对角线矩阵块分别求逆即可。考虑到这个特性，对线性方程组进行高斯消元，目标是消去右上角的非对角部分$E$，得
$$\left[\begin{array}{cc}I& -E{C}^{-1}\\ & \\ 0& I\end{array}\right]\left[\begin{array}{cc}B& E\\ & \\ E^T& C\end{array}\right]\left[\begin{array}{c}\Delta x_c\\ \\ \Delta x_p\end{array}\right]=\left[\begin{array}{cc}I& -E{C}^{-1}\\ & \\ 0& I\end{array}\right]\left[\begin{array}{c}v\\ \\ w\end{array}\right]$$
整理得
$$\left[\begin{array}{cc}B-E{C}^{-1}{E}^T& 0\\ & \\ E^T& C\end{array}\right]\left[\begin{array}{c}\Delta x_c\\ \\ \Delta x_p\end{array}\right]=\left[\begin{array}{c}v-E{C}^{-1}w\\ \\ w\end{array}\right]$$
消元后，方程组第一行变成了和$\Delta x_p$无关的项。单独拿出来后可以得到关于位姿部分的增量方程：
$$\left[\begin{array}{c}B-E{C}^{-1}{E}^T\end{array}\right]\Delta x_c=v-E{C}^{-1}w$$
先求解这个方程，然后把解得的$\Delta x_c$代入原方程，求解$\Delta x_p$。这个过程成为Marginalization，或者Schur消元。

优势

相比于直接解线性方程的做法，优势在于：

1. 在消元过程中，由于$C$为对角块，所以$C^{-1}$容易解出。
2. 求解了$\Delta x_c$之后，路标部分的增量方程由$\Delta x_p=C^{-1}(w-E^T\Delta x_c)$给出。依然用到了$C^{-1}$易于求解的特性。