简介

vins-mono+vins-fusiuon中的一些主要问题进行总结

1.vins-mono算法框架介绍

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

• 在数据预处理模块主要包括图像直方图均衡、KLT特征点提取以及去畸变处理和光流跟踪，IMU部分主要是做一个预积分操作。

• 系统初始化首先进行纯视觉的sfm，在当前滑窗中寻找视差(20个像素)和匹配点(30)比较多的两帧图像做初始化，主要是基于对极约束得到一个不带尺度信息的R和t，然后三角化出一组3D点。最后通过pnp求解出其他几帧图像对应的位姿。

• 在视觉初始化完成之后会对相机和IMU之间的旋转外参、陀螺仪偏置信息进行一个基于松耦合的在线标定，然后对每帧图像对应的速度、尺度信息和重力加速度进行基于对齐，最后将重力加速度在单位空间切平面分解成由两个单位正交基表达形式，重新带入残差函数进行优化求解。

• 基于滑窗的VIO过程就是正常的VIO过程，残差函数由边缘化得到先验残差、视觉重投影残差、IMU残差三部分组成。

• 回环检测模块主要基于DBOW2词袋库实现，对于每个关键帧会提取500个特征点并计算描述子然后转化为词袋向量，通过词袋向量寻找回环关键帧。在建立特征匹配关系后会依次基于RANSAC迭代求解本质矩阵和进行PNp，目的是为了筛选外点，如果内点数量满足要求则检测到了回环，通过将回环部分的残差加入到残差函数进行4自由度位姿图优化。
  

2.vins-fusion算法框架介绍

vins-fusion主要是在vins-mono的基础上增加了对双目，双目+IMU以及GPS的支持。

对于gps这块主要是进行了坐标系的统一，通过将初始帧的SLAM位姿和GPS坐标进行对齐，从而将相对位姿转换为绝对位姿。同时将GPS的绝对位姿信息以及SLAM与GPS坐标之间的相对变换加入到残差函数进行优化求解。

3.图像信息预处理

传感器预处理模块相当于SLAM前端。对于视觉模块来说主要包括图像预处理(直方图均衡，其实用处不是很大)、KLT角点检测(Shi-Tomas角点)和去畸变处理、LK金字塔光流跟踪等几个部分。

4.光流跟踪专项

包括光流跟踪的三大假设，LK光流算法原理，金字塔光流算法原理、KLT光流跟踪、KLT角点检测原理。

4.1 直接法或者光流跟踪的三大假设

光流法跟踪的的主要原理就是通过计算像素点在相邻帧图像之间的速度，可以得到像素在下一帧的位置。主要基于以下假设：

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

4.2 LK光流跟踪算法原理

(此处参考14讲第8章)
对于t时刻的某个像素(x,y)，当相机运动到t+1时刻时，基于灰度不变性假设可以得到如下关系

对公式左边进行一阶泰勒展开，并根据灰度不变性可以得到以下公式：

首先dx/dt和dy/dt分别为像素点在x轴和y轴方向上的导数，分别记为u和v；
然后∂I/∂x和∂I/∂y分别为像素值在(x,y)处的偏导数，记为在(x,y)处的梯度；
则可以将上述公式写成矩阵的形式：

为了求解像素在相邻两帧的速度(u,v)，根据空间一致性假设同一像素块中的像素具有相同的运动，则可以在特征点像素周围选择一个邻域范围，该范围内的像素和特征点具有相同的运动，则可以联立方程组然后根据最小二乘法对变量(u,v)进行求解。

4.3 金字塔光流

这部分的参考链接：https://blog.csdn.net/shandianfengfan/article/details/110944659

当相机运动速度过快时，相邻帧图像之间就会出现较大的位移，这时小运动这一较强的假设就不再成立，由于同一特征点在相邻图像中的位移Δ_pixel较大，在进行迭代求解时很容易陷入局部最优的情况。

金字塔光流的核心思想就是将图像进行金字塔处理，在每次计算光流时首先计算像素点在分辨率最小的顶层图像的光流，因为图像分辨率较小所以像素移动的距离比较小，符合光流小运动的假设。在求解得到像素在金字塔顶层图像的速度后，会将该速度和在像素点处的梯度信息作为初值继续计算特征点在下一层的速度和梯度信息，直到求解出特征点在原图像的速度信息。

金字塔创建
通过高斯卷积对原始图像进行向下采样。

图像金字塔图像一般为4层，层数越高跟踪越精确但是计算量会越大。vins-mono中选择4层金字塔，vins-fusion中增加了一步预测，成功预测表示当前运动跟踪较好，将金字塔减少为2层，左右目之间的光流跟踪还是4层金字塔。

金字塔光流跟踪的一般步骤：

• 建立金字塔后，首先对于最高层的图像计算该层的初始光流和残余光流向量,每层的初始光流如下图所示
  

其中光流残余向量表示同一像素点从参考帧到当前帧的移动，这个通过最小化像素块灰度差得到。初始光流g决定初始跟踪点，剩余光流d在初始跟踪点的基础上进行迭代优化，获取精确跟踪点。

• 根据上述初始光流计算公式，可以得到每一层的初始光流，然后通过迭代找到剩余光流和精确的光流位置
• 对上述过程不断迭代，直到得到像素点在原始图像上的坐标

4.4 KLT光流跟踪

核心思想就是提供了一种能够被很好的跟踪的特征点检测方法，即Shi-Tomasi角点检测，在vins中直接调用cv中的函数goodFeaturesToTrack。

4.5 Harris角点

参考链接：https://zhuanlan.zhihu.com/p/150411153
Harris角点检测算法的核心是利用局部窗口在图像上进行移动，判断灰度是否发生较大的变化。如果窗口内的灰度值（在梯度图上）都有较大的变化，那么这个窗口所在区域就存在角点。

Harris角点通过计算窗口移动前后窗口内像素的灰度差得到像素值变化量函数E(x,y)，通过对E(x,y)中的矩阵计算特征值进行判断是否存在角点，当两个特征向量都比较大时认为该窗口内存在角点。

1.Harris角点检测步骤:

• 当窗口（局部区域）同时向x（水平）和 y（垂直）两个方向移动时，计算窗口内部的像素值变化量E(x,y)；
• 对于每个窗口，都计算其对应的一个角点响应函数 R；
• 然后对该函数进行阈值处理，如果 R>threshold，表示该窗口对应一个角点特征。

2.Harris角点数学模型
建立数学模型的目的，是为了确定哪些窗口会引起较大的灰度值变化。对于窗口中某个点(x,y)其灰度值为I(x,y)，当其移动一个(u,v)的距离后，其对应灰度值为I(x+u,y+v)，I(x+u,y+v) - I(x,y)即为窗口在移动是该像素灰度值的变化。

为了更为清晰的确定窗口内的像素灰度变化，选择计算窗口内所有像素灰度值的变化。如果窗口中心点像素是角点，那么窗口移动前后，中心点的灰度值变化非常强烈，所以该点权重系数应该设大一点，表示该点对灰度变化的贡献较大；而离窗口中心（角点）较远的点，这些点的灰度变化比较小，于是将权重系数设小一点，表示该点对灰度变化的贡献较小。

因此这里引入窗口函数w(x,y)(其实就是一个高斯核)，以计算整个窗口中像素灰度值得变化。设窗口在移动前后的灰度值变化为E(u,v)，则当窗口中心像素为角点时，E(u,v)会变得比较大。

为简化计算，将I(x+u,y+v)进行一阶泰勒展开。

其中对应的矩阵M为：

3.Harris角点求解
现在定义了Harris角点的数学模型，下一步就是如何去寻找角点的坐标以及存在角点的对应的窗口。

（1）通过M矩阵的特征值判断
如果特征值在两个方向都比较大就是角点；如果特征值在一个方向比较大是边；如果特征值都很小就是平面。

可以把R看成旋转因子，其不影响两个正交方向的变化分量。经对角化处理后，将两个正交方向的变化分量提取出来，就是λ1和λ2（特征值）。

（2）通过R响应判断
本质上也是根据M矩阵的特征值计算一个分数R，根据分数进行判断:R大于某个阈值，认为是角点；R大于0但小于阈值是图像块；R小于0是边缘。
R的计算方式如下：

Harris 角点检测的结果是带有这些分数R的灰度图像，设定一个阈值，分数大于这个阈值的像素就对应角点。

4.6 Shi-Tomasi角点检测

参考链接：https://blog.csdn.net/qq_40374812/article/details/117016214
Shi-Tomasi角点是Harris角点的改进版，主要是对角点响应值的计算方式进行了改进，将较小的特征值作为角点响应值并和阈值进行比较。

Harris角点检测算法引入角点响应值R，根据R值的大小来判断是否存在角点和边界。

Shi-Tomasi角点检测算法就是针对R值的计算公式进行了优化和修改。由于 Harris角点检测算法的稳定性和α值有关，而α是个经验值，需要我们根据自身经验去设置，不好设定最佳值。

Shi-Tomasi发现，角点的稳定性其实和矩阵M的较小特征值有关(即λ1，λ2)，于是直接用较小的那个特征值作为分数。这样就不用调整α值了。

所以 Shi-Tomasi 将R值的计算公式改为如下形式：R = min(λ1，λ2)

和 Harris 一样，如果该分数大于设定的阈值，我们就认为它是一个角点。

5.vins-mono中的特征点去畸变方法

参考链接：https://blog.csdn.net/qq_33733356/article/details/124356158

采用一种逐渐逼近式方法，核心思想是越靠近图像边缘产生的畸变程度就越大。

已知含有畸变的A'坐标，需要求出发生畸变前的真实坐标A。根据畸变公式可以由真实坐标得到包含畸变坐标是比较容易计算的，反之要复杂很多。

因此VINS中将已知的A'当做真实的坐标B，通过畸变公式得到畸变后的B'。得到畸变强度（近似表示）BB'，从A'移动BB'，得到更加靠近A的C点。然后以C继续推导出更靠近A点的D。反复迭代，即可得到在接受范围内无限靠近A的点。

6.特征点均匀化处理

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

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

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

8.vins-mono中关键帧创建方法

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

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

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

1.耗时对比

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

2.性能对比
LK光流相比描述子匹配较鲁棒一些，但基于描述子的方法在遮挡，运动速度较快的情况下性能更佳。因此光流跟踪更适合作帧间匹配，描述子匹配更适合于重定位和回环检测。

10.为什么要采用预积分

• 减少误差的传播，在积分过程中偏置等信息需要跟着计算，随着重复积分会造成误差的累计
• 在SLAM系统中激光或相机的频率要远小于IMU频率的，在后端优化时会迭代很多次，每次调整位姿都需要重新对两帧之间的IMU数据进行重复积分，会占用比较大的计算资源。

10.传感器预处理之IMU预积分

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

10.1 连续时间的PVQ积分

连续时间的关于pvq的积分公式如下：

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

可以推导出四元数求导过程：

最终k+1时刻的旋转q_k+1可以由K时刻的旋转q_k乘以Δt时刻旋转的积分得到,其中w为陀螺仪测得的角速度

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

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

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

10.3 离散时刻下的预积分

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

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

10.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）误差量较小，可以忽略其二阶导数，只计算一阶雅各比

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

IMU误差关于各状态的导数：

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

误差函数对位移部分的导数：

误差函数对于速度的导数：

误差函数对于旋转的导数：

误差函数对于偏置的导数：

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

具体为：

离散时间角度误差的推导

离散时间速度误差的推导

离散时间位移误差的推导

离散时间零偏误差的推导

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

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

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

求解预积分量关于零偏的雅各比

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

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

11.VIO初始化过程简介

系统初始化首先进行纯视觉的sfm，在当前滑窗中寻找视差(20个像素)和匹配点(30)比较多的两帧图像做初始化，主要是基于对极约束得到一个不带尺度信息的R和t，然后三角化出一组3D点。最后通过pnp求解出其他几帧图像对应的位姿。

然后进行视觉惯性对齐，包括初始化旋转外参、陀螺仪偏置、每一帧对应的速度、尺度和重力加速度，最后进行重力加速度的细化。

12.VIO不进行初始化的变量

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

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

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

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

14.纯单目视觉初始化

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

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

2.根据对极约束原理，基于RANSAC迭代求解可以计算第4帧和第10帧的运动(R,t)，并恢复出不带尺度信息的3D空间点
这过程和ORB-SLAM中相同。

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

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

15.陀螺仪零偏初始化

首先可以定义在k时刻IMU到c0参考系的旋转和平移:

具体推导如下:

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

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

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

求解过程如下：

16.视觉惯性对其之速度、尺度、重力加速度对齐

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

首先定义每一帧对应的IMU坐标系速度、尺度和重力加速度：

然后可以根据两个连续帧k和k+1帧关于位移和速度的预积分可以得到以下公式：

根据上面两个公式可以得到以下线性约束方程

然后构建最小二乘问题进行求解

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

17.重力细化

大部分情况下,重力实际上是已经知道的,我们可以通过查表等方式是可以知道当地的重力加速度的。如果我们求解的实际的重力加速度和重力不同,那么肯定就有一些问题。因此在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中。

18.紧耦合单目VIO

基于滑动窗口的紧耦合单目VIO，主要是根据滑窗内的图像帧和IMU预积分数据建立残差函数，通过最小化残差函数求解使得误差最小的当前帧位姿。为了控制优化变量的规模，滑动窗口会对过去时刻的图像帧位姿以及观测信息进行边缘化处理，作为先验信息参与其他时刻的位姿估计。

涉及到边缘化规则、边缘化先验误差、IMU误差卡尔曼以及视觉残差的定义。

19.VIO优化变量的组成

当前滑窗中每一帧对应IMU状态、相机与IMU的外参、视觉观测的逆深度信息。

• IMU状态包括PVQ以及加速计和陀螺仪的偏置
• IMU旋转外参在初始化时进行了松耦合标定，平移外参通过测量获得，然后会再每一帧进行优化
• 采用逆深度信息减小较远点产生的误差，能够表达较远的点，且更加符合高斯分布。
  

20.紧耦合VIO残差函数

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

21.VINS中的边缘化处理

1.边缘化规则：
这么做是为了缓解边缘化带来的fill-in问题，被边缘化的状态和其他帧之间的约束越少就越不容易产生fill-in问题，否则宁愿直接丢弃一些信息也不进行边缘化(OKVINS的边缘化策略)。

• 如果次新帧是关键帧，则将最老帧的状态、视觉测量和IMU数据进行边缘化
• 如果次新帧不是关键帧，则丢弃次新帧视觉测量值保留该帧的位姿和IMUpose

视觉测量指的是特征点的逆深度信息，IMU数据则是PVQ和加速计陀螺仪的偏置。

2.vins中对边缘化导致的线性点不一致问题的解决
边缘化导致了线性化点的早期固定，这可能导致次优估计结果。但是在vins中作者并没有使用FEJ策略，由于小型漂移对于VIO来说是可以接受的，认为边缘化所造成的负面效果并不重要

22.视觉残差计算

视觉残差即特征点的重投影误差，vins采用了一种新的投影误差描述，在单位球面上定义摄像机的测量残差。

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

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

23.IMU残差的含义

IMU残差还是基于误差卡尔曼滤波模型，计算的是估计的运动以及预积分得到的运动之间的差值。

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

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

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

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

24.回环检测

使用DBOW库进行基于词袋向量的候选回环关键帧进行检测，为了执行回环检测，vins在光流跟踪的角点基础上额外提取500个特征并计算BRIEF描述子，然后将图像抽象为词袋向量进行保存。对于新的关键帧计算词袋向量的相似性进行回环候选帧判断。

另外，算法提供地图保存和加载功能，加载进来的地图也会作为候选匹配库存在，用于回环检测。

24.1特征恢复

存在回环关系的关键帧之间首先会进行基于描述子的特征匹配，基于RANSAC实现2D-2D本质矩阵和3D-2D的PNP筛选外点并计算相对位置关系，内点数量满足要求时被认定为候选回环帧。

24.2紧耦合重定位

就是基于当前滑动窗口的位姿以及与回环帧之间的共视关系建立约束关系，优化变量依然为当前滑窗中的imu状态，相机和IMU的外参，特征点逆深度等信息。

不同的是在视觉残差部分加入了回环帧的位姿信息作为先验，以及回环帧的特征和当前滑窗中关键帧的匹配与约束

25.传感器的时延估计

核心思想就是基于时延时间在相邻两帧时间之间进行插值。

作者采用了移动特征点的方法来解决这些问题。作者做了一个假设,在短时间内,相机的运动是匀速运动。基于假设,计算特征点在相机平面上的运动。利用不同时刻,在相机上的投影,则可以很轻松的算出在中间某个时刻特征点的坐标(假设了特征点的速度也是匀速运动)。

如果我们特征点是用 3D坐标表示的话,就直接利用特征点投影的坐标,和特征点速度,以及补偿时间,来算出对应时刻特征点在图像上的坐标。

在vins中机器人的位姿估计是围绕IMU进行的，所以才会出现对时延这种处理，正如ORB-SLAM中描述的那样，无法保证每一帧图像的时间戳都能严格的和相应的IMU数据保持一致。在vins对这种现象的处理就是根据特征点移动速度和时间差，虚拟构造出一条约束关系。

26.vins-fusion的改进点

1. 新增双目，双目VIO以及和GPS的融合
2. 引入双向光流检查机制，对于特征点在跟踪到下一帧后，会将其由下一帧再反向跟踪到上一帧，对于跟踪结果要满足对于同一特征点对应的状态位相同(正着跟踪成功同时反向也跟踪成功)，以及正反跟踪到的特征点的像素差值小于阈值(0.5)，才会判定对该特征点跟踪成功
3. 引入预测机制优化帧间光流跟踪的金字塔层数
4. 双目模式下的视觉初始化比较简单，3D点可以直接通过相机视差获得

27.引入预测机制优化光流金字塔的层数

这里定义了一个匀速模型，通过匀速模型和当前帧位姿可以初步预测出下一帧的位姿，然后根据预测的下一帧位姿将当前帧图像坐标系下的3D点投影到下一帧图像平面，为光流跟踪提供初值。

如果预测成功可以得到一个初始的光流，所以不需要太多层金字塔，将光流跟踪的图像金字塔由4层变为2层。如果基于2层金字塔跟踪到的点比较少(<10)，则重新使用4层金字塔进行跟踪。

28.引入双向光流的检查机制

当特征点从上一帧跟踪到下一帧之后，再进行一次反向跟踪即使用下一帧的特征点在上一帧图像上进行跟踪，对于跟踪结果要满足对于同一特征点对应的状态位相同(正着跟踪成功同时反向也跟踪成功)，以及正反跟踪到的特征点的像素差值小于阈值(0.5)。

对于不满足判定条件的特征点，将其对应的状态位重置为0。

29.双目模式下的光流跟踪

对于双目相机，还会进行一步左目到右目的光流跟踪，实现左右目特征点的数据关联。同样的，左右目的之间也会进行一次双向光流跟踪。

左右目之间的光流跟踪使用4层金字塔进行光流跟踪。

30. 后端优化中的双目重投影残差

如果要约束的第i帧和第j帧不是同一帧： 第i帧左目和第j帧左目之间关于重投影的视觉残差和vins-mono相同，第i帧左目和第j帧右目的重投影视觉残差中间多了一步外参的转换，需要先将右目的特征点转换到左目相机坐标系再计算重投影误差。

如果要约束的第i帧和第j帧是同一帧： 无法基于位姿建立约束关系，但是可以基于外参和特征点深度信息建立约束进行求解。

4. 外点剔除

1.在后端增加外点的筛选机制
遍历滑窗中的图像帧，对于某个特征点计算其在观测到该点的第一帧和其他帧之间的重投影误差，双目模式下还要计算参考帧左目与当前帧右目的重投影误差，最后求一个平均重投影误差，如果重投影误差大于3个像素，则认为该点是一个外点。

5. GPS融合

GPS主要给slam系统提供了绝对位姿估计的信息。

在global_fusion节点中主要是订阅vio的位姿和gps信号，然后将vio的位姿转换到gps坐标系并进行单位的统一。在SLAM系统运行后，将初始帧对应的GPS经纬度作为SLAM系统的起点和参考系，后续的位姿都基于该参考系进行更新。

在构建约束关系时，主要利用VIO之间的帧间位姿约束、GPS的绝对位置(x,y,z)以及VIO位姿和GPS位置之间的旋转变换关系设计代价函数。