【学习笔记】Vin-Mono论文阅读笔记（一）

VINS-Mono 概述

VINS-Mono

VINS-Mono是由一个单目相机和一个低成本IMU组成的鲁棒通用的单目视觉惯性系统。通过融合预积分的IMU测量值和特征观测值来获得高精度的视觉惯性里程计，在结合闭环检测和图优化，构成了一个完整的单目VIO-SLAM系统。

VINS-Mono包含以下的特性：

可以从未知的状态进行初始化，，来引导滑窗优化的VIO
紧耦合优化的VIO同时优化IMU
在线重定位功能和4自由度的全局位姿优化
位姿图可以复用，保存，加载以及融合多个局部位姿图

大致流程

1.从单目相机得到视觉图片，对采集到的图片做一次自适应直方图均衡化处理，然后基于FAST算法提取Harris角点特征，这里直接调用了OpenCV中的cv::goodFeaturesToTrack函数，并采用KLT金字塔光流算法进行特征点跟踪。对跟踪到的特征点放入一个队列中，五点法求出本质矩阵后，用随机一致性采样剔除Outliers。随后进行IMU的预积分，主要是为防止重新传播IMU观测值，加大运算难度。选用IMU预积分模型，从世界坐标系转为本体坐标系。离散状态下采用中值积分的预积分。

2.初始化完成后，采用基于滑窗的非线性优化方法来估计body的状态。其中body的状态包括滑窗中关键帧的IMU状态(位置，速度，旋转，加速度bias和角速度bias)；

3.当系统检测到回环时，在1中提到的代价函数中添加闭环检测得到的视觉测量残差部分，对状态进行重定位。其中闭环检测采用的DBoW2的方法。

4.在闭环检测后，对位姿中会产生漂移的4个参数进行位姿图优化，四个参数为X,Y, Z，yaml。

由于篇幅限制，下面就只介绍边缘化方法和IMU预积分。

视觉处理前端

利用KLT稀疏光流法对每一个新图像存在的特征点进行跟踪。与此同时，检测新的角点特征以保证每幅图像特征的最小数目(100-300)。该检测器通过在两个相邻特征之间设置最小像素间隔来执行均匀的特征分布（尽量使特征分布更加均匀，减少特征重叠，这样可以减少不必要的计算量，并使建图效果更好）。二维特征首先是不失真的，然后在通过外点剔除后投影到一个单位球面上。利用基本矩阵模型的RANSAC算法进行外点剔除。（这一段讲述了视觉图像特征点的提取与剔除）

在此步骤中还选择了关键帧。我们有两个关键帧选择标准。第一是与上一个关键帧的平均视差。如果在当前帧和最新关键帧之间跟踪的特征点的平均视差超出某个特定阈值，则将该帧视为新的关键帧。请注意，不仅平移，旋转也会产生视差。然而，特征点无法在纯旋转运动中三角化。为了避免这种情况，在计算视差时我们使用陀螺仪测量值的短时积分来补偿旋转。注意这种旋转补偿仅仅用于关键帧的选择上，不涉及VINS公式中的旋转计算。为此，即使陀螺仪含有较大的噪声或存在偏置，也只会导致次优的关键帧选择结果，不会直接影响估计质量。另一个标准是跟踪质量，如果跟踪的特征数量低于某一阈值，我们将此帧视为新的关键帧。这个标准是为了避免跟踪特征完全丢失。

IMU预积分推导

为什么需要预积分

解释：因为IMU数据频率往往高于图像的频率，一般都能达到100_{500Hz，而图像往往只有30}60Hz，所以为了获得每个图像帧对应的IMU数据，就需要对两个图像帧之间的IMU数据进行积分，才能实现图像帧和IMU数据的意义配对；另一方面，在图优的框架下，经常需要对历史状态进行更新，如果不使用预积分的话，每当一个状态发生变化时，就需要从头往后，运用每一帧IMU数据进行计算，直至更新完所有的状态量为止，这样显然就过于费时费力。但当我们使用IMU预积分时，当图中某个状态量发生了变化，可以直接通过预积分的值直接更新之后的每个关键帧的状态量，这就高效很多。因此可以形象地把IMU预积分理解为，将连续不断的IMU数据根据关键帧的时间戳进行打断，然后每次只记录前后帧的相对位姿关系。但是在数值上，IMU预积分并不是简单的前后帧间P、V、Q的差值，因为在预积分项的计算中，没有加入重力g。

理解：如上文，一帧图片可能会有大量的IMU数据，但是我所需要的只是一组数据，为了避免数据浪费同时优化运算，就把一帧图像中的IMU数据组合起来进行一次处理（数据提纯），这个处理结果就是预积分。

公式理解与推导

首先，P、V、Q 代表位置、速度、旋转矩阵，

这些东西还是很好理解的，左边表示t+1时刻的P、Q、V ，右边显然就是一些物理公式了，a和w为世界坐标系下的线加速度和角速度，根据IMU模型，有

后面那些乱七八遭的就是考虑现实情况加入的一些误差和噪声，我们代入上式中有：

假如我们从 1 到 2时刻进行积分，然后后端优化，这时候我们可以求出来 p,q,v 优化后的值，那不能求出来就不管了吧，我们要对之前的位姿态修正呀，可是你这一修正，q变了，对应的R变了，那就从新从1 积分到 2，得到一个准确的2对应的位置；OK，咱在往前走，2 到 3， R右变了，在回来重新积分一下，得到准确的3，就这样往复着，很麻烦！这时考虑到计算的简洁性，我们希望所得的预积分只和变化有关而与具体的位姿无关，所以我们左边乘一个矩阵,通过了坐标系的转换，将绝对的坐标姿态转化为了相对的坐标转换，把要积分的项转化了一个形式。即把R的绝对位姿转化成相对位姿，相对于每一帧的绝对位姿

上式右侧则为IMU的预积分值，可以看到，IMU预积分值与IMU的位姿无关，仅仅代表第k帧至第k+1帧的PVQ变化值。也可以从另一个方面进行理解，等式左侧均为系统的优化变量，而等式的右侧则为两帧间的观测值

用表示PVQ的预积分值（与论文保持一致），则图优化中的IMU残差可以表示为

现在三个临时量只和当前的加速度计偏移和陀螺仪的偏移有关。而于他们在世界坐标系的绝对位姿没有任何关系。任何帧的P，V，Q的改变不影响积分过程，因此就不用一直重复积分。这三个变量只和偏移ba,bw有关。更新偏移，由于偏移变化是随时间缓慢变化的，因此可以用泰勒一阶展开

这里我附加一个小知识：若函数不存在原函数，则其定积分不可用公式求得。一般用离散的方法计算其定积分。

所以通过离散积分的方式，可以获取这三个积分项的离散迭代表达：

到这里，我们就可以带入IMU 6个轴的观测值，进行迭代求解，最终解算出这三个积分项；这里的预积分里面还是有IMU偏差的，通常是和视觉联合初始化，可以求出来，补偿一下就行了，更新预积分的值！而不用像以前一样更新PVQ；得到更新后的预积分值后，叠加到以前的PVQ，在转换到世界坐标系下！到这里，我们还不知道这三个积分项的含义是什么，直观上理解，可以认为，bk帧做自由落体运动，以这个帧作为参考系，imu在bk到bk+1这段时间的位移，速度，姿态的变化。

这个预积分的值是我们用公式推算出来的，代表的意思就是b坐标系下， K+1 时刻PVQ 减去 K 时刻PVQ ；我们还有一个预积分的测量值，如上面的（7）；因此imu的残差vins就定义为：

残差是一个向量，最终的残差是一个标量，因此我们就要把向量转化为一个标量。即要乘以他的协方差矩阵。协方差矩阵可以理解为，因为向量中每个元素变化的情况值不一样，有些元素变化得比较大（方差大），有些元素变化比较小（方差小），为了使得把这种变化统一起来，因此需要有一个标准来量化它，就认为是这个协方差矩阵。为了获取协方差矩阵，需要建立一个线性高斯误差状态传播方程，由线性高斯系统的协方差，就可以推导出方程协方差矩阵了，也就是测量状态的协方差矩阵了。

噪声对角线协方差矩阵就是我们需要标定imu的四个参数。两个测量噪声的方差，两个偏置倒数的方差。

得到这个残差项的目的是干吗？当然是为了求解待优化的变化量了，然后补偿，修正；继续下一步的推算、测量、优化....

这个优化的方法常用的有EKF，或者 LM等；

反思与探讨

整体推导完VINS的预积分过程之后，会发现它和经典的IMU预积分[3]，还有ORBSLAM3[5]里的预积分都有些许的差别，总结起来主要有以下几点：

计算角度残差的不同：VINS中的方向残差没有取对数，IMU预积分，还有ORBSLAM3主要是多了一个LOG函数，将李群映射到李代数上，而VINS则是只取四元数的虚部进行比较，精度上可能有一定差距。
协方差更新的方式不同：VINS中使用的是ESKF里协方差的更新方式，而IMU预积分，还有ORBSLAM3则是通过具体的推导给出的协方差更新方式。
角度的表示不同（VINS用的是四元数，而[3][5]用的是SO3

参考链接

VINS-Mono论文翻译：https://blog.csdn.net/yys2324826380/article/details/105046864/ 闭关修炼到走火入魔
总体框架：https://zhuanlan.zhihu.com/p/495837693 Derek Lee
VINS-Mono概述：https://zhuanlan.zhihu.com/p/395332185 阿木实验室
IMU预积分详细推导 https://zhuanlan.zhihu.com/p/496251065 Derek Lee
预积分部分理解：https://zhuanlan.zhihu.com/p/407892357 栗子
预积分部分理解：https://blog.csdn.net/hltt3838/article/details/109528320?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166357670716782417042987%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=166357670716782417042987&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2 他人是一面镜子，保持谦虚的态度
离散积分：https://zhuanlan.zhihu.com/p/393132981 书木东