《视觉SLAM十四讲》-- 后端 1（下）

8.2 BA 与图优化

Bundle Adjustment 是指从视觉图像中提炼出最优的 3D 模型和相机参数（内参和外参）。

8.2.1 相机模型和 BA 代价函数

我们从一个世界坐标系中的点 $\mathbf{p}$ 出发，把相机的内外参数和畸变都考虑进来，最后投影成像素坐标，步骤如下：

（1）世界坐标系转换到相机坐标系

$${\mathbf{P}}^{\prime }=\mathbf{Rp}+\mathbf{t}=[X^{\prime },Y^{\prime },Z^{\prime }{]}^{\mathrm{T}}\text{\hspace{1em}(8-30)}$$

（2）将 ${\mathbf{P}}^{\prime }$ 投影至归一化平面，得到归一化坐标

$${\mathbf{P}}_c=[u_c,v_c,1{]}^{\mathrm{T}}=[X^{\prime }/Z^{\prime },Y^{\prime }/Z^{\prime },1{]}^{\mathrm{T}}\text{\hspace{1em}(8-31)}$$

（3）去畸变（这里仅考虑径向畸变）

$$\{\begin{array}{l}{u}_{\mathrm{c}}^{\prime }=u_{\mathrm{c}}\left(1+k_1{r}_{\mathrm{c}}^2+k_2{r}_{\mathrm{c}}^4\right)\\ v_{\mathrm{c}}^{\prime }=v_{\mathrm{c}}\left(1+k_1{r}_{\mathrm{c}}^2+k_2{r}_{\mathrm{c}}^4\right)\end{array}\text{\hspace{1em}(8-32)}$$
（4）根据内参模型，计算像素坐标

$$\{\begin{array}{l}{u}_s=f_x{u}_{\mathrm{c}}^{\prime }+c_x\\ v_s=f_y{v}_{\mathrm{c}}^{\prime }+c_y\end{array}\text{\hspace{1em}(8-33)}$$

上面的过程也就是 观测方程，将它抽象的记为

$$\mathbf{z}=h(\mathbf{x},\mathbf{y})\text{\hspace{1em}(8-34)}$$

其中，$\mathbf{x}$ 是指此时相机的位姿，即 $\mathbf{R}$、$\mathbf{t}$，对应的李群为 $\mathbf{T}$，李代数为 $\mathbf{\xi }$；路标 $\mathbf{y}$ 即三维点 $\mathbf{p}$；观测数据 $\mathbf{z}$ 则是像素坐标。以最小二乘角度考虑，此次观测的误差为：

$$\mathbf{e}=\mathbf{z}-h(\mathbf{T},\mathbf{p})\text{\hspace{1em}(8-35)}$$

把其他时刻的观测都考虑进来，设 ${\mathbf{z}}_{ij}$ 为在位姿 ${\mathbf{T}}_i$ 处观察路标 ${\mathbf{p}}_j$ 产生的数据，那么整体的代价函数为

$$\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n\Vert {\mathbf{e}}_{ij}{\Vert }^2=\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n\Vert {\mathbf{z}}_{ij}-h({\mathbf{T}}_i,{\mathbf{p}}_j){\Vert }^2\text{\hspace{1em}(8-36)}$$

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

8.2.2 BA 的求解

容易看出 $h(\mathbf{T},\mathbf{p})$ 不是线性函数，因此我们希望采用非线性优化的方法求解最优值，所以关键在于梯度 $\Delta \mathbf{x}$ 的求解。

在整体 BA 目标函数上，我们把自变量定义为所有待优化的变量：

$$\mathbf{x}=[{\mathbf{T}}_1,{\mathbf{T}}_2,...,{\mathbf{T}}_m,{\mathbf{p}}_1,{\mathbf{p}}_2,...,{\mathbf{p}}_n{]}^T\text{\hspace{1em}(8-37)}$$

相应地，增量方程中的 $\Delta \mathbf{x}$ 是对整体自变量的增量。当我们给自变量一个增量时，目标函数变为：

$$\frac{1}{2}\Vert f(\mathbf{x}+\Delta \mathbf{x}){\Vert }^2\approx \frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n\Vert {\mathbf{e}}_{ij}+{\mathbf{F}}_{ij}\Delta {\mathbf{\xi }}_i+{\mathbf{E}}_{ij}\Delta {\mathbf{\xi }}_j{\Vert }^2\text{\hspace{1em}(8-38)}$$

其中，${\mathbf{F}}_{ij}$ 表示整个代价函数在当前状态下对相机位姿 $\mathbf{\xi }$ 的偏导数，${\mathbf{E}}_{ij}$ 表示整个代价函数在当前状态下路标位置 $\mathbf{p}$ 的偏导数。

把相机位姿放在一起

$${\mathbf{x}}_c=[{\mathbf{\xi }}_1,{\mathbf{\xi }}_2,...,{\mathbf{\xi }}_m{]}^{\mathrm{T}}\in {\mathbb{R}}^{6m}\text{\hspace{1em}(8-39)}$$

把空间点的变量也放在一起：

$${\mathbf{x}}_p=[{\mathbf{p}}_1,{\mathbf{p}}_2,...,{\mathbf{p}}_n{]}^{\mathrm{T}}\in {\mathbb{R}}^{3n}\text{\hspace{1em}(8-40)}$$

那么，式（8-38）可简化为

$$\frac{1}{2}\Vert f(\mathbf{x}+\Delta \mathbf{x}){\Vert }^2\approx \frac{1}{2}\Vert \mathbf{e}+\mathbf{F}\Delta {\mathbf{x}}_c+\mathbf{E}\Delta {\mathbf{x}}_p{\Vert }^2\text{\hspace{1em}(8-41)}$$

上式将二次项之和写成了矩阵形式。这里的雅克比矩阵 $\mathbf{F}$ 和 $\mathbf{E}$ 是整体目标函数对整体变量的导数，它是一个很大的矩阵，由每个误差项的导数 ${\mathbf{F}}_{ij}$ 和 ${\mathbf{E}}_{ij}$ 拼凑而成。可以采用高斯牛顿法或 L-M 法，得到增量方程

$$\mathbf{H}\Delta \mathbf{x}=\mathbf{g}\text{\hspace{1em}(8-42)}$$

为便于表示，我们将变量归类为位姿和空间点两种，则雅克比矩阵分块为

$$\mathbf{J}=[\mathbf{F}\mathbf{E}]\text{\hspace{1em}(8-43)}$$

以高斯牛顿法为例，则 $\mathbf{H}$ 矩阵为

$$\mathbf{H}={\mathbf{J}}^{\mathrm{T}}\mathbf{J}=\left[\begin{array}{ll}{\mathbf{F}}^{\mathrm{T}}\mathbf{F}& {\mathbf{F}}^{\mathrm{T}}\mathbf{E}\\ {\mathbf{E}}^{\mathrm{T}}\mathbf{F}& {\mathbf{E}}^{\mathrm{T}}\mathbf{E}\end{array}\right]\text{\hspace{1em}(8-44)}$$

但是，这个矩阵的维度非常大，而且直接对 $\mathbf{H}$ 求逆，复杂度也很高。所以我们需要利用 $\mathbf{H}$ 矩阵的特殊结构，加速计算过程。

8.2.3 稀疏性和边缘化

（1）$\mathbf{H}$ 矩阵的稀疏性是由雅克比矩阵 $\mathbf{J}(\mathbf{x})$ 引起的。考虑其中一个 ${\mathbf{e}}_{ij}$，它只描述了相机在 ${\mathbf{T}}_i$ 处看到 ${\mathbf{p}}_j$这件事，只与第 $i$个位姿和第 $j$ 个路标有关，而与其他位姿和路标都无关，因此对其余部分的变量的导数都为零。所以误差 ${\mathbf{e}}_{ij}$ 对应的雅克比矩阵为

$${\mathbf{J}}_{ij}(\mathbf{x})=\left(0_{2\times 6},\dots 0_{2\times 6},\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{T}}_i},0_{2\times 6},\dots 0_{2\times 3},\dots 0_{2\times 3},\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{p}}_j},0_{2\times 3},\dots 0_{2\times 3}\right)\text{\hspace{1em}(8-45)}$$

注意，误差对相机位姿的偏导 $\partial {\mathbf{e}}_{ij}/\partial {\mathbf{\xi }}_i$ 维度为 $2\times 6$，对路标点的偏导 $\partial {\mathbf{e}}_{ij}/\partial {\mathbf{p}}_j$维度为 $2\times 3$。

（2）以下图为例，假设 ${\mathbf{J}}_{ij}$ 只在 $i$、$j$ 处有非零块，那么它对 $\mathbf{H}$矩阵的贡献为 ${\mathbf{J}}_{ij}^{\mathrm{T}}{\mathbf{J}}_{ij}$，${\mathbf{J}}_{ij}^{\mathrm{T}}{\mathbf{J}}_{ij}$ 矩阵有 4 个非零块，位于 $(i,i)$、$(i,j)$、$(j,i)$、$(j,j)$。对整体的 $\mathbf{H}$，有

$$\mathbf{H}=\sum _{i,j}{\mathbf{J}}_{ij}^{\mathrm{T}}{\mathbf{J}}_{ij}\text{\hspace{1em}(8-46)}$$

将 $\mathbf{H}$ 矩阵分块

$$\mathbf{H}=\left[\begin{array}{ll}{\mathbf{H}}_{11}& {\mathbf{H}}_{12}\\ {\mathbf{H}}_{21}& {\mathbf{H}}_{22}\end{array}\right]\text{\hspace{1em}(8-47)}$$

结合式（8-44）可知，${\mathbf{H}}_{11}$ 只和相机位姿有关，${\mathbf{H}}_{22}$ 只和路标点有关。当遍历矩阵 $\mathbf{H}$时，总有：

① ${\mathbf{H}}_{11}$ 是对角矩阵，且只在 ${\mathbf{H}}_{ii}$ 处有非零块；

② ${\mathbf{H}}_{22}$ 也是对角矩阵，且只在 ${\mathbf{H}}_{jj}$处有非零块；

③ ${\mathbf{H}}_{12}$ 和 ${\mathbf{H}}_{21}$ 可能是稀疏的，也可能是稠密的，视具体观测数据而定。

（3）以下图为例

假设一个场景内，有 2 个相机位姿（${\mathbf{C}}_1$、${\mathbf{C}}_2$）和 6 个路标点（${\mathbf{P}}_1$、${\mathbf{P}}_2$、${\mathbf{P}}_3$、${\mathbf{P}}_4$、${\mathbf{P}}_5$、${\mathbf{P}}_6$），这些相机位姿和路标点所对应的变量为 ${\mathbf{T}}_i,i=1,2$ 和 ${\mathbf{p}}_j,j=1,2$。可以推出，此场景下的 BA 目标函数为：

$$\frac{1}{2}\left({\Vert e_{11}\Vert }^2+{\Vert e_{12}\Vert }^2+{\Vert e_{13}\Vert }^2+{\Vert e_{14}\Vert }^2+{\Vert e_{23}\Vert }^2+{\Vert e_{24}\Vert }^2+{\Vert e_{25}\Vert }^2+{\Vert e_{26}\Vert }^2\right)\text{\hspace{1em}(8-48)}$$

令 ${\mathbf{J}}_{11}$ 为 ${\mathbf{e}}_{11}$ 对应的雅克比矩阵，且 ${\mathbf{e}}_{11}$ 对其他相机变量和路标点的偏导都为零。我们把所有变量以 $\mathbf{x}={\left({\mathbf{\xi }}_1,{\mathbf{\xi }}_2,{\mathbf{p}}_1,\cdots ,{\mathbf{p}}_6\right)}^{\mathrm{T}}$ 的顺序摆放，则有

$${\mathbf{J}}_{11}=\frac{\partial {\mathbf{e}}_{11}}{\partial \mathbf{x}}=\left(\frac{\partial {\mathbf{e}}_{11}}{\partial {\mathbf{\xi }}_1},0_{2\times 6},\frac{\partial {\mathbf{e}}_{11}}{\partial {\mathbf{p}}_1},0_{2\times 3},0_{2\times 3},0_{2\times 3},0_{2\times 3},0_{2\times 3}\right)\text{\hspace{1em}(8-49)}$$

我们用下图直观地表示雅克比矩阵的稀疏性：

由此，可以得到整体雅克比矩阵 $\mathbf{J}$ 和 $\mathbf{H}$矩阵。

$\mathbf{H}$ 矩阵中非对角部分的非零矩阵块（长方形块）可理解为其对应的两个变量之间的关系。

更一般地，假设有 $m$ 个相机位姿，$n$个路标点，且通常路标点的数量远多于相机，即 $n\gg m$。这种情况下的 $\mathbf{H}$ 矩阵如下图所示，左上角块非常小，右下对角块很大，由于形状很像箭头，又称为箭头形矩阵。

（4）将 $\mathbf{H}$ 矩阵划分为四个区域，不难看出，左上角为对角块矩阵，且每个对角块元素的维度与相机位姿维度相同，同样的，右下角也是对角块矩阵，且每个对角块元素的维度与路标点维度相同。而且这四个区域和式（8-44）中的矩阵块是对应的。

那么，增量方程 $\mathbf{H}\Delta \mathbf{x}=\mathbf{g}$ 可写为以下形式

$$\left[\begin{array}{cc}\mathbf{B}& \mathbf{E}\\ {\mathbf{E}}^{\mathrm{T}}& \mathbf{C}\end{array}\right]\left[\begin{array}{l}\Delta {\mathbf{x}}_{\mathrm{c}}\\ \Delta {\mathbf{x}}_p\end{array}\right]=\left[\begin{array}{l}\mathbf{v}\\ \mathbf{w}\end{array}\right]\text{\hspace{1em}(8-50)}$$

对角块矩阵求逆的难度远小于一般矩阵的求逆难度，所以只需要对对角块矩阵分别求逆即可。对线性方程组进行高斯消元，得

$$\left[\begin{array}{cc}\mathbf{I}& -\mathbf{E}{\mathbf{C}}^{-1}\\ 0& \mathbf{I}\end{array}\right]\left[\begin{array}{cc}\mathbf{B}& \mathbf{E}\\ {\mathbf{E}}^{\mathrm{T}}& C\end{array}\right]\left[\begin{array}{l}\Delta {\mathbf{x}}_{\mathrm{c}}\\ \Delta {\mathbf{x}}_p\end{array}\right]=\left[\begin{array}{cc}\mathbf{I}& -\mathbf{E}{\mathbf{C}}^{-1}\\ 0& \mathbf{I}\end{array}\right]\left[\begin{array}{l}\mathbf{v}\\ \mathbf{w}\end{array}\right]\text{\hspace{1em}(8-51)}$$

整理，得

$$\left[\begin{array}{cc}\mathbf{B}-\mathbf{E}{\mathbf{C}}^{-1}{\mathbf{E}}^{\mathrm{T}}& 0\\ {\mathbf{E}}^{\mathrm{T}}& \mathbf{C}\end{array}\right]\left[\begin{array}{l}\Delta {\mathbf{x}}_{\mathrm{c}}\\ \Delta {\mathbf{x}}_p\end{array}\right]=\left[\begin{array}{c}\mathbf{v}-\mathbf{E}{\mathbf{C}}^{-1}\mathbf{w}\\ \mathbf{w}\end{array}\right]\text{\hspace{1em}(8-52)}$$

可以看出，方程第一行只和 $\Delta {\mathbf{x}}_c$ 有关，我们可以先将 $\Delta {\mathbf{x}}_c$ 解出来：

$$(\mathbf{B}-\mathbf{E}{\mathbf{C}}^{-1}{\mathbf{E}}^{\mathrm{T}})\Delta {\mathbf{x}}_c=\mathbf{v}-\mathbf{E}{\mathbf{C}}^{-1}\mathbf{w}\text{\hspace{1em}(8-53)}$$

$\Delta {\mathbf{x}}_c$ 解出来后，再将其代入第二行方程，从而将 $\Delta {\mathbf{x}}_p$ 求解出来。这个过程称为 Schur （舒尔）消元。相较于直接求解的方法，它的优势在于：

① 消元过程中，$\mathbf{C}$ 为对角块，故 ${\mathbf{C}}^{-1}$ 容易求出；

② $\Delta {\mathbf{x}}_c$ 解出来后，根据 $\Delta {\mathbf{x}}_p={\mathbf{C}}^{-1}\left(\mathbf{w}-{\mathbf{E}}^{\mathrm{T}}\Delta {\mathbf{x}}_{\mathrm{c}}\right)$ 解出路标的增量方程。

（5）从概率角度来看，我们称这一步为 边缘化。我们将求解 $(\Delta {\mathbf{x}}_c，\Delta {\mathbf{x}}_p)$ 的问题，转化成了先固定 $\Delta {\mathbf{x}}_p$，求出 $\Delta {\mathbf{x}}_c$，再求 $\Delta {\mathbf{x}}_p$ 的过程。这相当于做了条件概率展开：

$$P({\mathbf{x}}_c,{\mathbf{x}}_p)=P({\mathbf{x}}_c|{\mathbf{x}}_p)P({\mathbf{x}}_p)\text{\hspace{1em}(8-54)}$$

8.2.4 鲁棒核函数

在前面的 BA 问题中，我们将最小化误差项的二范数平方和作为目标函数，这样虽然直观，但是如果出现误匹配，该误差项会很大，从而将对整体函数产生较大影响，进而影响最终优化结果。

对此，我们将原先误差的二范数度量替换成一个增长没那么快的函数，同时保证光滑性质，使得优化结果更加稳健（减小误匹配项的影响），这样的函数称为 鲁棒核函数。鲁棒核函数有很多种，如 Huber 核：

$$H(e)=\{\begin{array}{ll}\frac{1}{2}{e}^2& \text{ 当 }|e|⩽\delta ,\\ \delta \left(|e|-\frac{1}{2}\delta \right)& \text{ 其他 }\end{array}\text{\hspace{1em}(8-55)}$$

当误差 $e$ 大于阈值 $\delta$ 时，函数增长由二次形式转为一次形式，相当于限制了梯度的最大值。同时 Huber 核函数又是光滑的，可以很方便的求导。如下图，在误差较大时，Huber 核函数增长明显低于二次函数。