深蓝学院从零开始手写VIO（二）——VIO中的BA问题

声明：本专栏文章为深蓝学院《从零开始手写VIO》课程个人学习笔记，更多学习资源请咨询深蓝学院相关课程。

最小二乘问题的求解

最小二乘问题指的是对于$n$维变量$\mathbf{x}\in {\mathbb{R}}^n$，寻找一个最优值${\mathbf{x}}^{\ast }$使得代价函数（cost function）$F(\mathbf{x})$取最小值，而代价函数通常具有如下形式：
$$F(\mathbf{x})=\frac{1}{2}\sum _{i=1}^n{f}_i(\mathbf{x}{)}^2\text{\hspace{1em}(1)}$$

式中$f_i(\mathbf{x})$称为残差函数，且$m\ge n$。在大多数应用中，$F(\mathbf{x})$的导数是难以直接求导的，所以我们通常会寻求将其写为泰勒级数形式，从而通过求解一系列的线性方程获得极值点的位置。

此外，式（1）也可以写为矩阵形式，注意这里的$f_k(\mathbf{x})$为了方便我们认为其是标量函数，若$f_k(\mathbf{x})$为矢量函数，后续结论依旧成立，只需做对应的矢量改写即可：
$$\begin{array}{rl}F(\mathbf{x})& =\frac{1}{2}{\mathbf{f}}^T(\mathbf{x})\mathbf{f}(\mathbf{x})\\ \mathbf{f}(\mathbf{x})& =\left[\begin{array}{c}{f}_1(\mathbf{x})\\ f_1(\mathbf{x})\\ ⋮\\ f_m(\mathbf{x})\end{array}\right]\end{array}$$

牛顿法

我们自然可以对代价函数$F(\mathbf{x})$求解其在初始迭代点${\mathbf{x}}_0$处的泰勒级数：
$$F({\mathbf{x}}_0+\Delta \mathbf{x})=F({\mathbf{x}}_0)+\mathbf{J}\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^T\mathbf{H}\Delta \mathbf{x}+o(\Vert \Delta \mathbf{x}{\Vert }^3)\text{\hspace{1em}(2)}$$

式中$\mathbf{J}$为$F(\mathbf{x})$的一阶导数矩阵，称为Jacobian矩阵；$\mathbf{H}$为$F(\mathbf{x})$的二阶导数矩阵，称为Hessian矩阵。在标量函数中，一阶导数为0的点是函数的极值点，极值点中二阶导大于零的为极小值点，二阶导小于零的为极大值点，这一结论同样可以扩展到矢量函数中：

• 使得$\mathbf{J}=0$的${\mathbf{x}}^{\ast }$点为稳定点；
• ${\mathbf{x}}^{\ast }$点处的Hessian矩阵为正定矩阵，则该点为极小值点；
• ${\mathbf{x}}^{\ast }$点处的Hessian矩阵为负定矩阵，则该点为极大值点；
• ${\mathbf{x}}^{\ast }$点处的Hessian矩阵为即非正定矩阵亦非负定矩阵，则该点为鞍点；

我们对式（1）求一阶导数有：
$$\frac{\partial }{\partial \Delta \mathbf{x}}\left(F({\mathbf{x}}_0)+\mathbf{J}\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^T\mathbf{H}\Delta \mathbf{x}\right)={\mathbf{J}}^T+\mathbf{H}\Delta \mathbf{x}\text{\hspace{1em}(3)}$$

令一阶导数为零，则我们可以获得迭代步长$\Delta \mathbf{x}=-{\mathbf{H}}^{-1}{\mathbf{J}}^T$，，由于这里我们求解到了二阶Hessian，因此该方法称为二阶牛顿法；此外，若我们只求解一阶Jacobian矩阵，该方法即为我们熟悉的最速下降法，又称一阶牛顿法。

此外，在牛顿法中，有时为了避免$\Delta \mathbf{x}$过大，对添加一个惩罚项限制$\Delta \mathbf{x}$的步长，即求解一阶导数的函数变为：
$$\frac{\partial }{\partial \Delta \mathbf{x}}\left(F({\mathbf{x}}_0)+\mathbf{J}\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^T\mathbf{H}\Delta \mathbf{x}+\frac{1}{2}\mu \Delta {\mathbf{x}}^T\Delta \mathbf{x}\right)={\mathbf{J}}^T+\mathbf{H}\Delta \mathbf{x}+\mu \Delta \mathbf{x}\text{\hspace{1em}(4)}$$

式中$\frac{1}{2}\mu \Delta {\mathbf{x}}^T\Delta \mathbf{x}$称为阻尼项，添加阻尼项之后的迭代步长变为$\Delta \mathbf{x}=-(\mathbf{H}+\mu \mathbf{I}{)}^{-1}{\mathbf{J}}^T$。

高斯牛顿法

在牛顿法中，我们是通过对代价函数$F(\mathbf{x})$进行泰勒展开来获得导数零点的，但这一方式中二阶导数Hessian矩阵$\mathbf{H}$的求解是较为繁琐且复杂的，因此我们可以采用另一种方式，即对残差函数$\mathbf{f}(\mathbf{x})$做泰勒展开（为书写方便，这里采用了矩阵写法）：
$$\mathbf{f}({\mathbf{x}}_0+\Delta \mathbf{x})=\mathbf{f}({\mathbf{x}}_0)+\mathbf{J}\Delta \mathbf{x}$$

将其带入代价函数并求取一阶导数有：
$$\begin{array}{rl}\frac{\partial }{\partial \Delta \mathbf{x}}F(\mathbf{x})& =\frac{\partial }{\partial \Delta \mathbf{x}}\frac{1}{2}\left((\mathbf{f}({\mathbf{x}}_0)+\mathbf{J}\Delta \mathbf{x}{)}^T(\mathbf{f}({\mathbf{x}}_0)+\mathbf{J}\Delta \mathbf{x})\right)\\ & =\frac{\partial }{\partial \Delta \mathbf{x}}\left(\frac{1}{2}F({\mathbf{x}}_0)+\Delta {\mathbf{x}}^T{\mathbf{J}}^T\mathbf{f}({\mathbf{x}}_0)+\frac{1}{2}\Delta {\mathbf{x}}^T{\mathbf{J}}^T\mathbf{J}\Delta \mathbf{x})\right)\\ & ={\mathbf{J}}^T\mathbf{f}({\mathbf{x}}_0)+{\mathbf{J}}^T\mathbf{J}\Delta \mathbf{x}\end{array}\text{\hspace{1em}(5)}$$

令一阶导数有${\mathbf{J}}^T\mathbf{J}\Delta \mathbf{x}=-{\mathbf{J}}^T\mathbf{f}({\mathbf{x}}_0)$，该方程即为最小二乘问题中的正则方程（normal equation）。若$\mathbf{J}$满秩，则${\mathbf{J}}^T\mathbf{J}$正定，此时式（5）一定有非零解，即随时函数一定存在最小值（当然实际情况中很少有$\mathbf{J}$为正定的，其通常为半正定）。

LM法（阻尼牛顿法）

类比于牛顿法中的阻尼项，在高斯牛顿法（GN）中，我们同样可以对其添加阻尼项以抑制过大的迭代补偿，添加阻尼项之后的正则方程为：
$$({\mathbf{J}}^T\mathbf{J}+\mu \mathbf{I})\Delta \mathbf{x}=-{\mathbf{J}}^T\mathbf{f}({\mathbf{x}}_0)\text{\hspace{1em}(6)}$$

阻尼因子$\mu$具有如下性质：

• $\mu$一定大于零，以保证式（6）所示的正则方程一定有非零解；
• 当$\mu$很大时，$\mu \mathbf{I}\gg {\mathbf{J}}^T\mathbf{J}$，此时正则方程近似于一阶牛顿法，即$\mu \mathbf{I}\Delta \mathbf{x}=-{\mathbf{J}}^T\mathbf{f}({\mathbf{x}}_0)$；
• 当$\mu$很小时，$\mu \mathbf{I}\ll {\mathbf{J}}^T\mathbf{J}$，此时正则方程近似于不存在阻尼的高斯牛顿法；

可以看到，对于LM方法来说，最重要的莫过于阻尼因子$\mu$的选取，一个简单的初值选取策略为选择${\mathbf{J}}^T\mathbf{J}$中最大的对角线元素的比例，即：
$$m{u}_0=\tau \cdot \max (({\mathbf{J}}^T\mathbf{J}{)}_{ii}),\tau \sim [10^{-8},1]$$

而后，通过比例因子$\rho$调节每一步的阻尼，比例因子$\rho$定义为代价函数的实际下降值与近似下降值之间的比值：
$$\rho =\frac{F(\mathbf{x})-F(\mathbf{x}+\Delta \mathbf{x}))}{L(\mathbf{x})-L(\mathbf{x}+\Delta \mathbf{x})}$$

式中$L(\mathbf{x})$表示式（5）中使用泰勒展开近似的$F(\mathbf{x})$函数。记$\mathbf{b}=-{\mathbf{J}}^T\mathbf{f}(\mathbf{x})$，同时由于$L(\Delta \mathbf{x})$中的$\Delta \mathbf{x}$满足正则方程，因此有：
$$\begin{array}{rl}L(\mathbf{x})-L(\mathbf{x}+\Delta \mathbf{x})& =-\Delta {\mathbf{x}}^T{\mathbf{J}}^T\mathbf{f}({\mathbf{x}}_0)-\frac{1}{2}\Delta {\mathbf{x}}^T{\mathbf{J}}^T\mathbf{J}\Delta \mathbf{x}\\ & =\frac{1}{2}\Delta {\mathbf{x}}^T\left(-2{\mathbf{J}}^T\mathbf{f}(\mathbf{x})-\left({\mathbf{J}}^T\mathbf{J}+\mu \mathbf{I}-\mu \mathbf{I}\right)\Delta \mathbf{x}\right)\\ & =\frac{1}{2}\Delta {\mathbf{x}}^T\left(2\mathbf{b}-\mathbf{b}+\mu \mathbf{I}\Delta \mathbf{x}\right)\\ & =\frac{1}{2}\Delta {\mathbf{x}}^T\left(\mathbf{b}+\mu \Delta \mathbf{x}\right)\end{array}$$

从而我们可以根据当前的$\rho$值调节阻尼因子$\mu$，Marquardt和Nielsen分别提出了两种策略，其中Nielsen策略震荡较小，为g2o和Ceres中采用的策略：
$$\{\begin{array}{rl}& \mu =\mu \ast \max \{\frac{1}{3},1-(2\rho -1{)}^3\},\nu =2;\rho >0\\ & \mu =\mu \ast \nu ,\nu =2\ast \nu ;otherwise\end{array}$$

鲁棒核函数

鲁棒核函数又称为M-Estimation，其作用是为了防止在残差函数的计算中存在野值（outlier）将估计结果大幅度拉偏，附加鲁棒核函数之后的最小二乘的代价函数变为：
$$F(\mathbf{x})=\underset{\mathbf{x}}{\min }\frac{1}{2}\sum _k\rho (\Vert f_k(\mathbf{x}){\Vert }^2)$$

记${\mathbf{s}}_k=\Vert f_k(\mathbf{x}){\Vert }^2$，进而对$\rho ({\mathbf{s}}_k)$进行二阶泰勒展开并求和，既而求解一阶导数零点，最终可以得到：
$$\begin{array}{rl}\sum _k{\mathbf{J}}_k^T\left({\rho }^{\prime }\mathbf{I}+2{\rho }^{\prime \prime }{f}_k(\mathbf{x})f_k^T(\mathbf{x})\right){\mathbf{J}}_k\Delta \mathbf{x}& =-\sum _k{\rho }^{\prime }{\mathbf{J}}_k^T{f}_k(\mathbf{x})\\ \sum _k{\mathbf{J}}_k^T{W}_k{\mathbf{J}}_k\Delta \mathbf{x}& =-\sum _k{\rho }^{\prime }{\mathbf{J}}_k^T{f}_k(\mathbf{x})\end{array}$$

将上式写成矩阵形式有：
$$\begin{array}{rl}\left[\begin{array}{cccc}{\mathbf{J}}_1^T& {\mathbf{J}}_2^T& \cdots & {\mathbf{J}}_k^T\end{array}\right]\left[\begin{array}{cccc}{W}_1& & & \\ & W_2& & \\ & & \ddots & \\ & & & W_k\end{array}\right]\left[\begin{array}{c}{\mathbf{J}}_1\\ {\mathbf{J}}_2\\ ⋮\\ {\mathbf{J}}_k\end{array}\right]\Delta \mathbf{x}& =-\left[\begin{array}{cccc}{\rho }_1^{\prime }{\mathbf{J}}_1^T& {\rho }_2^{\prime }{\mathbf{J}}_2^T& \cdots & {\rho }_k^{\prime }{\mathbf{J}}_k^T\end{array}\right]\left[\begin{array}{c}{f}_1(\mathbf{x})\\ f_2(\mathbf{x})\\ ⋮\\ f_k(\mathbf{x})\end{array}\right]\\ \Rightarrow {\mathbf{J}}^T\mathbf{W}\mathbf{J}\Delta \mathbf{x}& =-\mathbf{b}\end{array}$$

式中对角矩阵$\mathbf{W}$称为鲁棒加权系数矩阵，若$f_k(x)$为标量函数，则其对角线元素$W_i$为标量；若若$f_k(x)$为矢量函数，则其对角线元素$W_i$为对角矩阵。

VIO残差的构建

加权最小二乘代价函数

在基本的最小二乘中，我们认为所有变量的权值是一致的，但在实际应用中，不同的变量对于残差项的影响是不同的，自然我们应该更为相信误差更小的变量，而不相信误差更大的变量，即我们可以通过对不同变量赋予不同的权重以期获得更好的估计效果，这一过程称为加权最小二乘估计，其代价函数如下：
$$F(\mathbf{x})=\frac{1}{2}\sum _{i=1}^n\mathbf{f}(\mathbf{x}{)}^T{\mathbf{\Sigma }}^{-1}\mathbf{f}(\mathbf{x})=\Vert \mathbf{f}(\mathbf{x}){\Vert }_{\mathbf{\Sigma }}^2$$

加权估计下的VIO代价函数

VINS_MONO中给出的VIO加权代价函数如下：
$$\begin{array}{rl}ϵ\left(\mathbf{\chi }\right)=\underset{\mathbf{\chi }}{\min }& \{\Vert {\mathbf{r}}_{prior}-{\mathbf{H}}_{prior}\mathbf{\chi }{\Vert }^2+\sum _{k\in \mathcal{B}}\Vert {\mathbf{r}}_{\mathcal{B}}\left({\hat{\mathbf{z}}}_{k+1}^{b_k},\mathbf{\chi }\right){\Vert }_{{\mathbf{P}}_{k+1}^{b_k}}^2\\ & +\sum _{\left(l,j\right)\in \mathcal{C}}\rho \left(\Vert {\mathbf{r}}_{\mathcal{C}}\left({\hat{\mathbf{z}}}_l^{c_j},\mathbf{\chi }\right){\Vert }_{{\mathbf{P}}_l^{c_j}}^2\right)\}\end{array}$$

式中$\Vert {\mathbf{r}}_{prior}-{\mathbf{H}}_{prior}\mathbf{X}{\Vert }^2$为先验残差，由边缘化给出；$\Vert {\mathbf{r}}_{\mathcal{B}}\left({\hat{\mathbf{z}}}_{k+1}^{b_k},\mathbf{X}\right){\Vert }_{{\mathbf{P}}_{k+1}^{b_k}}^2$和${\mathbf{r}}_{\mathcal{C}}\left({\hat{\mathbf{z}}}_l^{c_j},\mathbf{X}\right){\Vert }_{{\mathbf{P}}_l^{c_j}}^2$分别为IMU残差和视觉残差；同时，为了减少视觉残差变化过大引起的系统不稳定，使用如下所示的Huber函数$\rho \left(s\right)$提供阻尼：
$$\rho \left(\cdot \right)=\{\begin{array}{rl}& ss⩽1\\ & 2\sqrt{s}-1s>1\end{array}$$

逆深度下的视觉重投影误差

记一个空间点$\mathbf{f}$在相机坐标系$\mathbf{C}$下的坐标为${\mathbf{f}}^{C_i}=[x^{C_i},y^{C_i},z^{C_i}{]}^T$，则其在归一化相机坐标系$\mathbf{c}$下的坐标为${\mathbf{f}}^{c_i}=[x^{c_i}/z^{c_i},y^{c_i}/z^{c_i},1{]}^T=[u^{c_i},v^{c_i},1{]}^T$，因而我们有：
$${\mathbf{f}}^{C_i}={\mathbf{z}}^{C_i}{\mathbf{f}}^{c_i}=\frac{1}{\lambda }{\mathbf{f}}^{c_i}$$

这里参数$\lambda$由于是深度${\mathbf{z}}^{C_i}$的逆，因此称为逆深度。现在我们假设特征点$\mathbf{f}$在第$i$帧下的用逆深度表示的齐次相机坐标为${\mathbf{f}}^{C_i}=[\frac{1}{\lambda }{u}^{c_i},\frac{1}{\lambda }{v}^{c_i},\frac{1}{\lambda },1{]}^T$，其在第$j$帧下的重投影坐标可以表示为：
$$\left[\begin{array}{c}{x}^{C_j}\\ y^{C_j}\\ z^{C_j}\\ 1\end{array}\right]={\mathbf{T}}_{bc}^{-1}{\mathbf{T}}_{w{b}_j}^{-1}{\mathbf{T}}_{w{b}_i}{\mathbf{T}}_{bc}\left[\begin{array}{c}\frac{1}{\lambda }{u}^{c_i}\\ \frac{1}{\lambda }{v}^{c_i}\\ \frac{1}{\lambda }\\ 1\end{array}\right]$$

进而视觉重投影误差可定义为：
$${\mathbf{r}}_{reproject}=\left[\begin{array}{c}\frac{x^{C_j}}{z^{C_j}}-u^{c_j}\\ \frac{y^{C_j}}{z^{C_j}}-v^{c_j}\end{array}\right]$$

预积分形式的IMU误差

VIO中IMU的误差项通常采用预积分项来描述，具体推导可参见IMU预积分类，这里直接给出中值积分离散后的结果：
$$\{\mathbf{\alpha }$$