VINS-MONO原理笔记（基于计算机视觉life课程）

本文章是基于计算机视觉life课程关于论文的讲解做的相关笔记，建议搭配论文阅读

IV 预处理

A 视觉处理

对图像使用KLT角点检测算法，对角点使用LK光流（在金字塔上逐层进行）追踪(用光流计算最小化光度误差来进行追踪)。对已有特征点进行追踪时，不需要在新一帧图像上提取特征点,但当特征点数较少时，会额外提取特征点来维持一定的数量。每张图片维护大约100-300个特征点。检测出特征点时还会进行均匀化操作（对应于ORB:使用的是四叉树，对应于VINS：每检测出特征点，对特征点进行排序，认为被追踪的越久的特征点稳定性更好，质量更高。以特征点为中心画圈，在圈中不允许再有其他的特征点存在）。提取完特征点后要去畸变，去畸变使用了一个trick：逐渐逼近式去畸变。通过对极约束求一个本质矩阵，使用RANSAC选取一个置信度最高的本质矩阵，对outlier（不符合对极约束的点）进行筛查。

如何选关键帧：

1. 视差（对每个追踪的关键点计算$\delta$t,计算平均值，比阈值大时）
2. 追踪质量（追踪到的特征点数量小于阈值时）

B IMU预积分

• （1）从连续时间基于四元数的预积分开始，进行了一系列建模处理。假设零偏的导数符合高斯分布
• （2）使用旋转向量描述协方差矩阵
• （3）零偏的修复:因为零偏是随机游走的，所以需要实时估计出来。我们认为零偏的变化非常小，所以可以对它进行一阶线性展开近似，求取它的一阶雅克比矩阵。（当$\delta$t很大时，一阶线性近似就失效了，就要重新计算，例如IMU初始化结束的时候）但在滑窗就可以这样使用,极大节约计算成本

V 初始化

A 纯视觉struct of motion

对滑窗所有帧做纯视觉单目SFM。首先，对连续两帧图像通过对极约束（五点法+RANSAC）恢复初始的位姿（带有不准的尺度），这两帧需要满足拥有足够多的特征点（超过30个），且特征点的平均视差要足够大（避免纯旋转)(超多20个像素）。

最后做一个FULL BA。

优化之后得到带尺度的3D点位姿和相机坐标。代码中把相机第四帧作为参考帧（和论文不同）

B 视觉IMU对齐

IMU通过预积分得到旋转，相机通过对极约束得到旋转。两个旋转之间差一个旋转外参。对旋转外参构建一个超定方程来进行求解。

• 平移外参没有优化，因为非常难求解。
• 加速度的零偏对最后的优化结果影响非常小，还参杂重力，基于松耦合建模算法不可观，也没有求解。

（1）求解陀螺仪零偏:求解一个超定方程

（2）速度，重力，和尺度

把旋转和重力方向对齐。借助预积分的约束，把已知量放到一边，未知量放到另一边，构造AX=B的方程。累计很多帧构造超定方程。
未知量：
$v_{b_k}^{b_k}:$d第k帧图像时IMU坐标系下的速度；$g^{c_0}$:第0帧相机坐标系下的重力向量；s：视觉SFM恢复到米制尺度下的比例

（3）重力调整：
重力的先验信息：重力的大小

具体的修正步骤：

所有重力的大小是不变的，在第二步基础上对重力的方向进行一些调整:在切向空间描述两个b1.b2。实现把对重力的描述由之前的三自由度变为在b1,b2方向上的两自由度的问题。将每次的重力根据b1，b2进行迭代调整（借用第二步重力的初始值，只用了方向，大小使用了先验信息（该维度的重力大小））。

（4）完全初始化：把第一帧图像帧的姿态调整到与重力对齐。（由于我加了重力，所以我们对roll角和pitch角是已知的，但是我们对垂直重力方向的yaw角是未知的，所以我们就把第一帧的yaw角置为0，并对其他帧做依次调整。至此初始化的过程全部结束了。

VI 紧耦合

基于滑窗的紧耦合的VIO

A formulation

滑窗优化的变量：每一帧的位置，速度和姿态（p，v，q），零偏(b)，外参（x），地图点（λ,维护的是地图点的逆深度，绑定滑窗观测到该特征点的第一帧来作为逆深度的参考帧，当滑窗中没有任何关键帧能看到该地图点时将其抹掉，不同于ORBSLAM中的全局三维地图点）

滑窗模型一共有三种残差：

• 第一个是边缘化，提供一个先验的约束；
• 第二个是预积分，提供一个帧间约束（两帧之间状态量的约束）；
• 第三个是视觉重投影，基于间接的,基于特征点的经典约束项。因为视觉重投影会存在误匹配，所以需要使用$huber$核函数（当误差逐渐变大，其造成的影响会逐渐降低）

B IMU的约束

IMU的残差，即IMU的帧间约束。如果系统只使用IMU来进行状态递归，那么(25)式的等号是成立的。但是我们还融入了视觉重投影的约束，所以每次优化下式的状态值（位置，速度，位姿）都会发生改变。当然原则上$\alpha ，\beta ，\gamma$作为观测值是不会改变的，以此会形成一个约束，使状态值如论如何改变都不会离观测值太远。

对式（16），因为$\gamma$计算的旋转残差是基于误差模型的，这里只取它的虚部。因为四元数虚部是1/2个旋转向量，所以前面要乘2。

关于零偏的建模:因为IMU的零偏在一段时间内是稳定的，认为两帧的零偏不会差太远，所以对两帧的零偏作差做为一个简单的约束。

C 视觉重投影的约束

将上一帧观测到的特征点恢复到三维世界，再通过求出的状态量投影到当前帧图像的平面，与实际的特征点做残差计算雅克比矩阵。

D 边缘化

因为滑动窗口的大小是确定的，当来新的一关键帧时需要剔除老帧。利用其对已有状态量的约束固定优化问题，形成一个先验的约束，约束滑窗里其他的状态量。（舒尔补）

E 有限资源平台下权衡

不优化地图点只优化当前位姿

F IMU速率状态估计的IMU正向传播

以IMU的频率发送里程计信息：用最新的滑窗位姿和IMU数据做一个简单的积分，把当前时刻的位姿发送出去。

VII 重定位

回环检测模块，借用了ORB的回环检测方式，回环检测后进行了一个四自由度的优化。

A 回环检测

使用了词袋模型DBoW2，在后端额外提取了$500$个$fas$t角点并计算了特征描述子，然后丢弃原图片来节约内存的空间。

B 特征索引

所有描述子转成词袋的$node$，做一个历史数据库。每来一个新的关键帧，会提取其角点，计算描述子，查询在历史数据库中是否有与它相似度比较高的关键帧。查询到关键帧的话会进行一系列的$check$（描述子匹配，几何校验2D-2D对极约束+RANSAC，3D-2D PNP+RANSAC）

C 紧耦合重定位

把回环相关的约束增加到总约束方程中，并不是为了约束滑窗中的位姿，而是借助滑窗来帮助我得到更精确的当前帧和回环帧之间的$△pose$。

VII 全局优化

A 四个累积漂移方向

四自由度全局优化，认为$roll$和$pitch$是绝对可观的，不需要去优化。而$x,y,z$,和$yaw$是绝对不可观的量，所以我们对$x,y,z$,和 $yaw$进行一个四自由度的优化。

B 在位姿图中添加关键帧

连续边：表示两关键帧的相对位姿，直接取自VIO
回环边：将回环边的误差分散到各个边中

四位姿图优化

本身两帧之间会算出一个$△pose$，$△pose$的边（约束）是不会动的，而约束两头的变量是会变得，每一个边都会约束这条边两头的变量的误差,保证其不能离约束太远。

(20)式上半部分是关于位移的约束，下半部分是关于$yaw$角的约束。
由于回环边可能会有误匹配，所以加入核函数来降低优化的风险

所有的观测都是$△pose$，所有的优化变量都是位姿。以这样的优化模型进行优化。位姿优化和重投影以两个线程分别进行。

D 位姿图合并

如果我们有一个已知的先验地图，我们可以以回环的方式，把已知的地图融合到当前的地图中来。

E 位姿图的保存

视觉地图的保存只保存一个txt的文本文件。

• $i$：$frame$的索引。
• $p，q$：VIO的位姿
• $v$：有回环的话，对应回环帧的索引
• ${\hat{\mathbf{p}}}_{iv}^i,{\hat{\psi }}_{iv}$：回环帧的位姿
• $D$：所有特征点和描述子
• $u，v$：特征点的二维坐标
• $des$：描述子