VINS-MONO的非线性优化

需要优化的变量如下：

系统的代价函数如下：

其中三个残差项依次是：

• 边缘化的先验信息
• IMU测量残差
• 视觉的观测残差

1）视觉的观测残差
视觉测量残差就是重投影误差，定义为一个特征点在归一化相机坐标系下的估计值与观测值的差。视觉的观测误差可表示如下：

参考该博客，推导上述公式。在这里，我们使用了逆深度λ作为参数，将特征点的三维坐标的z坐标值归一化为1。

使用逆深度作为参数有以下原因：
1）观测到的特征点深度可能非常大，难以进行优化；
2）可以减少实际优化的参数变量；
3）逆深度更加服从高斯分布。
从第i帧的特征点坐标投影到第j帧的相机坐标系下坐标值的转换关系如下：

其中T_bc表示从陀螺仪坐标系到相机坐标系的转换矩阵，T_wbi表示从世界坐标系到陀螺仪坐标系的转换。

最后做一点补充说明：我们将重投影和测量值都归一化为单位向量，将视觉残差为两个单位向量的差。因为视觉残差的自由度为2，再将视觉残差投影到正切平面上，b1，b2为正切平面的两个正交基。求出该残差后，可以对两个时刻的状态量、外参，以及逆深度进行求导，从而得到雅克比矩阵，
2）IMU的观测残差
IMU的观测误差在该博客推导的较清楚。简单来说，如果每一帧IMU都要相对于第一帧求位姿，计算量很大，因此考虑将PVQ积分公式改为相对第i时刻的姿态。最后可以得到观测残差为：

求出上述残差后（该残差为第k+1时刻的位姿信息减去第k时刻的位姿信息与预积分之和），对IMU的第i、j时刻下的p位置，v速度，Q旋转（PVQ），两个偏置ba,bw（利用IMU预积分残差求得的雅克比矩阵）进行求导便可求出其雅克比矩阵，从而对上述参数优化。
3）边缘化的先验信息
传感器的时间对齐示意图如下（截自该博客）：
市面上的视觉与惯性元件如果时钟相同，那么可以在硬件上做到时间对齐，此时没有延时，效果最好；否则，只能通过软件对齐的手段处理视觉与IMU信息。
在VINS-MONO中，使用了滑动窗口进行非线性优化，使得优化问题始终在一个有限的复杂度内，不会随时间不断增长。

边缘化的过程就是将滑窗内的某些较旧或者不满足要求的视觉帧剔除的过程，所以边缘化也被描述为 将联合概率分布分解为边缘概率分布和条件概率分布的过程(就是利用shur补减少优化参数的过程)。

直接进行边缘化而不加入先验条件的后果：

• 无故地移除这些pose和feature会丢弃帧间约束，会降低了优化器的精度，所以在移除pose和feature的时候需要将相关联的约束转变为一个先验的约束条件作为prior放到优化问题中
• 在边缘化的过程中，不加先验的边缘化会导致系统尺度的缺失(参考[6])，尤其是系统在进行退化运动时(如无人机的悬停和恒速运动)。一般来说 只有两个轴向的加速度不为0的时候，才能保证尺度可观，而退化运动对于无人机或者机器人来说是不可避免的。所以在系统处于退化运动的时候，要加入先验信息保证尺度的可观性

VINS-MONO的边缘化方法分为如下两种情况，可参考该博客：

• 当次新帧为关键帧时，将marg掉最老帧及其看到的路标点和相关联的IMU数据，将其转化为先验信息加到整体的目标函数中:
• 当次新帧不是关键帧时，我们将直接扔掉次新帧及它的视觉观测边，而不对次新帧进行marg，因为我们认为当前帧和次新帧很相似，也就是说当前帧跟路标点之间的约束和次新帧与路标点的约束很接近，直接丢弃并不会造成整个约束关系丢失过多信息。但是值得注意的是，我们要保留次新帧的IMU数据，从而保证IMU预积分的连贯性。
  
  边缘化的图解如上，其过程可用如下公式表示：
  
  详细推导与过程建议阅读视觉SLAM十四讲的内容。