vio-视觉与IMU融合实现里程计（第三章）——基于优化的IMU和视觉信息融合

1. 基于Bundle Adjustment的VIO融合
   
   通过调整q,p,使得预测值与观测值的平方误差或带权重的平方误差最小。$\sum$范数是调节各个误差分量的权重的矩阵_BA问题。
   归一化平面：三维的空间中的点投影到相机平面上，就是像素坐标，是二维的，当三维空间中的点投影到像素平面时，首先会打到归一化平面上，再从归一化平面通过单位(米)的映射投影到像素平面(像素单位）。
   
   
   相机坐标系和IMU(body）坐标系之间可能有一个相对外参(R,t)，图像采集的频率是30HZ，IMU采集到的频率是200HZ，使得相邻两张图像之间有多个IMU数据，需要通过预积分的方式，将两个相邻相机间多个IMU数据当作一个IMU来处理

2.最小二乘的求解

J代表梯度，Jd<0,最速下降法d的方向取J梯度的反方向

阻尼法：在牛顿法的基础上加$\mu$限制$\Delta x$不能太大，如果走的太大就不能用二阶展开对其进行更好的近似，阶数越高近似程度会变差的，需要受函数的形状限制不能走太远。




$\mu$和最大的特征值$\lambda$是同等数量级的，$\mu$的初始值的设定和JTJ对角线元素上的最大特征值和$\tau$有关， KaTeX parse error: Undefined control sequence: \tao at position 1: \̲t̲a̲o̲的选取取迭代前的初始值x0如果与最优值x*比较近，此时用二阶泰勒展开能很好的近似系统，所以用高斯牛顿去求解，此时阻尼要小一点所以选10的-8次方。如果初始值离最优值远，则阻尼取大点(广义的韦达定理)

$\Delta x$越小，就会有对损失函数越好的近似。理解上述更新策略依据：阶数越高近似程度越差。

如果出现outliner，错误的一些数据，如特征匹配的时候匹配了一些错误的数据，导致某个残差项平方后变得很大，不是由于噪声的影响，这个现象是BA中不愿意看到的，所以我们希望当residual大的时候cost不要变的那么块，我们在cost的外面加入了$\rho$, 修饰cost的$\rho$的种类有Huber, Cauchy等。

通过最小化残差达到对状态量（路标点和相机位姿）最优的估计

协方差的引入起权重（该残差是否更可信）的作用，协方差越小，协方差的逆越大，该残差更可信。
两种不同单位的残差相融合的时候，由于单位不同，融合时的物理含义不清晰，所以通过一个协方差的引入可以将不同传感器残差的单位进行归一化（无量纲的范围0～1）
即视觉，IMU，外点的鲁棒核函数进行融合的时候，视觉传感器构成的是重投影误差 ，单位是像素，IMU项构成的是相邻帧之间的约束或者是米字单位的平移的约束（加速度和角速度）。
使用滑动窗口的过程中，假设滑动窗口的状态量只有三个的话，刚开始如上述图所述对C1， C2， C3进行优化，等c4进来的时候为了节省计算量，需要把老的状态量丢掉C1,在丢掉之前我们不能将有用的信息，即观测信息丢掉，需要将其转化为先验信息prior，此时对c2, c3, c4进行有化的时候要加入先验信息进行优化。

$\lambda$表示的是路标点的逆深度，采用逆深度的原因是如果路标点在无群远处，则直接用深度z表示的化不太好表示，而且逆深度更符合高斯分布。