视觉SLAM十四讲第十讲笔记（2）

Bundle Adjustment与图优化

在只有观测方程的情况下，这个问题称为BA，并可利用非线性优化方法求解。
所谓的Bundle Adjustment，是指从视觉重建中提炼出最优的3D模型和相机参数(内参数和外参数)。从每一个特征点反射出来的几束光线(bundles of light rays)，在我们把相机姿态和特征点空间位置做出最优的调整(adjustment)之后，最后收束到相机光心的这个过程，简称为BA。BA算法不仅具有很高的精度，也开始具备良好的实时性。

1. 投影模型和BA代价函数

回顾一下一个世界坐标系的点$P$出发，把相机的内外参数和畸变都考虑进来，最优投影成像素坐标，需要以下步骤:

这个过程就是观测方程，之前我们把它抽象地记成:
$$z=h(x,y)$$

现在，我们给出了它的详细参数化过程。具体地说，这里的$x$指代此时相机的位姿，即外参$R,t$，它对应的李代数为$ϵ$。路标$y$即这里的三维点$p$，而观测数据则是像素坐标$z\triangleq [u_s,v_s{]}^T$。以最小二乘的角度来考虑，那么可以列写关于此次观测的误差:
$$e=z-h(ϵ,p)$$
然后，把其他时刻的观测量也考虑进来，我们可以给误差添加一个下标。设$z_{ij}$为在位姿${ϵ}_i$处观察路标$p_j$产生的数据，那么整体的代价函数(cost function)为:
$$\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||e_{ij}|{|}^2=\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||z_{ij}-h({ϵ}_i,p_j)|{|}^2$$

对这个最小二乘进行求解，相当于对位姿和路标同时作了调整，也就是所谓的BA。

2. BA的求解

观测模型$h(ϵ,p)$是非线性函数，所以使用一些非线性手段来优化。据非线性优化的思想，我们应该从某个的初始值开始，不断地寻找下降方向$△x$来找到目标函数的最优解，即不断地求解增量方程中的增量$△x$。尽管误差项都是针对单个位姿和路标点的，但在整体的BA目标函数上，我们必须把自变量定义成所有待优化的变量:
$$x=[{ϵ}_1,..,{ϵ}_m,p_1,...,p_n{]}^T$$
相应的，增量方程中的$△x$则是对整体自变量的增量。在这个意义下，当我们给自变量一个增量时，目标函数变为:
$$\frac{1}{2}||f(x+△x)|{|}^2\approx \frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||e_{ij}+F_{ij}△{ϵ}_i+E_{ij}△p_j|{|}^2$$

其中$F_{ij}$表示整个代价函数在当前状态下对相机姿态的偏导数，而$E_{ij}$表示该函数对路标点位置的偏导。

现在，把相机位姿变量放在一起:
$$x_c=[{ϵ}_1,{ϵ}_2,...,{ϵ}_m{]}^T\in {\mathbb{R}}^{6m}$$
并把空间点的变量也放在一起:
$$x_p=[p_1,p_2,...,p_n{]}^T\in {\mathbb{R}}^{3n}$$

那么目标函数可以简化为:
$$\frac{1}{2}||f(x+△x)|{|}^2=\frac{1}{2}||e+F△x_c+E△x_p|{|}^2$$

这里的雅可比矩阵$E$和$F$必须是整体目标函数对整体变量的导数。最后，无论使用G-N还是L-M方法，最后都將面对增量线性方程:
$$H△x=g$$

我们知道G-N和L-M的主要差别在于，这里的$H$是取$J^{T}J$还是$J^{T}J+\lambda I$的形式。由于我们把变量归类成了位姿和空间点两种，所以雅可比矩阵可以分块为:
$$J=[FE]$$

那么，以G-N为例，则$H$矩阵为:
$$H=J^{T}J=\left[\begin{array}{c}{F}^{T}F{F}^{T}E\\ E^{T}F{E}^{T}E\end{array}\right]$$

不难发现，因为考虑了所有的优化变量，这个线性方程的维度将非常大，包含了所有的相机位姿和路标点。

3. 稀疏性和边缘化

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

这个误差项的雅可比矩阵，除了这两处为非零块之外，其余地方都为零。这体现了该误差项与其他路标和轨迹无关的特性。

我们设$J_{ij}$只在$i,j$处有非零块，那么它对$H$的贡献为$J_{ij}^T{J}_{ij}$，具有下面示意图所画的稀疏形式。

这个$J_{ij}^T{J}_{ij}$矩阵也仅有四个非零块，位于$(i,i),(i,j),(j,i),(j,j)$。对于整体的$H$，由于:
$$H=\sum _{i,j}{J}_{ij}^T{J}_{ij}$$

我们把$H$进行分块:
$$H=\left[\begin{array}{c}{H}_{11}{H}_{12}\\ H_{21}{H}_{22}\end{array}\right]$$

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

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

这显示了$H$的稀疏结构。

对于具有这种稀疏结构的$H$，线性方程 $H\triangle x = g $的求解会有什么不同呢？现实当中存在着若干种利用$H$的稀疏性加速计算的方法，最常用的手段如:Schur消元(Schur trick)。在SLAM研究中亦称为Marginalization(边缘化)。

我们转化一下，$H$矩阵可以划分为下面矩阵:

于是，对应的线性方程组也可以由$H△x=g$变为如下形式:
$$\left[\begin{array}{c}BE\\ E^{T}C\end{array}\right]\left[\begin{array}{c}△x_c\\ △x_p\end{array}\right]=\left[\begin{array}{c}v\\ w\end{array}\right]$$

其中$B$是对角块矩阵，每个对角块的维度和相机参数的维度相同，对角块的个数是相机变量的个数。由于路标数量会远远大于相机变量个数，所以$C$往往也远大于$B$。三维空间中每个路标点为三维，于是$C$矩阵为对角块矩阵，每个块为3 × 3维矩阵。对角块矩阵逆的难度远小于对一般矩阵的求逆难度，因为我们只需要对那些对角线矩阵块分别求逆即可。考虑到这个特性，我们线性方程组进行高斯消元，目标是消去右上角的非对角部分$E$，得
$$\left[\begin{array}{c}I-E{C}^{-1}\\ 0I\end{array}\right]\left[\begin{array}{c}BE\\ E^{T}C\end{array}\right]\left[\begin{array}{c}△X_c\\ △X_p\end{array}\right]=\left[\begin{array}{c}I-E{C}^{-1}\\ 0I\end{array}\right]\left[\begin{array}{c}v\\ w小结本讲比较深入地探讨了状态估计问题与图优化的求解。我们看到在经典模型中，SLAM可以看成状态估计问题。如果我们假设马尔可夫性，只考虑当前状态的话，则得到以EKF为代表的滤波器模型。如若不然，我们也可以选择考虑所有的运动和观测，它们构成一个最小二乘问题。在只有观测方\end{array}\right]$$

整理可得:
$$\left[\begin{array}{c}B-E{C}^{-1}{E}^{T}0\\ E^{T}C\end{array}\right]\left[\begin{array}{c}△X_c\\ △X_p\end{array}\right]=\left[\begin{array}{c}v-E{C}^{-1}w\\ w\end{array}\right]$$

经过消元之后，第一行方程组变成和$△x_p$无关的项。单独把它拿出来，得到关于位姿部分的增量方程:
$$[B-E{C}^{-1}{E}^T]△X_c=v-E{C}^{-1}w$$

这个线性方程组的维度和$B$矩阵一样。我们的做法是先求解这个方程，然后把解得的$△x_c$代入到原方程，然后求解$△x_p$。 这个过程称为Marginalization，或者Schur消元(Schur Elimination)。相比于直接解线性方程的做法，它的优势在于:

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

4. 鲁棒核函数

在前面的BA问题中，我们最小化误差项的二范数平方和，作为目标函数。这种做法虽然很直观，但存在一个严重的问题:如果出于误匹配等原因，某个误差项给的数据是错误的，会发生什么呢？我们把一条原本不应该加到图中的边给加进去了，然而优化算法并不能辨别出这是个错误数据，它会把所有的数据都当作误差来处理。这时，算法会看到一条误差很大的边，它的梯度也很大，意味着调整与它相关的变量会使目标函数下降更多。

出现这种问题的原因是，当误差很大时，二范数增长得太快了。于是就有了核函数的存在。核函数保证每条边的误差不会大的没边，掩盖掉其他的边。 具体的方式是，把原先误差的二范数度量，替换成一个增长没有那么快的函数，同时保证自己的光滑性质。因为它们使得整个优化结果更为鲁棒，所以又叫它们为鲁棒核函数(Robust Kernel)。

鲁棒核函数有许多种，例如最常用的Huber核:
$$H(e)=\{\begin{array}{rl}& \frac{1}{2}{e}^2\text{if}|e|\le \delta ,\\ & \delta (|e|-\frac{1}{2}\delta )\text{otherwise}\end{array}$$

我们看到，当误差$e$大于某个阈值$\delta$后，函数增长由二次形式变成了一次形式，相当于限制了梯度的最大值。同时，Huber核函数又是光滑的，可以很方便地求导