VINS知识点汇总

0、总体框架

        包括5个部分：数据预处理、初始化、后端非线性优化、闭环检测、位姿图优化。

图片来自大佬博客：https://blog.youkuaiyun.com/try_again_later/article/details/104854698 

VINS原框架：

图解VINS框架：

闭环检测拆解：

1、数据预处理

        1、视觉 

                1）提取Harris角点，KLT金字塔光流跟踪相邻帧；

                2）2维特征点先矫正为不失真的（畸变校正），通过外点剔除后投影到一个单位球面的；

                3）去除异常点：先进行F矩阵测试，通过RANSAC去除异常点；

                4）关键帧选取：（1）当前帧相对最近的关键帧的特征平均视差大于一个阈值就为关键就为关键帧（因为视差可以根据平移和旋转共同得到，而纯旋转导致不能三角化成功，所以这一步需要IMU预积分进行补偿）；（2）当前帧跟踪到的特征点数量小于阈值视为关键帧；              

        2、IMU 

                1）两帧k和k+1之间进行位置、速度、姿态（PVQ）预测；

                2）避免每次姿态优化调整后重复IMU传播，采用预积分算法，计算预积分误差的雅克比矩阵和协方差项。

2、初始化

        采用松耦合（将IMU定位与视觉的位姿直接后处理融合）的传感器融合方法得到初始值。

        首先利用SFM进行纯视觉估计滑动窗口内所有帧的位姿以及路标点逆深度（步骤1）；

        然后与IMU预积分对齐（步骤2），继而恢复①对齐尺度s，②重力g，③IMU速度v和④陀螺仪偏置bg。

        VINS初始化过程中忽略了加速度的bias偏置ba，因为加速度计与重力耦合，并且重力向量很大，初始化过程动态过程很短，幅度又不大，加速度偏置很难观测到。

        初始化的过程：是为视觉和IMU“牵线搭桥”的过程。视觉计算的轨迹和IMU计算的轨迹，需要“门当户对”才能凑成一对，方能“儿孙满堂”：重力、尺度、bg、速度。

2.0 初始化代码

        具体的实现可以在VINS代码中查找下面几个函数：

 1. relativePose：计算当前帧跟参考帧的相对位姿。（步骤1）

2. GlobalSFM::construct：SFM计算滑窗内的路标点和相机位姿。（步骤1）

3. cv::solvePnP：计算所有位姿（步骤1，上面三步都是视觉sfm的过程）

4. visualInitialAlign：视觉和IMU对齐。（步骤2，这是真正的对齐过程）

2.1 初始化视觉与IMU对齐流程（visualInitAlign()函数过程详解）

        这部分内容来自深蓝学院《手写VIO》第7讲，首先要归结出视觉和IMU之间的联系，则利用外参数q_bc，t_bc构建等式，几何约束如下：

其中，s为尺度因子，表示非米制单位的轨迹。等式（3）等价于：

视觉与IMU对齐流程如下：

         接下来就按照这个步骤进行初始化工作：

2.1.1 利用旋转约束估计外参数旋转q_bc 

具体代码见：initial_ex_rotation.cpp函数CalibrationExRotation()。

2.1.2 基于旋转约束的Gyroscope Bias 

        

具体代码见：initial_alignment.cpp函数solveGyroscopeBias()。函数在solve_5pts.cpp中，作用是利用旋转约束估计陀螺仪bias Bg，并根据新的bg重新预积分。

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

        

  

具体代码见：initial_alignment.cpp函数LinearAlignment()。函数在initial_alignment.cpp文件中。

2.1.4 优化重力向量 

        在其切线空间上用两个变量重新参数化重力，采用球面坐标进行参数化。

其中，|| g || 是已知重力的大小，为重力方向的单位向量。b1和b2是跨越切平面的两个正交基。w1和w2是待优化变量，表示沿着两个正交基方向的位移。

替换后，Hx=b，变化为：之后采用最小二乘对变量重新优化。

2.1.5 将相机坐标系对齐世界坐标系 

        对齐流程：

2.2 滑动窗口（Sliding Window） 纯视觉SFM

       上面的过程全部是步骤2：视觉sfm恢复出的位姿和IMU预积分得到的PVQ对齐的过程。那么这一节主要记录视觉SFM是怎么得出位姿和点的逆深度。

        1. 选择一个滑动窗，在最后一帧与滑动窗中第一帧中寻找参考帧：跟踪到的点数目大于30个的并且视差超过20的，找到后用5点法（其实使用RANSAC）求本质矩阵E，然后分解E矩阵恢复出R和t。

        2. 

2.3 视觉惯性校准（IMU预积分与纯视觉结构对齐）

       函数调用关系如下：

Estimator::processImage()

Estimator::initialStructure()

        

2.3.1 陀螺仪零漂bg标定

        旋转两种方式：陀螺仪测量值和视觉观测值，二者的误差其实就是其实就是陀螺仪偏置bg。

目标函数：visual给出的相邻帧间的旋转应等于IMU预积分的旋转值Q之间的差。

考虑到了陀螺仪bias噪声，我们得到了陀螺仪偏置bias的初始校准，需要将陀螺仪偏置bg代入到IMU预积分重新计算预积分。

2.3.2 速度v、重力g和尺度初始化s

2.4 重力矢量修正

        为什么需要优化重力向量？

        利用如下公式求解重力向量过程中，并没有加入模长限制||  || = 9.81。三维变量实际只有两个自由度。

重力向量参数化

---

3、紧耦合后端非线性优化（IMU约束+视觉约束+先验约束）

3.1 公式

3.2 IMU残差

3.3 视觉残差

3.4 边缘化 Marginalization

        为了防止pose和特征的个数的复杂度随着时间不断增长，引入边缘化，在移出位姿时将关联的约束转化为先验放入优化问题中。

        为了限制基于优化的VIO计算复杂度，引入边缘化。有选择性地从滑动窗口中将IMU状态xk和特征λ1边缘化，同时将对应于边缘状态的测量值转换为先验。

分为两种情况： 

        1、一种是新来帧是关键帧的话（与倒数第二帧相比追踪到的点数少于20个或者平均视差大于30，视差够大可以保证特征点三角化），将最旧的pose移出SLiding window，将最旧帧关联的视觉和惯性数据边缘化掉。把第一个老关键帧及其测量值边缘化，Margin_Old作为先验值。

         2、如果新来帧与倒数第二帧差别不大（追踪到上一帧点数较多，并且平均视差小于30），那么剔除倒数第二帧的视觉观测，拼接IMU约束。原因是边缘化保证关键帧之间有足够视差而能够三角化足够多的地图点，并且保证了IMU预积分的连贯性。

        

为了保持系统的稀疏性，我们不会边缘化非关键帧的所有测量值。

3.5 相机速率下的状态估计——只有运动的VIO

4、重定位（relocalization）

4.1 回环检测（只对关键帧）

4.2 回环候选帧之间的特征匹配

4.3 紧耦合重定位

5、全局位姿图优化

5.1 位姿图中添加关键帧

5.2 4自由度位姿图优化

5.3 位姿图管理

6、IMU预积分

6.1 IMU模型

6.2 连续时间IMU运动模型，积分PVQ（两帧之间）

6.3 运动模型的离散积分

6.4 IMU预积分

6.5 预积分量

6.6 预积分误差

6.7 预积分离散形式（IMU增量）

6.8 bias预积分量（bias发生变化）

7、基于舒尔补的边缘化

7.1 舒尔补

7.2 marg后形成的先验

7.3 具体例子

7.3.1 原来信息矩阵H的构成

7.3.2 舒尔补后形成新的信息矩阵new_H,并构造为先验

7.3.3 新测量信息和先验构成新的系统