VSLAM学习(四) Bundle Adjustment

目录
VSLAM学习(一) 三维运动、相机模型、SLAM模型
VSLAM学习(二) 非线性优化
VSLAM学习(三) 单目相机位姿估计
VSLAM学习(四) Bundle Adjustment

一、重投影误差

1.1 误差项

前面学过了，对于任意第i帧相机像素与空间点坐标的关系：

$$s_i{\mathbf{x}}_i=\mathbf{K}\mathbf{T}{\mathbf{P}}_i或{\mathbf{x}}_i=\frac{1}{s_i}\mathbf{K}\mathbf{T}{\mathbf{P}}_i$$

但是有噪声的存在，所以这个等式并不严格相等
我们记误差

$${\mathbf{e}}_i={\mathbf{x}}_i-\frac{1}{s_i}\mathbf{K}\mathbf{T}{\mathbf{P}}_i$$

该误差项称为重投影误差
因为$\frac{1}{s_i}\mathbf{K}\mathbf{T}{\mathbf{P}}_i$相当于是将世界坐标${\mathbf{P}}_i$再重新投影到相机中来的

1.2 误差优化

对于一个重投影误差项
$$\mathbf{e}(\mathbf{x})=\mathbf{x}-\frac{1}{s}\mathbf{K}\mathbf{T}\mathbf{P}$$

我们记

$${\mathbf{P}}^{\prime }=(\mathbf{T}\mathbf{P}{)}_{1:3}=\mathbf{R}\mathbf{P}+\mathbf{t}=(X^{\prime },Y^{\prime },Z^{\prime }{)}^{\mathrm{T}}$$

再记
$${\mathbf{x}}_r=\frac{1}{s}\mathbf{K}{\mathbf{P}}^{\prime }$$

将$s{\mathbf{x}}_r=\mathbf{K}{\mathbf{P}}^{\prime }$展开:

$$\left(\begin{array}{c}s{u}_r\\ s{v}_r\\ s\end{array}\right)=\left(\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{X}^{\prime }\\ Y^{\prime }\\ Z^{\prime }\end{array}\right)$$

消去s得

$$\{\begin{array}{rl}{u}_r& =f_x\frac{X^{\prime }}{Z^{\prime }}+c_x\\ v_r& =f_y\frac{Y^{\prime }}{Z^{\prime }}+c_y\end{array}$$

下面我们开始对误差函数优化
对误差函数做一阶泰勒展开
$$\mathbf{e}(\mathbf{x}+\Delta \mathbf{x})\approx \mathbf{e}(\mathbf{x})+\mathbf{J}\Delta \mathbf{x}$$

也就是考虑Jacobian矩阵$\mathbf{J}$的形态

求出$\mathbf{J}$的形态之后，便可以使用牛顿法或者LM方法求解极值即可。

1.2.1 优化位姿

现在我们的目标是优化$\mathbf{R},\mathbf{t}$，也就是优化$\mathbf{\xi }$
根据扰动模型有

$$\begin{array}{rl}\frac{\partial \mathbf{e}}{\partial \mathbf{\xi }}& =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\mathbf{e}(\delta \mathbf{\xi }\oplus \mathbf{\xi })-e(\mathbf{\xi })}{\delta \mathbf{\xi }}\\ & =\frac{\partial \mathbf{e}}{\partial {\mathbf{P}}^{\prime }}\frac{\partial {\mathbf{P}}^{\prime }}{\partial \mathbf{\xi }}\end{array}$$

分别计算一下$\frac{\partial \mathbf{e}}{\partial {\mathbf{P}}^{\prime }}$和$\frac{\partial {\mathbf{P}}^{\prime }}{\partial \mathbf{\xi }}$

先来求第一项$\frac{\partial \mathbf{e}}{\partial {\mathbf{P}}^{\prime }}$
我们知道
$$\mathbf{e}(\mathbf{x})=\mathbf{x}-{\mathbf{x}}_r$$

其中$\mathbf{x}$是从图像上读出来的像素（常数），${\mathbf{x}}_r$是需要优化的量
故而

$$\frac{\partial \mathbf{e}}{\partial {\mathbf{P}}^{\prime }}=-\frac{\partial {\mathbf{x}}_r}{\partial {\mathbf{P}}^{\prime }}=-\left(\begin{array}{ccc}\frac{\partial u_r}{\partial X^{\prime }}& \frac{\partial u_r}{\partial Y^{\prime }}& \frac{\partial u_r}{\partial Z^{\prime }}\\ \frac{\partial v_r}{\partial X^{\prime }}& \frac{\partial v_r}{\partial Y^{\prime }}& \frac{\partial v_r}{\partial Z^{\prime }}\end{array}\right)=-\left(\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right)$$

对于第二项$\frac{\partial {\mathbf{P}}^{\prime }}{\partial \mathbf{\xi }}$，根据李代数扰动模型可知：

$$\frac{\partial {\mathbf{P}}^{\prime }}{\partial \mathbf{\xi }}=\frac{\partial (\mathbf{T}\mathbf{P}{)}_{1:3}}{\partial \mathbf{\xi }}=(\mathbf{T}\mathbf{P}{)}_{1:3}^{\odot }={\left(\begin{array}{cc}\mathbf{I}& -{\mathbf{P}}^{\prime \wedge }\\ 0^{\mathrm{T}}& 0^{\mathrm{T}}\end{array}\right)}_{1:3}=(\mathbf{I},-{\mathbf{P}}^{\prime \wedge })$$

所以最终计算得
$$\frac{\partial \mathbf{e}}{\partial \mathbf{\xi }}=-\left(\begin{array}{cccccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}& -\frac{f_x{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& f_x+\frac{f_x{X}^{\prime 2}}{Z^{\prime 2}}& -\frac{f_x{Y}^{\prime }}{Z^{\prime }}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}& -f_y-\frac{f_y{Y}^{\prime 2}}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }}{Z^{\prime }}\end{array}\right)$$
即为$\mathbf{J}$的形态

1.2.2 优化空间点路标

现在我们的目标是优化空间点$\mathbf{P}$
那么我就这里对$\mathbf{P}$求导

$$\frac{\partial \mathbf{e}}{\partial \mathbf{P}}=\frac{\partial \mathbf{e}}{\partial {\mathbf{P}}^{\prime }}\frac{\partial {\mathbf{P}}^{\prime }}{\partial \mathbf{P}}$$

由于
$${\mathbf{P}}^{\prime }=\mathbf{R}\mathbf{P}+\mathbf{t}$$
所以
$$\frac{\partial {\mathbf{P}}^{\prime }}{\partial \mathbf{P}}=\mathbf{R}$$

故而
$$\frac{\partial \mathbf{e}}{\partial \mathbf{P}}=-\left(\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right)\mathbf{R}$$
即为$\mathbf{J}$的形态

二、光束平差法（Bundle Adjustment）批量优化

2.1 一阶梯度的表示

回忆之前的SLAM模型
我们主要对观测方程进行优化：

$${\mathbf{z}}_{ij}=h({\mathbf{\xi }}_i,{\mathbf{p}}_j)+{\mathbf{v}}_{ij}$$

其中${\mathbf{z}}_{ij}$　是指$i$时刻对第$j$个路标点的观测值，即拍摄图片上读出的像素坐标
　　${\mathbf{\xi }}_i$　是指$i$时刻的相机位姿
　　${\mathbf{p}}_j$　表示第$j$个路标点坐标
　　${\mathbf{v}}_{ij}$　表示$i$时刻第$j$个路标点的观测噪声

那么可以定义误差：

$${\mathbf{e}}_{ij}={\mathbf{z}}_{ij}-h({\mathbf{\xi }}_i,{\mathbf{p}}_j)$$

那么代价函数就可以定义为：

$$\frac{1}{2}\sum _i\sum _j\Vert {\mathbf{e}}_{ij}{\Vert }^2=\frac{1}{2}\sum _i\sum _j\Vert {\mathbf{z}}_{ij}-h({\mathbf{\xi }}_i,{\mathbf{p}}_j){\Vert }^2$$

为了计算最小值，我们可以将相机位姿$\mathbf{\xi }$和路标$\mathbf{p}$整合到一起
记

$$\mathbf{x}=({\mathbf{\xi }}_1,\cdot \cdot \cdot ,{\mathbf{\xi }}_m,{\mathbf{p}}_1,\cdot \cdot \cdot ,{\mathbf{p}}_n{)}^{\mathrm{T}}$$

那么误差函数${\mathbf{z}}_{ij}-h({\mathbf{\xi }}_i,{\mathbf{p}}_j)$就可以写成$f(\mathbf{x})$
则
$$f(\mathbf{x}+\Delta \mathbf{x})\approx \sum _{i=1}^m\sum _{j=1}^n({\mathbf{e}}_{ij}+\sum _{k=1}^m\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{\xi }}_k}\Delta {\mathbf{\xi }}_k+\sum _{l=1}^n\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{p}}_l}\Delta {\mathbf{p}}_l)$$

其中$\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{\xi }}_i},\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{p}}_j}$的具体形式上面§1.2已经讨论过。

下面来推导

$$f(\mathbf{x}+\Delta \mathbf{x})\approx f(\mathbf{x})+\mathbf{J}\Delta \mathbf{x}$$

之中的$\mathbf{J}$的形式

过程略繁琐，我稍微写得详细点
记

$$\begin{gathered} {\mathbf{x}}_c=({\mathbf{\xi }}_1,\cdot \cdot \cdot ,{\mathbf{\xi }}_m{)}^{\mathrm{T}}\in {\mathbb{R}}^{6m} \\ {\mathbf{x}}_p=({\mathbf{p}}_1,\cdot \cdot \cdot ,{\mathbf{p}}_n{)}^{\mathrm{T}}\in {\mathbb{R}}^{3n} \\ {\mathbf{F}}_{ij}=(\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{\xi }}_1},\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{\xi }}_2},\cdot \cdot \cdot ,\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{\xi }}_m})\in {\mathbb{R}}^{2\times 6m} \\ {\mathbf{E}}_{ij}=(\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{p}}_1},\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{p}}_2},\cdot \cdot \cdot ,\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{p}}_n})\in {\mathbb{R}}^{2\times 3n} \end{gathered}$$

那么有

$$\begin{gathered} \sum _{k=1}^m\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{\xi }}_k}\Delta {\mathbf{\xi }}_k={\mathbf{F}}_{ij}\Delta {\mathbf{x}}_c \\ \sum _{l=1}^n\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{p}}_l}\Delta {\mathbf{p}}_l={\mathbf{E}}_{ij}\Delta {\mathbf{x}}_p \end{gathered}$$

再记
$${\mathbf{J}}_{ij}=({\mathbf{F}}_{ij},{\mathbf{E}}_{ij})\in {\mathbb{R}}^{2\times (6m+3n)}$$

则有

$${\mathbf{F}}_{ij}\Delta {\mathbf{x}}_c+{\mathbf{E}}_{ij}\Delta {\mathbf{x}}_p={\mathbf{J}}_{ij}\Delta \mathbf{x}$$

继续记

$$\mathbf{J}=\left(\begin{array}{c}{\mathbf{J}}_{11}\\ ⋮\\ {\mathbf{J}}_{1n}\\ ⋮\\ {\mathbf{J}}_{m1}\\ ⋮\\ {\mathbf{J}}_{mn}\end{array}\right)\in {\mathbb{R}}^{2mn\times (6m+3n)}$$

那么

$$\sum _{i=1}^m\sum _{j=1}^n{\mathbf{J}}_{ij}\Delta \mathbf{x}=\mathbf{J}\Delta \mathbf{x}$$

这也就是整合过后的一阶雅克比$\mathbf{J}$的形式

2.2 稀疏性与边缘化

在优化过程中，不论是使用高斯-牛顿法，还是使用LM方法，都会涉及到计算${\mathbf{J}}^{\mathrm{T}}\mathbf{J}$的值
下面对$\mathbf{J}$进行分析，看是否还能简化。

注意矩阵${\mathbf{J}}_{ij}$的实际意义，实际是在相机位姿$i$查看路标$j$所产生的整体误差

但不是所有的位姿$i$中都会看见路标$j$
如下图所示

两个相机位姿与4个路标点
其中相机$C_1$没有看到路标$p_4$，相机$C_2$没有看到路标$p_1$
所以

$${\mathbf{J}}_{14}={\mathbf{J}}_{21}=0$$

故而整合的$\mathbf{J}$可以省去这两项，直接写成
$$\mathbf{J}=\left(\begin{array}{c}{\mathbf{J}}_{11}\\ {\mathbf{J}}_{12}\\ {\mathbf{J}}_{13}\\ {\mathbf{J}}_{22}\\ {\mathbf{J}}_{23}\\ {\mathbf{J}}_{24}\end{array}\right)$$

不会影响$\mathbf{J}\Delta \mathbf{x}$和${\mathbf{J}}^{\mathrm{T}}\mathbf{J}$的值

下面再来看看剩下不是$0$的${\mathbf{J}}_{ij}=({\mathbf{F}}_{ij},{\mathbf{E}}_{ij})$长什么样
其中

$${\mathbf{F}}_{ij}=(\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{\xi }}_1},\cdot \cdot \cdot ,\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{\xi }}_i},\cdot \cdot \cdot ,\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{\xi }}_m})$$

表示的是位姿$i$和路标$j$的误差，对所有位姿的导数
而实际上这个误差只对位姿$i$有影响，对其他位姿的误差为$0$
即$\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{\xi }}_k}{|}_{k\ne i}=0$，也就是

$${\mathbf{F}}_{ij}=(0_{2\times 6},\cdot \cdot \cdot ,0_{2\times 6},\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{\xi }}_i},0_{2\times 6},\cdot \cdot \cdot ,0_{2\times 6})$$

同理，对于路标的误差也有
$${\mathbf{E}}_{ij}=(0_{2\times 3},\cdot \cdot \cdot ,0_{2\times 3},\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{p}}_j},0_{2\times 3},\cdot \cdot \cdot ,0_{2\times 3})$$

所以整个${\mathbf{J}}_{ij}$就只会有两个地方不是0

所以在使用高斯-牛顿法时的${\mathbf{J}}_{ij}^{\mathrm{T}}{\mathbf{J}}_{ij}\in {\mathbb{R}}^{(6m+3n)\times (6m+3n)}$，其实只有4个小块矩阵的位置有值，其余全为0

所以这个$\mathbf{H}$矩阵

$$\mathbf{H}={\mathbf{J}}^{\mathrm{T}}\mathbf{J}=\sum _{i=1}^m\sum _{j=1}^n{\mathbf{J}}_{ij}^{\mathrm{T}}{\mathbf{J}}_{ij}=\left(\begin{array}{cc}{\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)$$

是个稀疏矩阵，大致形式如下（一般相机位姿少，特征点、路标点较多）

我们将矩阵$\mathbf{H}$分块
显然$\mathbf{B},\mathbf{C}$都是对角矩阵
所以，以高斯牛顿法为例，求解方程$\mathbf{H}\Delta \mathbf{x}=\mathbf{g}$就变成

$$\left(\begin{array}{cc}\mathbf{B}& \mathbf{A}\\ {\mathbf{A}}^{\mathrm{T}}& \mathbf{C}\end{array}\right)\left(\begin{array}{c}\Delta {\mathbf{x}}_c\\ \Delta {\mathbf{x}}_p\end{array}\right)=\left(\begin{array}{c}\mathbf{v}\\ \mathbf{w}\end{array}\right)$$

下面使用Schur Elimination（Schur消元）法
将矩阵右上角的$\mathbf{A}$消掉，即左乘矩阵$\left(\begin{array}{cc}\mathbf{I}& -\mathbf{A}{\mathbf{C}}^{-1}\\ 0& \mathbf{I}\end{array}\right)$
（消去左下角的${\mathbf{A}}^{\mathrm{T}}$去求$\Delta {\mathbf{x}}_p$也行）
得：

$$\left(\begin{array}{cc}\mathbf{I}& -\mathbf{A}{\mathbf{C}}^{-1}\\ 0& \mathbf{I}\end{array}\right)\left(\begin{array}{cc}\mathbf{B}& \mathbf{A}\\ {\mathbf{A}}^{\mathrm{T}}& \mathbf{C}\end{array}\right)\left(\begin{array}{c}\Delta {\mathbf{x}}_c\\ \Delta {\mathbf{x}}_p\end{array}\right)=\left(\begin{array}{cc}\mathbf{I}& -\mathbf{A}{\mathbf{C}}^{-1}\\ 0& \mathbf{I}\end{array}\right)\left(\begin{array}{c}\mathbf{v}\\ \mathbf{w}\end{array}\right)$$

$$\left(\begin{array}{cc}\mathbf{B}-\mathbf{A}{\mathbf{C}}^{-1}{\mathbf{A}}^{\mathrm{T}}& 0\\ {\mathbf{A}}^{\mathrm{T}}& \mathbf{C}\end{array}\right)\left(\begin{array}{c}\Delta {\mathbf{x}}_c\\ \Delta {\mathbf{x}}_p\end{array}\right)=\left(\begin{array}{c}\mathbf{v}-\mathbf{A}{\mathbf{C}}^{-1}\mathbf{w}\\ \mathbf{w}\end{array}\right)$$

所以

$$(\mathbf{B}-\mathbf{A}{\mathbf{C}}^{-1}{\mathbf{A}}^{\mathrm{T}})\Delta {\mathbf{x}}_c=\mathbf{v}-\mathbf{A}{\mathbf{C}}^{-1}\mathbf{w}$$

便可求出$\Delta {\mathbf{x}}_c$，继而求出$\Delta {\mathbf{x}}_p$

这样处理求解的原因是因为对角矩阵$\mathbf{C}$的逆可以直接写出，不需要复杂的计算。

至此就算出了一次迭代中的下降路线$\Delta \mathbf{x}$，之后便可以根据高斯牛顿或LM的原理继续往下走了，从而完成优化。