VIO中的IMU预积分

一般的最小二乘问题

一般的最小二乘问题，都可以写成下面的形式：

$\xi = \arg\min\limits_\xi\frac{1}{2}\sum_i\|\mathbf{r}_i\|_{\Sigma_i}^2$

其中，ξ是优化变量，r是残差。对于这样一个最小二乘问题，对应的高斯牛顿求解方程为：

$\mathbf{J^T\Sigma^{-1}J\delta\xi = -J^T\Sigma^{-1}r}$

这里的 $\Sigma^{-1}$ 是信息矩阵，同时也是协方差矩阵Σ的逆。当系统中某个变量的观测噪声相对其他变量较大时，其信息矩阵就越小，意味着该观测越不可信。这里的信息矩阵的绝对值没有意义，相对值才有意义。信息矩阵决定了那个观测更可信，因此，权重矩阵在紧耦合优化中起到了更新权重的作用。

VIO中的BA

VIO的优化问题，对应到上一节中，需要求解的方程是：

$\arg\min\limits_{\chi}(prior+imgerror+imuerror)$

这三个残差项分别是边缘化的先验信息，IMU测量误差，视觉重投影误差。现在只关注IMU测量误差：

$\sum_{i\in B}\rho(\|\mathbf{r}_b(\mathbf{z}_{b_ib_{i+1}},\chi)\|_{\Sigma_{b_ib_{i+1}}}^2)$

从上述表达式中看出，为了构建优化方程，我们需要知道IMU测量误差对于状态量的Jacobian，以及协方差矩阵。

IMU预积分

IMU预积分的目的在于得到相机在相邻两个视频帧之间的运动，作为系统的预测值；同时得到IMU在两个视频帧之间的误差传递(协方差矩阵)，作为VI紧耦合优化时的权重矩阵。

1. 预积分的变量分离

IMU积分公式是求得当前视频帧时刻，IMU在世界坐标系中的位移(p)，旋转(q)以及速度(v)。既然是世界坐标系下的变量，就一定带有从当前IMU坐标系到世界坐标系的变换矩阵。而这个变换矩阵是需要优化的变量，当该变量每次在后端优化更新后，IMU积分公式都需要重新积分。于是，IMU预积分从希望求得当前视频帧时刻IMU的世界坐标系的[p,q,v]，变成了求得当前视频帧时刻的IMU相对于上一视频帧时刻的[p,q,v]。这样就将变换矩阵从积分中分离出来。