从VINS-MONO到VIO学习（三）

BA算法

    Bundle Adjustment算法,也被称为光束平差法，具体的算法就是使用最小二乘法求解重投影误差，同时对pose和landmark进行优化。
    算法的输入为landmark和pose，以及相机所拍摄图像的像素坐标。

    算法计算的是重投影误差，顾名思义就是将三维点投影到相机坐标系下，通过最小化“计算出的相机系坐标”和“测量出的相机系坐标”之间的投影误差，来进行位姿的优化。误差函数构造如下：

    其中 $\pi$是投影函数，是将一个3D点坐标从世界坐标系转化到对应的相机坐标系，$Z_{f_j}^{c_i}$是表示观测到的结果，这里的最小二乘使用的是马氏距离的写法，${\Sigma }_{i,j}$是一个范数，是误差的协方差矩阵。

VIO系统的后端优化

    但是对于VIO系统来说，我们引入了IMU数据，同时对于IMU来说也需要进行误差的优化，所以要在BA之中加入IMU的误差同时进行优化，但是遇到的问题是如何对IMU的误差进行构建，以及和视觉信息的误差之间的权重如何设置，以及最后的最小二乘问题如何进行计算。

非线性最小二乘法

    如果残差函数为非线性函数，则需要先对其进行泰勒展开将其近似为线性函数，其中J为残差函数f的雅各比矩阵。

    如此一来将其带入损失函数，我们可以得到损失函数：

    通过上式我们可以发现损失函数被近似成了一个二次函数，对于这种二次函数，只要我们保证$J^{T}J$是正定的，即可保证该函数由最小值。并且可知$F^{\prime }(x)\approx (J^{T}f{)}^T$ $F^{\prime \prime }(x)\approx J^{T}J$。带入标准最小二乘法的公式之中，我们可以得到：

LM算法

    高斯牛顿法存在缺陷，就是它要求我们所用的矩阵是可逆的（而且是正定的），但实际数据中计算得到的$J^{T}J$却只有半正定性。也就是说，在使用Gauss Newton方法时，可能出现$J^{T}J$为奇异矩阵或者病态的情况，此时增量的稳定性较差，导致算法不收敛。更严重的是，就算我们假设H非奇异也非病态，如果我们求出来的步长△x太大，也会导致我们采用的局部近似不够准确，这样一来我们甚至都无法保证它的迭代收敛，哪怕是让目标函数变得更大都是可能的。
    因为高斯牛顿法是要进行线性近似的，这种线性近似只有在近似点附近才能保持良好的线性性质，所以我们可以对下降的过程加一个阻尼，能够保证下降的速度既不过快导致局部近似不准确，也不会过慢导致迭代次数明显增加。


   我们经常使用的Marquardt策略如下所示：

   Ceres和g2o使用的Nielsen策略如下所示：

鲁棒核函数

    鲁棒核函数是为了处理outlier，防止outlier对优化的结果造成太过巨大的影响，具体的作用方式为：

    其中$\rho$表示的是核函数，鲁棒核误差的二阶泰勒展开有：

求解过程

VIO残差函数的构建

    残差函数主要是由三个部分组成，marg的先验，IMU的残差，图像的重投影误差。后端BA优化的状态向量为$\chi$，如下所示：

1. 视觉重投影误差

    对于视觉重投影误差来说，其定义就是特征点在归一化平面之上估计值和观测值之间的差。

2. IMU误差

    对于IMU来说，我们已经对其进行了数学建模，知道对于IMU来说误差是高斯白噪声以及测量器件的bias。

    我们可以利用IMU的测量值获取位置，速度，姿态，具体的计算方式如下：

    从上面的公式我们可以看出来我们要计算j时刻的的$pvq$，需要从i时刻开始，对i时刻到j时刻所有的IMU数据进行积分，得到j时刻的$pvq$。
    但是这样就会出现一个问题，后端优化的过程中，会对$q_{w{b}_t}$进行优化，也就是说这个量对应的数值会改变，那么就有了一个问题，因为$pvq$是通过积分的形式更新的，如果一个$q_{w{b}_t}$被优化改变数值，那么积分就要重新计算，才能得到优化后的$pvq$，牵一发而动全身，这会需要极大的计算量。
    所以引入了预积分的方法，将$q_{w{b}_t}$转化为一个固定的$q_{w{b}_i}$叉乘上一个增量$q_{b_i{b}_t}$，其模型为：

    通过上面的预积分改变，我们可以得到如下所示的$pvq$公式：
    公式可以分为两个部分，一个部分为对i时刻的正常计算，第二个部分为i到j时刻的积分增量，第二个部分也叫预积分部分，其可以表示为：

    最后整理出的$PVQ$公式为：

    通过上述公式构建预积分误差公式，如下所示：

    对应的预积分离散形式：

    对应的预积分的协方差：

    确定了误差传递的状态量，噪声量：

    构建残差方程，其中F，G为协方差传递矩阵，公式分为两个部分，第一部分表示上一时刻的误差值，第二部分表示对应的高斯白噪声。

残差Jacobian

Jacobian详细推导