vins-mono算法学习

超爱吃小蛋糕的66

已于 2023-06-12 09:02:53 修改

阅读量3.7k

点赞数 16

分类专栏： VIO学习文章标签：算法学习计算机视觉

于 2023-02-28 21:50:41 首次发布

本文链接：https://blog.csdn.net/guanjing_dream/article/details/129270442

版权

VIO学习专栏收录该内容

10 篇文章 3 订阅

订阅专栏

文章目录

简介
1.VINS-mono的主要贡献
2.VINS-mono的主要流程简介
3.传感器信息预处理之视觉前端
4 传感器预处理之IMU预积分
5.VIO初始化过程
6. 紧耦合的单目VIO
7 针对手机等设备对算法进行的轻量化处理
8 故障检测
8 重定位模块
9. 全局位姿图优化
10. 传感器的时延估计

简介

VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator
VINS-Mono：一种鲁棒且通用的单目视觉惯性状态估计器

1.VINS-mono的主要贡献

提供在线方式进行摄像机IMU外部校准和IMU陀螺仪偏置校正;
在VIO基础上基于DBow2词袋库进行回环检测;
基于IMU和视觉特征的紧耦合实现重定位;
支持地图的保存和加载
故障检测：实时检测slam跟踪状态，及时对系统进行初始化，进行位姿矫正

2.VINS-mono的主要流程简介

算法框架主要分为数据预处理、系统初始化、基于滑窗的VIO过程和回环检测等四个模块。

在数据预处理模块主要包括图像直方图均衡、KLT特征点提取以及去畸变处理和光流跟踪，IMU部分主要是做一个预积分操作。
系统初始化首先进行纯视觉的sfm，在当前滑窗中寻找视差(20个像素)和匹配点(30)比较多的两帧图像做初始化，主要是基于对极约束得到一个不带尺度信息的R和t，然后三角化出一组3D点。最后通过pnp求解出其他几帧图像对应的位姿。
在视觉初始化完成之后会对相机和IMU之间的旋转外参、陀螺仪偏置信息进行一个基于松耦合信息的在线标定，然后对每帧图像对应的速度、尺度信息和重力加速度进行基于对齐，最后将重力加速度在单位空间切平面分解成由两个单位正交基表达形式，重新带入残差函数进行优化求解。
基于滑窗的VIO过程就是正常的VIO过程，残差函数由边缘化得到先验残差、视觉重投影残差、IMU残差三部分组成。
回环检测模块主要基于DBOW2词袋库实现，对于每个关键帧会提取500个特征点并计算描述子然后转化为词袋向量，通过词袋向量寻找回环关键帧。在建立特征匹配关系后会依次基于RANSAC求解本质矩阵和进行PND，目的是为了筛选外点，如果内点数量满足要求则检测到了回环，通过将回环部分的残差加入到残差函数进行4自由度位姿图优化。

3.传感器信息预处理之视觉前端

视觉前端使用KLT光流进行Shi-Tomasi角点检测和跟踪，在跟踪过程中为了保证特征点的稳定性，当跟踪到的特征点较少时，除了跟踪到的特征点之外还会额外在当前帧提取一些特征点以保证跟踪质量。

对于检测到的特征点进行均匀化操作和去畸变处理，最后通过对极约束基于RANSAC迭代计算本质矩阵并排除外点。

代码入口函数在feature_track.cpp->readImage()，主要包括以下步骤：

直方图均衡化预处理：针对图像过亮或者过暗的情况，提升图像对比度，方便提取角点
LK光流跟踪
提取新的特征点(如果需要将特征点信息发送给后端)
对所有特征点进行去畸变处理

3.1 KLT光流跟踪算法原理

LK光流法旨在说明对图像像素的一种光流跟踪算法，根据三大假设联立方程组最终将问题转换为求解最小二乘的问题，即AX=b的形式。由于Hessian矩阵的特征值都比较大时才能满足空间一致性的假设条件，即在选择角点作为关键点时符合这一情况。

实际上，一方面像素移动并不会那么简单，另一方面窗口内像素并不都是同样的移动方式，因为这样的近似必然会带来误差。而现在的问题就是如何去选择合适的窗口，或者特征点，从而获得最为精确的跟踪。KLT角点检测方法就是为了选择一个适合跟踪的特征点，它认为一个好的特征点的定义应当就是能被好的跟踪。

在opencv中集成的goodFeaturesToTrack函数即使用的Shi-Tomasi角点检测。

3.2 视觉前端KLT光流跟踪流程

LK金字塔光流算法是一种经典的稀疏光流算法，该算法有三个假设条件：亮度恒定、小运动、邻域空间一致。

亮度恒定即灰度一致性：同一个空间点的像素灰度值，在各个图像中是固定不变的
小运动即时间连续性：相邻帧间保持微小的运动，时间的变化不会引起像素的剧烈变化
空间一致性：特征点及其周围邻域的像素均具有相同的运动

LK光流跟踪过程：

图像金字塔处理
基于图像的灰度一致性和空间一致性，可以联立方程迭代求解使得特征点在初始图像像素块和匹配图像像素块之间的像素差值最小。
在求得特征点在顶层金字塔图像的坐标后，基于顶层图像中的速度和梯度作为下一层金字塔图像光流跟踪的初值，直到计算出原始图像的光流。

3.3 什么是金字塔光流

图像金字塔式为了避免在图像帧间运动过大时，光流跟踪算法陷入局部最优解锁使用的一种跟踪技巧。

在跟踪分辨率较大的时，估计值和真值之间的的差值可能会比较大，在进行迭代求解时容易在真值的附近得到一个局部最小值。金字塔光流的原理就是，对图像进行resize为分辨率更小的图像，并在这个图像上进行跟踪，得到真正的全局最优解。

在跟踪过程中，LK光流算法首先基于最顶层金字塔图像进行跟踪，然后依次向下一层金字塔图像继续进行跟踪，直到得到特征点在原始图像上的像素坐标。

这样对于一个n层金字塔，由对原图像进行1次光流跟踪变为了n次光流跟踪，在提高跟踪精度的同时也增加了计算资源，因此这里需要对实时性和精度进行权衡(在vins-fusion中引入了一些预测机制)。

3.4 在跟踪过程中对外点的筛选

光流跟踪会删除没有被跟踪到的特征点
在光流跟踪后会删除超出图像边界的点
在下一帧跟踪到特征点后，通过对极约束原理，基于RANSAC计算一个本质矩阵同时筛选外点

3.5 vins-mono特征均匀化处理方式

对特征点进行排序，认为被追踪的越久的特征点稳定性更好，质量更高。然后依次以特征点为圆心划定邻域范围，然后忽略该特征点邻域范围内的其他特征点。

3.6 vins-mono中去畸变方法

参考链接：https://blog.csdn.net/qq_33733356/article/details/124356158
畸变的程度越靠近图像边缘越高，这点是VINS中去畸变的主要依据思想。VINS采用一种逐渐逼近式去畸变方法.
在这里插入图片描述

根据畸变强度越靠近图像边缘越大 AA’ > BB’ > CC’

已知含有畸变的A'坐标，需要求出发生畸变前的真实坐标A
根据畸变公式可以由真实坐标得到包含畸变坐标是比较容易计算的，反之要复杂很多。
因此VINS中将已知的A'当做真实的坐标B，通过畸变公式得到畸变后的B'。得到畸变强度（近似表示）BB'，从A'移动BB'，得到更加靠近A的C点。然后以C继续推导出更靠近A点的D。反复迭代，即可得到在接受范围内无限靠近A的点。

3.7 vins-mono中关键帧创建方法

主要根据与上一帧的视差和当前跟踪质量进行判断

视差：当前帧和上一关键帧要有足够的旋转和平移视差，旋转视差通过陀螺仪角速度积分得到，平移视差通过对所有匹配特征点算平均位移得到
跟踪质量：如果跟踪的特征数量低于一定阈值，则将当前帧设置为新的关键帧。

3.8 光流跟踪和描述子匹配对比浅谈

1.耗时对比
对于光流跟踪，假设每帧要求提取100个特征点，因为光流可以直接跟踪到上一帧大部分的特征点(假如是N个)，因此对于帧间跟踪只需要花费提取(100-N)个特征点时间以及光流跟踪的时间。
对于描述子匹配，则每帧都需要计算提取100个特征花费的时间以及描述子匹配时间

2.性能对比
LK光流相比描述子匹配较鲁棒一些，但基于描述子的方法在遮挡，运动速度较快的情况下性能更佳。

光流跟踪更适合作帧间匹配，描述子匹配更适合于重定位和回环检测。

4 传感器预处理之IMU预积分

在本节由IMU在连续时刻的积分推导得到离散时刻下关于PVQ的预积分量计算，然后对vins中关于误差卡尔曼滤波方式的分析(即误差代价函数采用相邻时刻运动的估计值与相邻时刻的IMU预积分量之间的差值)，然后从连续时刻下IMU状态误差推导出基于中值积分的离散时刻IMU状态的误差传递，并对预积分中关于IMU零偏的建模进行了解析，最后推导了相邻时刻下预积分协方差矩阵的传递。

4.1 连续时间的PVQ积分

连续时间的关于pvq的积分公式如下：
在这里插入图片描述

对于t时刻加速度积分的处理，首先将当前加速计读数减去当前时刻加速度零偏，减去高斯测量噪声；然后将t时刻的加速度变换到世界坐标系(在VIO系统中，世界坐标系一定是和重力方向对齐的，因此将加速度转到世界系下之后可以直接减去重力的影响)；最后再减去重力得到t时刻最终的加速度。

在IMU预积分公式中使用四元数表示旋转，首先了解一下四元数的乘法。两个四元数p和q相乘可以得到一个新的四元数，过程如下：
在这里插入图片描述

根据上式可以将两个四元数相乘写成一个[4x4]矩阵和[4x1]向量相乘的形式。
在这里插入图片描述

根据上式可以推导出四元数求导过程：
在这里插入图片描述

最终k+1时刻的旋转q_k+1可以由K时刻的旋转q_k乘以Δt时刻旋转的积分得到,其中w为陀螺仪测得的角速度
在这里插入图片描述

4.2 连续时刻下的预积分量计算

不同于滤波，在优化问题中往往需要对当前时刻位姿以及前面多个时刻的位姿进行优化，因此在前一时刻的位姿发生变化时后面时刻的运动状态需要重新进行积分，这会带来过多的计算量，此外在VIO系统中优化的是相邻两个图像帧之间的运动，但由于IMU频率远远大于图像帧的频率，因此需要提前将两相邻帧时刻对应的IMU数据进行积分得到相应的位移速度和旋转，才能参与优化。

对于上述连续时刻的积分，通过将参考坐标系由世界坐标系转为局部坐标系时，可以得到局部坐标系下的预积分表示：
在这里插入图片描述

在这里插入图片描述

其中α，β，γ分别为k到k+1时刻位移，速度和旋转的预积分量。

4.3 离散时刻下的预积分

但实际上我们所获得的测量都是基于离散时刻下的，因此在计算相邻时刻的预积分时需要将连续时刻的预积分转为离散时刻下的预积分量。在vins-mono算法中采用中值积分进行离散化。
在这里插入图片描述

对于公式中的上下标问题，因为预积分时求相邻两个时刻的位移，速度和旋转，因此所有的预积分都是相对于k时刻下的变换。

4.4 误差卡尔曼滤波

在vins系列中对于优化时的误差函数定义与常规(ORB-SLAM)定义略有不同，对于k到k+1时刻，基于估计会得到关于k+1时刻的状态(包括相对位移，速度，旋转和偏置)，k+1时刻和k时刻之间状态的差值记为估计得到的相对运动，然后根据IMU预积分可以得到一个k时刻到k+1时刻相对运动的测量值，vins优化的误差函数是求解使得估计的相对运动和测量的相对运动(预积分)最小时对应的k+1时刻的状态。

为什么要使用误差卡尔曼滤波？？
（1）使用4自由度的四元数描述3自由度的旋转会引起过参数化，因此使用旋转向量直接描述旋转，一般来说旋转向量具有周期性(即旋转角度在-pi,pi之间变化)，引入误差卡尔曼，这时的旋转误差会在0附近，避免了这种周期性
（2）针对旋转，可以保证误差在0附近变换，可以避免万向节死锁等问题
（3）误差量较小，可以忽略其二阶导数，只计算一阶雅各比

4.5 连续时间IMU状态误差状态推导

IMU误差关于各状态的导数：
在这里插入图片描述

以下求导参考链接：https://blog.csdn.net/weixin_45626706/article/details/118074274

误差函数对位移部分的导数：
在这里插入图片描述

误差函数对于速度的导数：
在这里插入图片描述

误差函数对于旋转的导数：
在这里插入图片描述

在这里插入图片描述

误差函数对于偏置的导数：
在这里插入图片描述

4.6 基于中值积分的离散时间预积分误差传递

在这里插入图片描述

具体为：
在这里插入图片描述

离散时间角度误差的推导
在这里插入图片描述

离散时间速度误差的推导
在这里插入图片描述

在这里插入图片描述

离散时间位移误差的推导
在这里插入图片描述

离散时间零偏误差的推导
在这里插入图片描述

4.7 预积分中关于IMU零偏的建模

在IMU积分过程中的计算都会涉及到零偏，因为VINS-MONO紧耦合的VIO,其零偏也是一个待估计量,这意味着在优化问题求解过程中,每次优化都可能会改变零偏使满足整体优化最小的条件。这会带来一个问题:每一次优化都需要重新计算积分。

在vins中采用对零偏一阶近似的方法对预积分量进行更新
f(x+Δx) = f(x) + J(x)Δx = 原先预积分的结果 + 相对于零偏的一阶雅克比 * 补偿量
在这里插入图片描述

求解预积分量关于零偏的雅各比
在这里插入图片描述

4.8 预积分协方差矩阵的传递

假设a符合高斯分布,其方差为A。对a乘以系数k, 则ka的方差为k^2A
若a为向量,则其协方差为A的矩阵
若F矩阵乘以a向量,其协方差为FAF^T
若向量a的协方差矩阵为A,向量b的协方差矩阵为B,则向量a+向量b的协方差矩阵为A+B矩阵

在这里插入图片描述

5.VIO初始化过程

5.1 需要进行初始化的状态量

其中x_k是捕获第k图像时的IMU状态。它包含了IMU在世界坐标系中的位置、速度和方向PVQ，以及在IMU本体坐标系中的加速度计偏置和陀螺仪偏置。
x_bc为IMU与相机坐标系之间的外惨，旋转外惨通过松耦合的方式初始化，平移外参可以通过测量得到，并不需要一个非常准确的外参,因为在非线性优化中会不断去优化
λl是第一次观测到第l个特征的逆深度。
n是相邻两帧图像帧之间IMU状态的数量，m是特征点的数量

5.2 不进行初始化的状态量

平移外参
对于平移外参来说，需要加速度两重积分得到且本身带有噪声，基于这种带有噪声的平移估计得到的外参反而不如直接测量精确，通过测量IMU与相机之间的平移外参的误差可以控制在几个毫米之内。此外在后面的非线性优化时会不断的优化外参数。
加速计零偏
加速度计bias很难从重力估计中分离出来,忽略加速度计bias不会显著影响结果。在算法中将初始的零偏设置为0，然后在非线性优化过程中不断更新这个值。

5.3 IMU和相机之间旋转外参的标定

IMU和相机之间的旋转外参使用一种类似松耦合的标定算法，对于k时刻到k+1时刻，首先可以通过纯视觉SFM可以恢复出两帧之间的旋转运动R_c,同时基于IMU测量可以得到k时刻到k+1时刻的预积分R_b，则对于任一时刻满足以下公式：
在这里插入图片描述

在经过若干帧之后可以分别得到一组相邻帧之间的视觉旋转，以及对应的IMU预积分旋转，通过这种约束关系构建最小二乘问题可以求解出相机与IMU之间的旋转外参。

5.4 纯单目视觉初始化

对当前滑窗中的图像帧进行纯单目SLAM，恢复出不带尺度信息的相机位姿和3D点

1.在滑窗的图像帧中找到存在足够多的特征点(30)和足够大的视差(20x像素)的两帧图像，假设是第4帧和第10帧
在这里插入图片描述

2.根据对极约束原理，基于RANSAC迭代求解可以计算第4帧和第10帧的运动(R,t)，并恢复出不带尺度信息的3D空间点
在这里插入图片描述

3.根据求得的3D点对滑动窗口的其他帧通过pnp求解位姿(不带尺度)，同时恢复更多的3D点(不带尺度)
在这里插入图片描述

4.基于ceres自动求导最小化重投影误差，得到优化后的不带尺度的位姿

5.5 陀螺仪零偏初始化

首先可以定义在k时刻IMU到c0参考系的旋转和平移:
在这里插入图片描述

具体推导如下:
在这里插入图片描述

对于连续的两帧k帧和k+1帧，根据之前的纯视觉初始化可以分别得到在这两帧时通过估计得到的IMU相对于c0参考系的位姿估计值q_bk_c0和q_bk+1_c0

然后IMU自身可以得到一个两帧之间旋转的测量值γ_bk+1_bk，因为陀螺仪偏置的关系，估计得到的位姿之间的相对旋转和预积分得到的相对旋转不会完全相等，因此可以定义图片第一行的误差模型。
在这里插入图片描述

在vins中对加速计和陀螺仪的零偏误差进行了一阶线性近似，因为在PVQ的预积分过程中都涉及到了偏置的计算，当偏置发生变化时相应的预积分需要重新积分，为了减少计算复杂度，选择将加速计和陀螺仪的偏置对IMU误差进行了一阶线性化处理，这样当偏置发生更新时，只需要将偏执的一阶近似补偿加到原来的预积分结果后面。
在这里插入图片描述

求解过程如下：
在这里插入图片描述

5.6 视觉惯性对齐

构建关于每一帧对应的IMU坐标系速度、尺度和重力加速度
视觉惯性对齐主要干了什么事：截至到目前为止，已经完成了纯视觉下相机在每个时刻的位姿与3D点(不带尺度，位姿没有与重力方向对齐)，得到了相邻帧IMU的预积分量，接下来就是结合视觉估计结果和IMU预积分测量恢复位姿和3D点的尺度，并将位姿对齐到重力方向，同时估计每一帧图像帧对应的速度。

首先定义每一帧对应的IMU坐标系速度、尺度和重力加速度：
在这里插入图片描述

然后可以根据两个连续帧k和k+1帧关于位移和速度的预积分可以得到以下公式：
在这里插入图片描述

在这里插入图片描述

根据上面两个公式可以得到以下线性约束方程
在这里插入图片描述

然后构建最小二乘问题进行求解
在这里插入图片描述

最后得到每一帧对应的IMU坐标系速度、尺度和重力加速度和初值。

重力调整或者重力细化
大部分情况下,重力实际上是已经知道的,我们可以通过查表等方式是可以知道当地的重力加速度的。如果我们求解的实际的重力加速度和重力不同,那么肯定就有一些问题。因此在VINS的初始化中,还需要进行调整。我们借助已知重力加速度大小9.81对估计得到的重力加速度进行调整。我们取出计算出的重力的方向,然后对大小进行调整。

因为重力大小已知，则重力加速度由原来的3个自由度变为2个自由度(原本重力由xyz三个方向上的分量组成，现在对于重力的模长已知，如果知道xy方向的分量则可以求解出z方向的分量)。这时可以将重力在切平面分解为两个正交单位向量b1,b2乘以各自的权重(x,y方向上的分量).
在这里插入图片描述

重力加速度可以由以下公式进行表示：
在这里插入图片描述

然后用这种表示替代之前求解每一帧对应的IMU坐标系速度、尺度和重力加速度时对应的g_c0,通过迭代优化使得重力方向的单位向量收敛。其中，b=[b1,b2],W=[w1,w2]。
在这里插入图片描述

此时可以得到世界坐标系与摄像机坐标系c0之间的旋转q_c0_w。然后我们将所有变量从参考坐标系(·)^c0 旋转到世界坐标系(·)^w。本体坐标系的速度也将被旋转到世界坐标系。视觉SfM的变换矩阵将被缩放到度量单位。此时，初始化过程已经完成，所有这些度量值都将被输入到一个紧耦合的单目VIO中。

6. 紧耦合的单目VIO

6.1 优化状态变量的组成

其中x_k是捕获第k图像时的IMU状态。它包含了IMU在世界坐标系中的位置、速度和方向PVQ，以及在IMU本体坐标系中的加速度计偏置和陀螺仪偏置。
x_bc为IMU与相机坐标系之间的外惨，旋转外惨通过松耦合的方式初始化，平移外参可以通过测量得到，并不需要一个非常准确的外参,因为在非线性优化中会不断去优化
λl是第一次观测到第l个特征的逆深度。
n是相邻两帧图像帧之间IMU状态的数量，m是特征点的数量

6.2 残差函数的组成

基于边缘化得到的先验信息，IMU测量残差，视觉测量残差,在滑窗过程中通过最小化所有的测量残差得到每帧对应的状态。
在这里插入图片描述

6.3 IMU残差的含义

已知K时刻通过优化估计得到的imu位姿，速度，imu到世界坐标系的旋转，以及k时刻到k+1时刻的预积分，求解k+1时刻的位姿，速度和旋转

残差函数里面k时刻的状态pvq和偏置是已知的先验，k+1时刻的状态是待估计量，残差的物理意义是让两帧状态估计值的差值和imu预积分的测量值之间的差值达到最小
在这里插入图片描述

残差主要有5个部分组成：

位移的残差：通过k+1时刻位姿的估计值减去k时刻位姿的估计值得到一个相对位移，Imu测量值预积分会得到一个相对位移，残差即两个相对量的差值
速度以及旋转同理
IMU的偏置残差则是直接使用k+1时刻的残差减去k时刻的残差(与积分将两帧之间IMU数据的加速计和陀螺仪残差视作短暂的不变)

6.4 视觉残差的定义方式

vins采用了一种新的投影误差描述，在单位球面上定义摄像机的测量残差。
在这里插入图片描述

这里的单位球面的切平面应该就是归一化平面，由相机光心到归一化平面上的特征点存在一个向量，通过运动估计由i帧投影到第j帧上的特征点和通过测量得到的第j帧上的特征点，与光心相连会得到两个向量，对这两个向量归一化为单位向量并映射到切平面坐标系(归一化平面)，然后计算映射后向量的差值作为视觉残差

大体上比对极约束(不是ORB-SLAM)视觉残差计算多了一步，将特征点投影到归一化平面后，又将坐标归一化为了单位向量,目的是为了减少噪声和误匹配等对位姿估计带来的影响。

6.5 vins-mono视觉残差和ORB-SLAM的区别

在ORB-SLAM3基于对极约束的位姿求解时，使用了相似的操作。因为噪声、数值的四舍五入、误匹配等情况的存在，直接基于像素点计算得到的基础矩阵F会存在较大的偏差，因此在求解基础矩阵前需要对特征点像素坐标进行归一化处理，最终得到一组去均值去中心化的归一化坐标，和一个由特征点像素坐标到归一化坐标的变换矩阵T。