VINS

港科大的VINS-Mono，是研究视觉与IMU紧耦合的必读算法，感谢网上各路大神们的细心讲解分析。 本文主要参考：

1. 崔华坤老师的公式推导-VINS论文推导及代码解析
2. 邱笑晨：《IMU预积分与公式推导》
   【泡泡机器人原创专栏】IMU预积分总结与公式推导（一）
   【泡泡机器人原创专栏】IMU预积分总结与公式推导（二）
   【泡泡机器人原创专栏】IMU预积分总结与公式推导（三）
   【泡泡机器人原创专栏】IMU预积分总结与公式推导（四）
3. VINS-Mono理论学习——后端非线性优化
4. VINS-Mono中的VIO紧耦合方案
5. VINS-Mono代码分析总结
6. VINS-mono详细解读

边缘化参考贺一家的博客：

1. DSO 中的Windowed Optimization
2. SLAM中的marginalization 和 Schur complement

1 预积分的推导

1.1 离散状态下预积分方程

1.2离散状态下误差状态方程

将高斯噪声项从预积分理想值中分离了出来，使得预积分测量值具有理想值加白噪声的形式？

2 前端KLT跟踪

首先用cv::goodFeaturesToTrack在第一帧图像上面找最强的150个特征点，非极大值抑制半径为30。新的特征点都有自己的新的对应的id。然后在下一帧过来时，对这些特征点用光流法进行跟踪，在下一帧上找匹配点。然后对前后帧中这些匹配点进行校正。先对特征点进行畸变校正，再投影到以原点为球心，半径为1的球面上，再延伸到深度归一化平面上，获得最终校正后的位置。对于每对匹配点，基于校正后的位置，用F矩阵加ransac来筛选。然后再在匹配上的特征点之外的区域，用cv::goodFeaturesToTrack搜索最强的新的特征点，把特征点数量补上150个。

最后，把剩下的这些特征点，把图像点投影回深度归一化平面上，再畸变校正，再投影到球面上，再延伸到深度归一化平面上，得到校正后的位置。把校正后的位置发送出去。

特征点跟踪和匹配，就是前一帧到这一帧的，一帧帧继承下去。或者生成新的特征点。

3 系统初始化

在提取的图像的Features和做完IMU的预积分之后，进入了系统的初始化环节，那么系统为什么要进行初始化呢，主要的目的有以下两个：

• 系统使用单目相机，如果没有一个良好的尺度估计，就无法对两个传感器做进一步的融合。这个时候需要恢复出尺度；
• 要对IMU进行初始化，IMU会受到bias的影响，所以要得到IMU的bias。

所以我们要从初始化中恢复出尺度、重力、每帧速度以及IMU的bias、相机到IMU的外参估计，因为视觉(SFM)在初始化的过程中有着较好的表现，所以在初始化的过程中主要以SFM为主，然后将IMU的预积分结果与其对齐，即可得到较好的初始化结果。
  系统的初始化主要包括三个环节：求取相机与IMU之间的相对旋转、相机初始化(局部滑窗内的SFM，包括没有尺度的BA)、IMU与视觉的对齐。

3.1 相机与IMU之间的相对旋转

这个地方相当于求取相机与IMU的一部分外参。相机与IMU之间的标定非常重要。

3.2 相机初始化

这一阶段的思路就是单目相机的初始化过程，先求取本质矩阵求解位姿，进而三角化特征点，然后PnP求解位姿，不断重复的过程，直到恢复出滑窗内的Features和相机位姿，代码比较清晰。要注意的就是坐标系的和位姿的变换，容易混乱。如以下几个函数：

triangulateTwoFrames ：输入是相机外参(世界到相机)，求解出的3D点是在世界坐标系下。
cv::solvePnP：该API输入是世界坐标系的点，求解出的是世界坐标系到相机坐标系的变换，所以一般需要将结果转置。

3.3 视觉与IMU对齐

视觉与IMU的对齐主要解决三个问题：

• 修正陀螺仪的bias；
• 初始化速度、重力向量 𝑔和尺度因子(Metric scale)；
• 改进重力向量 𝑔的量值；

3.3.1 陀螺仪Bias修正

陀螺仪Bias校正的时候也是使用了一个简单的约束条件：
对于窗口中得连续两帧bkbk​和bk+1bk+1​，已经从视觉SFM中得到了旋转qc0bkqbk​c0​​和qc0bk+1qbk+1​c0​​，从IMU预积分中得到了相邻帧旋转γˆbkbk+1γ^​bk+1​bk​​。根据约束方程，联立所有相邻帧，最小化代价函数（论文式(15)）：
然后采用LDLT分解求得δbw，注意这个地方得到的只是Bias的变化量，需要在滑窗内累加得到Bias的准确值。

3.3.2 初始化速度、重力向量g和尺度因子s

3.3.3 纠正重力向量

4 后端基于滑动窗口的非线性优化实现紧耦合

后端优化是VINS-Mono中除了初始化之外，创新性最高的一块，也是真真的 紧耦合 部分，而初始化的过程事实上是一个 松耦合。因为初始化过程中的状态量并没有放在最底层融合，而是各自做了位姿的计算，但是在后端优化的过程中，所有优化量都是在一起的。

• 状态向量和代价函数
  状态向量包括滑动窗口内的所有相机状态（位置P、旋转Q、速度V、加速度偏置ba、陀螺仪偏置bw）、相机到IMU的外参、所有3D点的逆深度：
  这三项分别为边缘化的先验信息、IMU的测量残差、视觉的重投影误差。三种残差都是用马氏距离来表示的，这个在后面ceres的优化残差中要特别注意。
  

4.1 IMU约束

这部分内容主要对应在后端的优化过程中IMU测量部分的残差以及在优化过程中的雅克比矩阵的求解。

4.1.1 残差

这两个式子一样其实

那么IMU测量的残差即可写为

4.1.2 优化变量

优化变量主要包括第i、j时刻的p、q、v、ba、bg：

4.1.3 Jacobian

对ba、bg求导，因为i时刻的bias相关的预积分计算是通过迭代一步步递推的，直接求导太复杂，这里直接对预积分量在i时刻的bias附近用一阶泰勒展开来近似，而不是真的取迭代计算。

递推公式为：
此处推导比较复杂，其实对于VIO的非线性优化中残差的Jacobian的推导，包括IMU预积分增量递推方程的推导，看起来很复杂，其实就是在明确被求导的对象，以及关于什么状态量求导后，如果是和位姿（四元数）有关，就把旋转转换到李群上，用扰动模型去推，如果是其他的，就直接用状态量(或状态量的误差增量)求偏导。
主要参考：
VINS-Mono理论学习——后端非线性优化
VINS-Mono代码分析总结
【泡泡机器人原创专栏】IMU预积分总结与公式推导（四）

接下来推导关于第i、j帧PVQ的Jacobian，对于pqv的求导可以直接采用对误差增量进行计算：

V————————————————————
P————————————————————
Q————————————————————
写成矩阵形式————————————————————

4.1.4 协方差

4.2 视觉约束

4.2.1 残差

相机测量误差万变不离其宗，还是要会到像素坐标差或者灰度差(光度误差)。Vins-Mono中的相机测量误差本质还是特征点的重投影误差。
定义为一个特征点在归一化相机坐标系下的估计值与观测值的差。估计值即特征点的三维空间坐标(x,y,z)T，观测值为其在相机归一化平面的坐标。

逆深度参数化：采用逆深度λ来表示特征点在归一化相机坐标系下的坐标：

以逆深度作为参数的原因，一是因为一些观测到的特征点深度值可能会非常大，难以进行优化；二是可以减少实际优化的参数变量；三是逆深度更加服从高斯分布。
以上是对针孔相机模型的视觉残差，而在VINS论文中其实显示的是对一般相机模型的视觉残差，即使用了广角相机的球面模型。

最后定义的视觉残差为：

4.3 闭环修正与优化

4.4 系统边缘化

4.4.1 边缘化的定义和目的

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

• 滑窗内的pose和feature个数是有限的，在系统优化的过程中，势必要不断将一些pose和feature移除滑窗。
• 如果当前帧图像和上一帧添加到滑窗的图像帧视差很小，则测量的协方差(重投影误差)会很大，进而会恶化优化结果。LIFO导致了协方差的增大，而恶化优化结果？
  直接进行边缘化而不加入先验条件的后果：
• 无故地移除这些pose和feature会丢弃帧间约束，会降低了优化器的精度，所以在移除pose和feature的时候需要将相关联的约束转变为一个先验的约束条件作为prior放到优化问题中
• 在边缘化的过程中，不加先验的边缘化会导致系统尺度的缺失(参考[6])，尤其是系统在进行__退化运动__时(如无人机的悬停和恒速运动)。一般来说只有两个轴向的加速度不为0的时候，才能保证尺度可观，而退化运动对于无人机或者机器人来说是不可避免的。所以在系统处于退化运动的时候，要加入先验信息保证尺度的客观性。
  以上就可以描述为边缘化的目的以及在边缘化中加入先验约束的原因。

4.4.2 两种边缘化的措施