VINS-Mono论文笔记

目录

前言

VINS-Mono系统框架

思维导图

4 视觉和IMU测量的预处理步骤

5 鲁棒的初始化过程

6 紧耦合的单目VIO系统

7 紧耦合的重定位

8 全局姿态图的优化和重用

论文原文：

《VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator》

前言

1、VIO：视觉+IMU（惯性测量单元）

• 视觉：获取图像数据来估计相机运动

• IMU：返回被测量物体的加速度和角速度

• IMU适合短时间、快速的运动

• 视觉适合计算长时间、慢速的运动

2、VIO分类：

• 将视觉约束加入到联合优化是紧耦合，vins-mono是紧耦合

• 将视觉约束后的位姿加入到联合优化是松耦合

VINS-Mono系统框架

1.测量处理线程(measurement preprocessing (Section IV即第四章))：图像特征提取和光流追踪，在两个图像时间戳间的imu数据被进行预积分。
2.初始化线程（The initialization procedure (Section V即第五章)）：提供姿态（相机和imu）,速度（imu坐标系的速度），重力向量，陀螺仪偏置，特征点的三维坐标。这些值用于引导后续VIO的非线性优化。
3.视觉惯性里程计（滑动窗口内非线性优化）The VIO ( Section VI即第六章) with relocalization (see Section VII即第七章)：对特征观测和imu预积分值进行紧耦合优化，该模块带重定位功能。
4.姿态图优化模块（the pose graph optimization module (Section VIII即第八章)）：利用上面的重定位结果，进行姿态图优化，以消除漂移。

思维导图

vins-mono的特点

• 未知初始状态的鲁棒性初始化过程。
• 带imu-camera外参校准和imu校准的紧耦合，基于非线性优化的单目VIO系统
• 在线重定位和四个自由度的全局姿态图优化。
• 姿态图可以保存，加载，并和局部姿态图进行合并。

论文结构：
第四节：视觉和IMU测量的预处理步骤
第五节：VINS-Mono的鲁棒的初始化过程
第六节：紧耦合、自校准、基于非线性优化的单目VIO
第七节：紧耦合的重定位
第八节：全局姿态图的优化和重用

4 视觉和IMU测量的预处理步骤

该部分对应于系统总览第一部分：measurement preprocessing。

4.1 视觉部分

使用KLT稀疏光流追踪法对最新的图像进行光流追踪。如果追踪的点数量少于一个阈值，则会额外检测一些特征点。
特征点是以第一次观测到的为观测值，经过去畸变后，通过相机模型，投影到一个单位圆上（方便做视觉残差）。

关键帧的判断：
a)以视觉测量判断为主。如果追踪的特征点间视差大于某一个阈值，则认为是新的关键帧。另外视差不仅可以通过平移，还可以通过旋转来得到，为解决纯旋转问题，使用了短期imu预积分数据来补偿旋转。
b)为保证追踪的连续性，如果某一帧种追踪的特征点数少于一个阈值，则认为其是关键帧。

下面的没写完

4.2 imu部分
imu数据模型

1）噪声和偏置
测量值包括偏置和噪声，加速度则还包含重力加速度。
偏置假设服从随机游走模型（其导数服从高斯分布），噪声假设服从高斯分布。

2）imu预积分
补充知识，在世界坐标系下imu积分为

解释：计算imu在世界坐标系下的位移，需要用世界坐标系下的加速度。而imu测量结果都是自己坐标系（即body坐标系下的加速度），需要通过状态量q w b t q_{wb_{t}}q 
wb 
t
​
 
​
 变换为到世界坐标系下状态量。但每一次优化后状态量是会变化的，这样就需要重新积分，这种低效的做法是我们想避免的。
预积分所做的事，就是将状态量分解为上一时刻与当前时刻两部分。上一时刻状态量看为常数，那当前时刻状态量只和imu积分有关，积分运算进行一次就行。原来的积分运算换为如下形式：

所以积分公式可以变为如下：

回到论文：
在相邻两个关键帧（图像帧）之间，对imu数据进行如下预积分计算：
可以看到，这里的加速度值是减去了偏置的，上面推导时默认使用真值（没有偏置的数据）

对预积分量的协方差也会进行计算。

4.3 偏置纠正

当偏置优化结果变化不大时，利用一阶泰勒展开对预积分量进行更新：

如果偏置优化变化较大，则使用新偏置重新计算imu预积分量，方法同第二部分公式。
这种策略可以极大节省计算量。

5 鲁棒的初始化过程

单目惯性系统是高度非线性系统，系统的正常运行高度依赖准确的初始化过程。
初始化过程由松耦合下的imu预积分辅助纯视觉结构还原过程。

5.1 视觉重构
使用滑动窗口中纯视觉的sfm方法。初始化过程从一个sfm过程开始，还原一个缺乏尺度信息的，由相机姿态和特征点位置构成的图模型。
获取视觉计算所需的大位移帧：
因为图像是源源不断地传进来的，开始是不断对最新帧进行特征追踪，计算最新帧与之前所有帧特征的关系。如果最新帧与之前某一帧存在如下关系：对应追踪的特征点不少于30个，特征点间对应位移大于20像素（这里在代码里是平均位移），那么就算找到合适的大位移帧。
利用五点法，计算这两帧之间的位移和旋转（缺乏尺度信息的）。然后任意设置一个尺度，用三角测量方法计算出这两帧中特征点的三维坐标。
基于已三角化的特征点，使用pnp法计算整个滑动窗口中所有帧（一般为10帧）的姿态。
最后，使用完整BA(既包含姿态，又包含特征点)来最小化特征点的重投影误差。
因为还没有关于世界坐标系的信息，这里把相机的第一帧作为sfm过程的参考帧，也就是参考坐标系。
所有帧的姿态和特征点坐标结果都是相对于相机的第一帧（第一帧也就是相机坐标系）。借助于相机和imu的外参p c b p^b_cp 
c
b
​
 和q c b q^b_cq 
c
b
​
 ，可以把相机坐标系下位姿转换到imu坐标系下：

笔记：这里说的，其实是基于imu和相机的外参，计算每一个相机位姿下对应的imu位姿，其参考帧，还是基于相机坐标系，也就是相机坐标系下对应时刻imu位姿。
这里的尺度信息还是未知的，接下来会进行求解。
5.2 视觉惯性联合

使用imu预积分值，使纯视觉中的尺度信息得以和imu测量相匹配。
A)陀螺仪偏置校准
从纯视觉sfm中可以获得滑动窗口中所有帧得姿态，而从imu预积分中也可也获得连续两帧的相对旋转。最小化如下代价函数：

相对旋转的更新使用第二个函数，可以得到一个初始化的偏置的校正值，使用新的陀螺仪偏置值，对滑动窗口内imu预积分量进行重新积分。
B)速度、重力向量、尺度的初始化
陀螺仪偏置初始化后是对导航必要信息的速度、重力向量、尺度的初始化。

对imu预积分量有如下两个公式

该公式和速度，重力向量、尺度信息相关。

通过最小化上面这个代价函数，可以获得滑动窗口中每一帧的速度。相对于相机坐标系的重力向量和尺度值。
C)重力向量优化

重力向量的更新策略是将g gg向量使用g ( g ^ + δ g ) g(\hat{g}+\delta g)g( 
g
^
​
 +δg)来代替，其中g ^ \hat{g} 
g
^
​
 是测量出的平均重力向量单位方向，δ g \delta gδg是在正切球空间里的微小扰动。不断迭代这个值，直到g gg向量收敛。

D)初始化的完成
在对重力向量完成优化后，就可以通过把重力向量旋转到z轴来获得世界坐标系和相机坐标系的旋转关系。然后把所有相机坐标系的量旋转到世界坐标系下，此时imu坐标系的量同样也旋转到世界坐标系下。在sfm中的位移关系会缩放到米单位下。到这里，初始化完成，所有米制数据会提供给接下来的紧耦合单目VIO系统。

6 紧耦合的单目VIO系统

完成初始化后，就进入基于滑动窗口的紧耦合单目VIO系统。

6.1 公式

第一个是所有状态的集合。第二个是imu的状态向量，第三个是相机与imu的外参。这里就不解释每个字母含义了，论文里都介绍的很详细。对视觉特征的参数化使用的是逆深度形式。
这里使用的是视觉惯性BA优化方式，使得先验误差和所有测量值残差的马氏距离最小，从而获得最大后验估计。

第一项为先验误差，第二项为imu测量残差，第三项为摄像机测量残差。对视觉测量使用Huber函数以减小特征误匹配带来的影响。

6.2 imu残差
在滑动窗口中连续两帧imu测量残差为：

基于状态量计算得到的相对位移，速度，旋转和imu预积分量的差值。

6.3 视觉残差
与传统方法中将特征点投影到单位平面上计算残差不一样的是，VINS-Mono把像素误差投影到一个单位圆上。即相机残差定义在如下的单位圆：

其原因是这种残差定义方式几乎适用于所有类型的相机，包括广角、鱼眼或全向相机。
将特征点第一次观测的坐标，通过先转到imu局部坐标，然后imu局部坐标转换到世界坐标，imu世界坐标再转换到局部坐标，然后投影到当前相机坐标系，当前坐标系下投影到单位圆，就形成了路标的单位向量。
观测点单位向量与路标单位向量相减，向量差在单位圆上一切平面分解，就得到了两个残差值，即视觉的几何误差残差。

6.4 边缘化残差
为了限制VIO非线性优化的复杂度，使用了边缘化。对imu状态量和视觉特征选择性的进行边缘化，同时把边缘化的状态量转为先验。

如果第二新的帧是关键帧，则把滑动窗口中最老的关键帧边缘化掉（对应于第一种情况），如果第二新的帧不是关键帧，则把该帧对应的特征点舍弃掉，但保留其imu测量值。
为保证稀疏性，非关键帧的测量值不是所有的都会被边缘化掉。（对第二新的帧不是关键帧的情况）。
边缘化方案只是对窗口中的关键帧。它确保三角化特征点过程中计算特征点具有足够的视差，并且加速度计能得到充分的激励。边缘化使用舒尔补。
基于所有与移除状态的测量值，VINS-Mono建立了一个新的先验方法。新的先验被添加到现有的先验中。（VINS-Mono的代码中将姿态和路标信息都进行了边缘化（成为先验信息），这样客观上说是最大程度保留了所有有效信息，但也会导致海参矩阵的求解比较耗时）
我们注意到边缘化导致线性化点的早期固定，这可能导致次优估计结果。然而，由于VIO的小漂移是可以接受的，我们认为由边缘化造成的负面影响并不重要。

笔记：边缘化过程中残差的雅可比可能不是在同一点得到的，这是边缘化一直存在的问题，对于该问题，其实也有很多解决办法。但VINS-Mono中认为漂移本身就存在，后续通过回环也可以来进行消除。
6.5 针对相机实时帧率的纯运动视觉惯性状态估计器
对于计算能力有限的平台，比如手机，紧耦合单目VIO由于非线性优化沉重的计算量，在这些平台是难以达到相机实时帧率更新的。为此，除了完全优化之外，VINS-Mono还采用了轻量级运动视觉惯性优化，将状态估计频率提高到相机率(30Hz)。
纯运动视觉惯性优化的代价函数与单眼视觉优化的代价函数相同。但不同的是，纯运动状态估计并没有优化滑动窗口中的所有状态，而只是优化了固定数量的最新IMU状态的姿态和速度。将特征深度、外部参数、偏差和不想优化的旧IMU状态视为常数值。
但这确实使用所有的视觉和惯性测量来进行仅限运动的优化。（只不过有些值只是约束，不是优化量）

这种方法比单帧的pnp方法结果更为平滑。
这种方法把原来在嵌入式设备上需要50毫秒的全优化计算降低为5毫秒，使得在低算力设备上能得到相机帧率的姿态估计，特别适合在需要AR或者VR的手机上进行运行。

6.6 imu向前传播以得到imu频率的状态估计
IMU测量的速度比视觉测量的速度要高得多。虽然我们的VIO频率受图像捕获频率的限制，但我们仍然可以直接传播最近的IMU测量的VIO估计，以实现IMU速率性能。高频状态估计可以作为回环状态反馈。第九-c节介绍了一个利用这种imu速率状态估计的自主飞行实验。

7 紧耦合的重定位

滑动窗口和边缘化可以控制计算复杂度，但同时也给系统带来了累计漂移。
VINS-Mono使用了一种紧耦合的重定位方式来消除累计误差。重定位从一个回环检测模块开始，以分辨什么地方之前来过。
当前帧和回环帧在特征程度上的约束得以建立。这些对应特征的约束被集成到了单目VIO系统中，优化至最小以得到无漂移的状态估计。
多特征的多次观测约束直接在重定位中使用，可以得到更高精度和平滑度的状态估计。

7.1 回环检测
使用词袋方法DBoW2进行循环检测。
为了进行回环检测，从图像中抽取了500个特征角点并计算其BRIEF描述子。
额外的角点抽取是为了提高召回率。
描述子作为视觉数据库的检索词。
DBoW2在时间和几何一致性检查后返回候选闭环。数据库中保留所有简短的描述符以进行特征检索，但丢弃原始图像以减少内存消耗。

7.2 特征检索
当检测到一个回环时，需要通过特征对应关系来求解回环帧间关系。特征对应关系通过描述符进行匹配。但特征匹配可能会导致误匹配的问题，对该问题通过如下方式来解决。

2D-2D：使用RANSAC的基本矩阵检验。在当前图像和环闭合候选图像中检索到的特征的二维坐标来进行基本矩阵测试，去除离群点。

3D-2D:使用RANSAC进行的PnP测试。基于已知的局部滑动窗口中特征在局部滑动窗口中的三维位置，以及在回环帧图像中的二维特征点坐标，可以进行PnP测试。
在除去异常值后，将该候选对象视为正确的循环检测，并执行重新定位。

笔记：如果去除异常点后，正确匹配的特征点大于某一个阈值，则会基于匹配点计算回环帧间的相对关系，如果几何关系满足某一个限制，则认为发生了回环。

7.3紧耦合的重定位
重定位高效地将滑动窗口的帧与过去的姿态关联到了一起。在重定位时，把回环的帧（滑动窗口中的帧）设为固定帧。
回环帧观察到的特征的视觉测量模型，与VIO中的视觉测量模型相同，其残差的计算方法类似。
唯一的区别是，回环帧的姿态取自姿态图（见第八节，即姿态图合并的情况)，或直接来自过去的VIO里程计的输出(如果这是第一次重新定位），并被视为一个常数。
于是，可以很轻易把回环帧的残差添加进优化中：

其中，回环残差的变量是在回环帧中检索到的所有特征的集合。
请注意，尽管待求解函数与（14）略有不同，但要求解的状态的维数保持相同，因为回环帧的姿态被认为是常数。
当滑动窗口中发生了多个回环时，同时使用所有回环帧的所检索的所有对应特征进行优化。
这为重新定位提供了多视图约束，从而获得了更高的精度和更好的平滑度。重新定位后保持一致性的全局优化将在第八节中进行讨论。

8 全局姿态图的优化和重用

在重定位后，为保证过去的姿态能被正确加载到全局地图中，额外的姿态图优化被开发使用了。

8.1 四自由度的累计误差
得益于对重力的测量，在VINS中对翻滚角和俯仰角是完全可观测的。
如图所示：

随着物体的运动，相对于参考帧的相对的3D位置和旋转就改变了。
但是，我们可以通过重力向量的方向定义水平面，那么相对水平面的翻滚角和俯仰角是可以一直被观测到的。因此这两个量可以被当作世界坐标系下的绝对测量。
而3D位置和偏航角则作为参考帧的相对估计。因此对这四个量来说，会有累计漂移。
为了充分利用有效信息，我们认为翻滚角和俯仰角没有漂移，而对其余四个方向进行四自由度的姿态图优化。

8.2 向姿态图中添加关键帧
在经过VIO处理后，关键帧被加入姿态图。
每一个关键帧在姿态图里是一个顶点，该顶点与其他顶点通过两种类型的边连接。

A)序列边：
一个关键帧与它之前的几个关键帧通过序列边建立联系（图中表示与之前两帧），表示与之前几帧的相对位移关系，这种关系直接通过VIO获得。
考虑到关键帧i和之前的一个关键帧j，序列边只包含相对位置P PP和偏航角ψ ψψ的约束。

B)闭环边
如果一个帧有回环连接，那么它在姿态图中与闭环帧通过闭环边相连。
与序列边类似，闭环边只包含四自由度的相对位移关系。定义和上面的公式类似。
闭环边的值通过重定位获得。

8.3 四自由度的优化
我们将两帧间的残差最小化定义为：

其中R参数前两个表示翻滚角和俯仰角，从VIO中获得且固定为常值。

整个序列边和闭环边的代价函数为：

前面是序列边集合，后面是闭环边集合。
尽管紧耦合的重定位有助于消除错误的闭环，但我们还是加入了huber函数来进一步降低任何可能的误回环影响。
相反，对序列边则没有加任何鲁棒规范函数。因为这些边都是从VIO中取得，而在VIO中已经有了足够的异常点剔除机制。
姿态图优化和重定位分别跑在两个分离的线程，这使得：
1.重定位可以使用最新的姿态图优化结果来进行。
2.即使姿态图优化尚未完成，仍然可以使用现有的姿态图进行重定位。

8.4 合并姿态图
姿态图不仅可以优化当前地图，还可以将当前地图与以前构建的地图合并。
如果我们加载了以前构建的姿态图，并检测到两个姿态图之间的闭环关系，我们可以将它们合并在一起。
由于两者间所有的边都是相对约束，因此姿态图优化通过闭环边连接自动将两个姿态图合并在一起。如图所示。

当前的地图被闭环边拉到之前的地图中。每个顶点和每条边都是相对变量，因此，我们只需要固定姿态图中的第一个顶点（被加载姿态图的第一个顶点）。

8.5 保存姿态图
姿势图的结构非常简单。我们只需要保存顶点和边，以及每个关键帧的词典查询向量（特征点描述子构成的向量）。丢弃原始图像以减少内存消耗，保存的数据结构如下：

其中i为帧索引，p i w ^ \hat {p^w_i} 
​
 分别为VIO的位置和方向。如果此帧有一个闭环帧，则v是闭环帧的索引。p i v i ^ \hat {p^i_{iv}} 
​
 是这两帧之间的相对位置和偏航角，通过重新定位得到。D（u、v、des）是关键帧的可视化查询向量，每个特性都包含二维位置及其简短的描述子。

8.6 加载姿态图
姿态图的保存格式就是其加载格式。每个关键帧都是姿态图中的一个顶点。顶点的初始姿态是ˆpwi和ˆqwi。回环边由闭环信息ˆpiiv、ψˆiv直接建立。每个关键帧和相邻关键帧建立几条顺序边。加载姿态图后，我们立即进行一次全局4-DOF姿态图优化。姿态图的保存和加载速度与姿态图的规模呈线性关系。

                        
原文链接：https://blog.csdn.net/hu_hao/article/details/120257735