(秋招复习)VINS

Overview

Vins应该算得上是自己最熟悉的一个框架了，当初在看完ORB-SLAM2之后，就把VINS代码跟着这个系列的文章快速过了一遍，当然也很早就尝试了跑一些实际采的数据看看效果，当然最开始采集的室外实车数据实在不太理想就是了。不过后来自己做项目的时候，选择了Vins作为基础框架，当然也是出于自己的需求进行了删减与修改，所以整体对Vins的理解可能还是要优于ORB-SLAM2的吧，这里依旧是出于复习的目的，来把整个框架快速过一遍

预处理

图像

针对视觉部分，VINS其实没有做过多的处理，前端做了个很简单的特征提取(cv::goodFeaturesToTrack())然后特征点之间用光流法进行跟踪，当然这里可以选择把逆向光流也加上，我实际使用的效果感觉就是纹理丰富的地区加上了确实有用，纹理不丰富的地方加上了也不太行。。 然后这里是把处理好的特征点(归一化坐标，像素坐标和速度)在一个线程处理好了之后，后端直接用到这些特征值，感觉是比较不错的一种数据处理方式，后来自己很多代码的编写也是受到了这里的影响。

IMU

Vins种的IMU还是发挥了很大作用的，很多人可能会有一个误区，就是IMU在VINS系统里只起到了一个预积分约束的作用，但是其实我觉得不是这样的，IMU是提供位姿预测的，而且是高频的一种传感器，可以提供两自由度(roll/pitch)的绝对姿态，所以我觉得这里理解为VINS里是像简单的IMU积分系统增加了约束可能也不过分

IMU预积分

前面在第一篇复习SAD的文章里有简单记录过另一种预积分手段，包括ORB-SLAM3也是，整体是在SO3上做的，所以符号很简单，而且我觉得推导起来更加丝滑一些， VINS则是在四元数角度对整个预积分进行展开的，所以整体更复杂一些。不过因为两者的推导思路其实是类似的，这里结合任乾大佬早期的文章再做一遍梳理

首先还是预积分的式子是怎么来的，因为我们的目的是把和观测有关的放一边，没关的放一边，所以核心思路就是把无关的量从积分式中分离出来，也就得到了预积分的形式，下面式子中的积分项就是预积分项，当然后边都记成α，β和γ了

这里是用到了中值积分，其实用什么积分我觉得区别不是很大，因为我尝试过用牛老师的机械编排的那套，对我用的这个相对好的mems级别IMU来说，区别也不大

然后整个状态更新就可以用预积分联系起来了，当然这里对零偏的处理都是一样的，都是看作了一种简单的随机过程模型

还有就是一点，因为我们在预积分的时刻其实认为是不边的，也就是i时刻到j时刻我们两个零偏都认为是i时刻的来进行预积分，当然优化后零偏可能就发生了改变，这个时候我们没有做重新的预积分过程，而是看作了一种“修正”，当然这里也是看作了线性的

这里比起直接推导Jacobian，我们先来看一下残差是怎么设计的，前面有说过，预积分可以肤浅地理解为只包含观测值的部分，所以很自然的就有根据观测值和理论计算值的偏差当作残差的这种思路了。当然这里因为整体是基于四元数的，所以旋转的地方要用四元数的虚部

接下来看回预积分的雅可比计算，因为权重在融合中是个很重要的概念，也可以简单理解为，现在这个传感器在融合里发挥了多大的作用，自己之前有写错过，那个时候整个IMU方差都搞到10次方量级了，所以那个时候相当于整个IMU都没有了。。整个方差的计算其实也是一种传递

这里具体的各个雅可比怎么计算就有些复杂，而且过于占据篇幅了，所以这里就不再放了

后端优化

首先要明白我们要优化的量有哪些，Vins用到了滑动窗口优化的方法，所以优化的变量包括n+1个状态，相机和IMU之间的外参（这个可选），时间延迟td（这个也可选）以及窗口内相机对应的地图点的逆深度

• 这里还要额外说一句，硬件同步一定要做。。不然开td估计也没用，我之前因为IMU驱动是自己手撸的，然后相机用的是官方的ros包，没考虑时间同步做出来的效果就很差。。开了td估计就更差了，因为相机和IMU之间的时间差甚至在不停变，后来优化了下IMU的驱动，让IMU采集变均匀了之后效果立马就好了不少

Vins在优化的约束可以分为三种：先验残差约束，IMU预积分约束和地图点重投影误差约束，IMU预积分上边有讲到，然后先验约束主要是后面边缘化的东西，所以这里简单说下视觉约束，视觉约束可以简单理解为，我在第i个相机观察到的点，如果我的位姿、外参都是准的，那我转到j下就应该得到理论上j帧的地图坐标，但由于这中间有着各种误差，所以残差由此而生

这里对残差雅可比的计算也不在记录了，我觉得是要比IMU残差雅可比好计算不少的

边缘化

这个部分我觉得是滑动窗口算法的一个精髓，看回图优化的本质，图优化其实是把一批数据一起进行了一个优化处理，所以这个优化其实就是这一批数据的一个最优，那么如果没有先验的话，其实可以想到，整个轨迹应该会抖来抖去的，因为当前窗口和前面的位姿都没有关系了，所以边缘化就相当于把之前的状态删除的同时，又把约束信息给留下来了一部分
这里先来看一下经典的边缘化流程，我们把要边缘化的部分的量放在一起，然后留下来的部分的量放在一起

上式中的a部分就是我们要消去的部分，对这个式子就是经典的Schur补就好了，这里的第二行就是相当于包含a信息部分的b部分约束了

然后VINS是分成了两种边缘化方式，一种是MARGIN_OLD，也就是把最老帧边缘化掉，这里就是当次新帧为关键帧的时候，所以窗口内关键帧已经很多了，就把最老的，也就是理论上和当前帧联系应该是最少的给删掉了，这里具体的工作还是放一下崔神的文档截图

然后如果次新帧不是关键帧，可以简单认为当前帧和次新帧比较像，这个时候就把次新帧删除就好了，不过因为这个也不是关键帧，众所周知在SLAM里非关键帧的地位就像路边，所以这里直接就把次新帧删除了，也没有用到边缘化什么的，但是这里因为IMU是一个累积的过程，所以IMU信息记得要留下来，不然IMU就不平滑了

关键帧选择

Vins的关键帧策略简单的很，其实也能看出来，VIns在前端对关键帧的依赖实在是不多，所以这里关键帧的计算就用了两帧之间视差的一个概念，利用视差和阈值判断当前的帧到底是不是关键帧

FEJ

FEJ说白了还是对非线性系统强行线性化带来的问题，因为求雅可比的时候是固定在一个状态下进行的求导，但是边缘化后对新的值不会重新展开，所以这个时候两次线性化的展开点就出现了偏差，不过Vins里并没有用到这个策略，给出的原因是用了效果会不好而且没有明显的偏差出现

初始化

这里把初始化放在这么靠后的位置，也是因为认为初始化也是Vins系统里非常重要的一个部分。Vins的初始化可以看作视觉+IMU松组合的方式，也就是先视觉SFM，再和IMU对齐的这样一个流程

这里视觉的部分也是用到了参考帧这个概念，不过这个相对简单一些，就是看哪帧和当前帧匹配（共视点）的特征点数比较多，然后求一个基础矩阵就得到两帧之间的变换矩阵T了，然后这个参考帧就可以当作一个基础了，滑窗内的其他帧的位姿和地图的坐标都是基于这个恢复出来的，这里面还有一些PnP和三角化的东西，基本上就是调用OpenCV库解决的，三角化的话则比较简单，就是根据多个点建立一个最小二乘然后对矩阵SVD做的，这部分相当于就是做了纯视觉的SFM

陀螺零偏修正

这里是把视觉计算得到的旋转（姿态间的“差”）和IMU陀螺积分得到的旋转优化了一下，这里也是放一下崔神的文档

初始化速度、重力和尺度因子

这里其实是解决单目SLAM痛点的核心，因为这里相当于为整个系统提供了一个绝对尺度的信息，这里的核心思路也是把IMU和相机计算得到的值之间的残差构造出来就好

当然后边也是转成了Hx=b的形式然后直接Cholosky分解就可以了

修正重力矢量

接下来是比较抽象的一步，这里是把重力单独修正了一下，用到的就是重力的模长

这一步可以看成，把相机系下的重力真正旋转到了IMU坐标系下的z方向，这样后面的变量就都可以调整到惯性系下了

个人看法

Vins的初始化步骤当然非常好，考虑到了各种因素，然后对各个量都做了相对严格的估计。但我在实际应用中感觉这种初始化可能更适合无人机这种多轴激励很大的设备？实际上要知道单目VIO的初始化目的就可以简单理解为为系统提供据对尺度信息，所以工程上如果可以利用一些外部条件，这里也是可以很快pass掉，这里就不再详细记录了

闭环检测与修正

最后还是来看一下闭环检测的部分，Vins也是用到了DBoW词袋，这里比较的还是当前帧和关键帧数据库中的帧， 然后这里是加了个快速重定位的步骤，也就是当认为发生闭环的时候，会把检测到的闭环帧作为视觉约束加到当前的优化里，这样就能得到闭环帧和当前帧的位姿关系了，然后我们再根据这个关系对滑窗内的所有帧进行调整。整个闭环优化的话是相对xyz和yaw进行的，所以形式相对简单一些

对比ORB

这里再把两个框架简单对比下，首先就是ORB我觉得可以说是以视觉为核心的解决方案，因为在特征点、匹配包括建图上都有复杂的考虑；而VINS更像是一种以IMU为核心的解决方案，虽然IMU的约束只是简单的预积分，但是整个系统的基础。而且VINS用的光流相比之下，对纹理的依赖还是会低一些的，虽然在稀疏场景下照样会失效。同时VINS的优化部分也可以看到要比ORB少很多，这就注定了运行效率上会高一些，之前我也试过跑同样的数据ORB甚至明显不实时了的现象。但ORB优于优化部分更全面，考虑因素更多，所以在理想场景下的精度应该是要优于VINS的，但是如果想跑自己的数据，我建议还是自己选择一套熟悉的进行魔改，需要什么加什么，这样才能真正应付你需要使用的场景