一文详解单目VINS论文与代码解读目录

参考这个：
https://blog.csdn.net/wangshuailpp/article/details/78461171

本文旨在对前一阶段学习vins-mono开源框架的总结。结合暑假秋招之前报名的深蓝学院的《从零开始手写VIO》课程，本文从VIO原理以及开源代码分析两部分进行详细介绍。PS：提升代码能力最好的办法就是从头到尾撸开源框架，这种体验是单纯推导公式体会不到的。
本文只是VINS-Mono的目录部分，具体内容链接如下，共分为12部分，对应系统的4大部分：图像和IMU预处理、初始化、后端滑动窗口优化、闭环检测和优化。

目录

1.VINS-mono 论文解读（IMU预积分+Marg边缘化）
2.VINS-Mono 代码详细解读——视觉跟踪 feature_tracker
3.VINS-Mono 代码详细解读——vins_estimator/Estimator_node.cpp
4.VINS-Mono 代码详细解读——基础储备：IMU预积分的残差、Jacobian和协方差 processIMU（）+intergrationBase类+imu_factor.h
5.VINS-Mono 代码详细解读——基础储备：vins_estimator/feature_manager.cpp
6.VINS-Mono 代码详细解读——基础储备：外参标定 InitialEXRotation类
7.VINS-Mono 代码详细解读——初始化1：视觉SFM详解vins_estimator/estimator.cpp/processImage()+initialStructure（）
8.VINS-Mono 代码详细解读——初始化2：视觉惯性松耦合初始化 vins_estimator/estimator.cpp/visualIntialAlign（）
9.VINS-Mono 理论详细解读——紧耦合后端非线性优化 IMU+视觉的残差residual、Jacobian、协方差、基于舒尔补的边缘化
10.VINS-Mono 代码详细解读——基于滑动窗口的紧耦合后端非线性优化 optimization()
11.VINS-Mono 代码详细解读——回环检测与重定位、四自由度位姿图优化
12.实操：用imu_utils标定IMU，之后用于kalibr中相机和IMU的联合标定

一、文章概述及框架

Vins-mono是香港科技大学开源的一个VIO算法，用紧耦合的方法，通过单目+IMU恢复出尺度，效果非常棒。
**VINS的功能模块可包括五个部分：数据预处理、初始化、后端非线性优化、闭环检测及闭环优化。**代码中主要开启了四个线程，分别是：前端图像跟踪、后端非线性优化（其中初始化和IMU预积分在这个线程中）、闭环检测、闭环优化。

1 图像和IMU预处理

图像：提取图像Harris角点，金字塔光流跟踪相邻帧，RANSAC去除异常点，最后将跟踪到的特征点push到图像队列中，并通知后端进行处理。
IMU： 1）IMU积分，得到PVQ 位置、速度、旋转， 2）计算在后端优化中将用到的相邻帧的预积分增量， 3）计算预积分误差的Jacobian矩阵和协方差。

2 初始化

SFM纯视觉估计滑动窗所有帧的位姿和3D路标点逆深度 SFM与IMU预积分松耦合，对齐求解初始化参数。

3 后端滑动窗口优化

将视觉约束、IMU约束和闭环约束放在一个大的目标函数中进行非线性优化，求解滑动窗内所有帧的PVQ、bias。

4 闭环检测和优化

DBow进行闭环检测，检测成功后重定位，最后对整个相机轨迹进行闭环优化。

二、VINS-Mono论文以及代码详细解读

1、VINS-mono 论文解读（IMU预积分+Marg边缘化）。
2、VINS-Mono 代码详细解读——视觉跟踪 feature_tracker。
3、VINS-Mono 代码详细解读——vins_estimator/Estimator_node.cpp 。
4、VINS-Mono 代码详细解读——基础储备：IMU预积分的残差、Jacobian和协方差 processIMU（）+intergrationBase类+imu_factor.h。
5、VINS-Mono 代码详细解读——基础储备：vins_estimator/feature_manager.cpp。
6、VINS-Mono 代码详细解读——基础储备：外参标定 InitialEXRotation类。
7、VINS-Mono 代码详细解读——初始化1：视觉SFM详解8、vins_estimator/estimator.cpp/processImage()+initialStructure（）。
8、VINS-Mono 代码详细解读——初始化2：视觉惯性松耦合初始化 10、vins_estimator/estimator.cpp/visualIntialAlign（）。
9、VINS-Mono 理论详细解读——紧耦合后端非线性优化 IMU+视觉的残差residual、Jacobian、协方差、基于舒尔补的边缘化。
10、VINS-Mono 代码详细解读——基于滑动窗口的紧耦合后端非线性优化 optimization()
11、VINS-Mono 代码详细解读——回环检测与重定位、四自由度位姿图优化
12、实操：用imu_utils标定IMU，之后用于kalibr中相机和IMU的联合标定

三、代码的文件目录

1、ar_demo：一个ar应用demo
2、benchmark_publisher：接收并发布数据集的基准值
3、camera_model
   calib：相机参数标定
   camera_models：各种相机模型类
   chessboard：检测棋盘格
   gpl
   sparse_graph
   intrinsic_calib.cc：相机标定模块main函数
4、config：系统配置文件存放处
5、feature_trackers：
   feature_tracker_node.cpp ROS 节点函数，回调函数
   feature_tracker.cpp 图像特征光流跟踪
6、pose_graph：
   keyframe.cpp 关键帧选取、描述子计算与匹配
   pose_graph.cpp 位姿图的建立与图优化
   pose_graph_node.cpp ROS 节点函数，回调函数，主线程
7、support_files：帮助文档、Bow字典、Brief模板文件
8、vins_estimator
   factor：实现IMU、camera等残差模型
   initial：系统初始化，外参标定，SFM
   utility：相机可视化，四元数等数据转换
   estimator.cpp：紧耦合的VIO状态估计器实现
   estimator_node.cpp：ROS 节点函数，回调函数，主线程
   feature_manager.cpp：特征点管理，三角化，关键帧等
   parameters.cpp：读取参数
————————————————

四、论文介绍

VIO引出原因：
1）单纯视觉： 缺点: 尺度不确定性、单目纯旋转无法估计、快速运动易丢失、受图像遮挡运动物体干扰。
优点：不产生漂移、直接测量旋转与平移。
2）单纯IMU： 缺点：零偏导致漂移、低精度IMU积分位姿发散
优点：快速响应、可估计绝对尺度、角速度估计准确。
3）结合视觉+IMU： 可用视觉弥补IMU的零偏，减少IMU由于零偏导致的发散和累计误差，IMU可为视觉提供快速响应的定位。
融合方案：

1、松耦合： 将 IMU 定位与视觉的位姿直接后处理融合，融合过程对二者本身不产生影响，典型方案为卡尔曼滤波器.

2、紧耦合： 融合过程本身会影响视觉和 IMU 中的参数（如 IMU 的零偏和视觉的尺度）典型方案为 MSCKF 和非线性优化。

本文贡献：
1、一个紧耦合、基于优化的单目视觉惯性里程计，具有相机-IMU外部校准和IMU偏置估计。
2、基于有界滑动窗口迭代进行估计。
3、基于滑动窗口里的关键帧维持视觉结构，基于关键帧之间的IMU进行预积分维持惯性测量。
4、鲁棒性：未知状态的初始化、相机和IMU外参数的在线标定、球面不统一重投影误差、回环检测、四自由度位姿图优化（三位置和航向）。

0、总体框架

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

0.1 数据预处理（IMU预积分见标题1）

视觉：
1）提取Harris角点，KLT金字塔光流跟踪相邻帧；
2）2 维特征点先矫正为不失真的，然后在通过外点剔除后投影到一个单位球面上 ；
3）去除异常点：先进行F矩阵测试，通过RANSAC去除异常点；
4）关键帧选取： 1、当前帧相对最近的关键帧的特征平均视差大于一个阈值就为关键帧（因为视差可以根据平移和旋转共同得到，而纯旋转则导致不能三角化成功，所以这一步需要IMU预积分进行补偿）2、当前帧跟踪到的特征点数量小于阈值视为关键帧；
IMU：
1）两帧k和k+1之间进行位置、速度、姿态（PVQ）预测；
2）避免每次姿态优化调整后重复IMU传播，采用预积分算法，计算预积分误差的雅克比矩阵和协方差项。

0.2 初始化
采用松耦合的传感器融合方法得到初始值。首先用SFM进行纯视觉估计滑动窗内所有帧的位姿以及路标点逆深度，然后与IMU预积分对齐，继而恢复对齐**尺度s，重力g，imu速度v，和陀螺仪偏置bg。**VINS本文初始化过程中忽视掉了加速度计的bias，因为加速度计与重力耦合，并且重力向量很大，初始化过程动态过程很短，幅度又不大，加速度计偏置很难观测到。

A.滑动窗口(Sliding Window)纯视觉SfM
1、选择一个滑动窗，在最后一帧与滑动窗之前帧寻找帧：跟踪到的点数目大于30个的并且视差超过20的，找到后用5点法本质矩阵初始化恢复出R和t。否则，滑动窗内保留最新图像帧，继续等待下一帧。
2、随意设置一个尺度因子，三角化这两帧观测到的所有路标点。再用PnP算法估计滑动窗内所有其余帧的位姿。滑动窗内全局BA重投影误差优化所有帧位姿。
3、假设IMU-Camera外参已知，乘上视觉得到的位姿，转换到IMU坐标系下。
B. 视觉惯性校准（IMU预积分与视觉结构对齐）
1、陀螺仪零偏bg标定
旋转两种方式： 陀螺仪测量值和视觉观测值，二者的误差其实就是陀螺仪偏置bg。
目标函数： visual给出的相邻帧间的旋转应等于IMU预积分的旋转值Q之间的差。


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

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

优化变量：速度、重力向量和尺度

目标函数： 相邻两帧IMU预积分增量与预测值之间平移、速度（P、V）的差。通过HX=B 利用cholesky分解获得


结合


通过求解线性最小二乘问题：

3、重力矢量修正
重力向量的大小是已知的，加入了模长限制||g^c0|| = 9.81，这导致三维重力向量只剩2个自由度。
主要做的是优化方向，一个二维向量。

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


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

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


0.3 紧耦合后端非线性优化（IMU约束+视觉约束+闭环约束）
初始化后，采用基于滑动窗口的紧耦合单目VIO进行状态估计。
A 公式

第一个式子是滑动窗口内所有状态量，n是帧数，m是滑动窗口内特征点总数。特征点逆深度为了满足高斯系统。
第二个式子xk是在第k帧图像捕获到的IMU状态，包括位置，速度，旋转（PVQ）和加速度偏置，陀螺仪偏置。
第三个式子是相机外参。
xk只与IMU项和Marg有关；特征点深度也只与camera和Marg有关；

视觉惯性BA，最小化边缘化的先验信息和IMU、视觉测量残差之和。
BA优化模型分为三部分：
1、Marg边缘化残差部分（滑动窗口中去掉位姿和特征点约束）
2、IMU残差部分（滑动窗口中相邻帧间的IMU产生）
3、视觉代价误差函数部分（滑动窗口中特征点在相机下视觉重投影残差）
B IMU残差
残差：状态量传播预测与IMU预积分的残差.
优化变量：IMU时刻下的p位置，v速度，Q旋转，两个偏置ba,bw.

C 视觉残差
与传统的针孔相机模型不同，这里用的是单位半球体的相机观测残差。是一个鱼眼相机。
在相机的归一化平面上比较残差，再将视觉残差投影到单位球面的正切平面上。由于视觉残差的自由度是2，所以我们将残差向量投影到切平面上。

在第i帧第一次观测到第l个路标点，在第j帧中对该路标点进行观测的残差为：

第一个式子就是残差的表达式，第二个式子是鱼眼相机反投影函数将观测到的像素坐标转换成单位向量的观测值数据，b1和b2是此单位向量的切平面上的一组基。第三个式子是重投影估计模型。其实VINS代码中也可以使用普通的针孔相机模型。
D 边缘化 Marginalization（详见标题2）
为了防止pose和特征的个数的复杂度随着时间不断增长，引入边缘化，在移除位姿时将关联的约束转化为先验放入优化问题中。
为了限制基于优化的VIO计算复杂度，引入边缘化。有选择地从滑动窗口中将IMU状态xK和特征λ1边缘化，同时将对应于边缘状态的测量值转换为先验。

分为两种情况，
1、一种是倒数第二帧如果是关键帧的话，将最旧的pose移出Sliding Window，将最旧帧关联的视觉和惯性数据边缘化掉。把第一个老关键帧及其测量值被边缘化；Margin_Old作为先验值。
2、如果倒数第二帧不是关键帧的话，那么就**只剔除倒数第二帧的视觉观测，而不剔除它的IMU约束。**原因是边缘化保证关键帧之间有足够视差而能够三角化足够多的地图点。并且保证了IMU预积分的连贯性。
为了保持系统的稀疏性，我们不会边缘化非关键帧的所有测量值。

E 相机速率下的状态估计–只有运动的VIO
采用了一种轻量级的纯运动视觉惯性BA，以提升状态估计速率到相机速率(30Hz)。
代价函数不变。
1、只对固定数量的最新IMU状态的姿态pose和速度v进行了优化，而不是对滑动窗口中的所有状态进行优化
2、将特征深度、外部参数、偏置和旧的IMU状态这些不希望优化的状态作为常量来处理。
与在最先进的嵌入式计算机上可能导致超过50ms的完全紧耦合单目VIO不同，这种纯运动的视觉惯性BA只需大约5ms来计算。

0.4 重定位
尽管滑动窗和边缘化减小了计算复杂度，但是仍旧引进了系统的累计漂移误差。具体来说，就是全局三维位置（xyz）和围绕重力方向的旋转（yaw）。作者采用紧耦合重定位模块与单目VIO进行组合实现漂移误差的消除。
目的：局部滑动窗口移动并与过去的位姿对齐。
vins的重定位模块主要包含回环检测,回环候选帧之间的特征匹配,****紧耦合重定位三个部分.

上图展示了重定位步骤。
1中VIO启动时刻只进行位姿估计（蓝色部分），过去状态一直被记录（绿色部分）。2中如果最新帧中回环被检测到，呈现红色虚线连接，表示启动重定位3。4中多个特征的多个观测直接用于重定位，从而提高了定位的精度和状态估计的平滑性。5-7是位姿优化。

A、回环检测（只对关键帧）

1、采用DBoW2****词袋位置识别方法进行回环检测。经过时间空间一致性检验后，DBoW2返回回环检测候选帧。
2、除了用于单目VIO的角点特征外，还添加了500个角点并使用BRIEF描述子，描述子用作视觉词袋在数据库里进行搜索。这些额外的角点能用来实现更好的回环检测。
3、VINS只保留所有用于特征检索的BRIEF描述子，丢弃原始图像以减小内存。
4、单目VIO可以观测到滚动和俯仰角，VINS并不需要依赖旋转不变性。

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

1、检测到回环时，通过BRIEF描述子匹配找到对应关系。但是直接的描述子匹配会导致很多外点。
2、本文提出两步几何剔除法：
1）2D-2D：使用RANSAC进行F矩阵测试，
2）3D-2D：使用RANSAC进行PnP，基于已知的滑动窗特征点的3D位置，和回路闭合候选处图像的2D观测（像素坐标）。
当内点超过一定阈值时，我们将该候选帧视为正确的循环检测并执行重定位。

C、紧耦合重定位

1、重定位过程使单目VIO维持的当前滑动窗口与过去的位姿图对齐。
2、将所有回环帧的位姿作为常量，利用所有IMU测量值、局部视觉测量和从回环中提取特征对应值，共同优化滑动窗口。

和之前VIO优化模型不同的是，增加了回环项，从位姿图获得回环帧的姿态被视为常数。
在重定位之后（重定位只是基于检测到的回环处暂时对滑动窗里的位姿进行重新优化）下一步要对过去位姿和闭合回路图像帧的全局优化。

0.5 全局位姿图优化
这一步是为了确保基于重定位结果对过去的位姿进行全局优化。
由于视觉-惯性 使得横滚角和俯仰角完全可以观测，因此只有（XYZ和yaw航向）4个自由度存在累积漂移。接下来只进行4-DOF的位姿图优化。

A、位姿图中添加关键帧
当一个关键帧被滑动窗口中边缘化掉后，它会被添加到位姿图中。该关键帧会作为位姿图中一个定点，通过下面两类边与其他顶点相连接：

1、顺序边（Sequential Edge）： 关键帧将建立与之前关键帧的几个顺序边，一个顺序边表示局部滑动窗口中两个关键帧之间的相对转换，它的值直接从VIO中获取。令最新边缘化掉的关键帧为i，它的一个以前的关键帧为j，顺序边只包含相对位置和相对航向.

2、回路闭合边（Loop Closure Edge）： 如果最新的边缘化掉的关键帧存在回路连接，它可以通过位姿图中的回路比河边和回路闭合帧相连接。回环边的值由重定位结果得出。
B、4自由度位姿图优化
关键帧i和j之间的残差最小化表示为；


通过最小化以下代价函数，对顺序边和回环边的整个图进行优化：

S是所有顺序边的集合，L是回环边的集合。 尽管紧耦合的重定位已经有助于消除错误的回环，但我们添加了另一个Huber范数 ρ(·)，以进一步减少任何可能的错误回环的影响。相反，我们不对顺序边使用任何鲁棒范数，因为这些边是从VIO中提取出来的，VIO已经包含了足够多的外点排除机制。
位姿图优化和重定位(VII-C)异步运行在两个独立的线程中。以便在需要重定位时，能立即使用最优化的位姿图。同样，即使当前的位姿图优化尚未完成，仍然可以使用现有的位姿图配置进行重新定位。这一过程如图9(b)所示。
C、位姿图管理
随着行程距离的增加，位姿图的大小可能会无限增长，从而限制了长时间系统的实时性。为此，我们实行了一个下采样过程：将位姿图数据库保持在有限的大小。**所有具有回环约束的关键帧都将被保留，而其他与相邻帧过近或方向非常相似的关键帧可能会被删除。**关键帧被移除的概率和其相邻帧的空间密度成正比。

1、IMU预积分

1.1、IMU模型

测量值：加速度计a^{、陀螺仪w}, 加上了bias游走和随机白噪声。
真实值：加速度计a、陀螺仪w。
实际情况下，可以获得测量值a^和w，需要反推真实值。一般忽略随机游走高斯噪声n
w=w^-bg; a=qwb(a^-ba)-gw;

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

将第k帧和第k+1帧所有的IMU进行积分，可得到第k+1帧的 PVQ，作为视觉估计的初始值。

a和w是IMU测量的加速度和角速度，相对于Body坐标系。

1.3、运动模型的离散积分（前后IMU）

从第 i个IMU时刻到第 i+1个IMU时刻的积分过程。两个相邻时刻k到k+1的位姿是由第k时刻测量值a^,w计算得出的。
这与Estimator::processIMU（）函数中Ps[j]、Rs[j]、Vs[j]是一致的，代码中j就是此处的i+1
IMU积分出来第 j 时刻数值作为第 j 帧图像初始值。

欧拉法

中值法

1.4、 IMU预积分

每次qwbt优化更新后，都要重新进行积分，运算量较大。
将积分模型转为预积分模型：
**PVQ积分公式中的积分项变为相对于第i时刻的姿态，**而不是相对于世界坐标系的姿态:

1.5、预积分量

预积分量只与IMU测量值有关。

1.6、预积分误差

一段时间内IMU构建的预积分量作为测量值，与估计值进行相减。

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

中值法：k到k+1时刻位姿由两时刻的测量值a w的平均值来计算。

1.8、bias 预积分量（bias发生变化）

因为 i 时刻的 bias 相关的预积分计算是通过迭代一步一步累计递推的，可以算但是太复杂。所以对于预积分量直接在 i 时刻的 bias 附近用一阶泰勒展开来近似，而不用真的去迭代计算。


https://zhuanlan.zhihu.com/p/60299750
https://blog.csdn.net/u014527548/article/details/86599047

2、基于舒尔补的边缘化

基于高斯牛顿的非线性优化理论可知，H*delta_x=b可以写成：

其中，delta_xa和delta_xb分别是希望marg掉的部分和保留部分。
VINS中需要边缘化滑动窗口中的最老帧，目的是希望不再计算这一帧的位姿或者与其相关的路标点，但是希望保留该帧对窗口内其余帧的约束关系。我们基于与移除状态相关的所有边缘化测量值构造一个先验。新的先验项被添加到现有的先验项中。

2.1、舒尔补

2.2 marg后形成的先验

xa为需要marg的变量，假设为相机pose,我们更关心如何求解希望保留的xb，而不再求解xa（即marg的变量改为0，左乘时左上是0），这里是要变化为上三角。


即：new_H*delta_xb=new_b;
形成新的信息矩阵new_H具体流程：

**注意：**去掉了x1,但是之前和x1相连的所有量x2 x3 x4 x5 在marg掉x1后变得两两相连。

2.3 具体例子

2.3.1 原来的信息矩阵H的构成
上述最小二乘问题，对用的高斯牛顿求解为：


矩阵乘法公式写成连加：
**雅克比J和信息矩阵H的稀疏性：**由于每个残差只和某几个状态量有关，因此，雅克比矩阵求导时，无关项的雅克比为 0。

将五个残差的信息矩阵加起来，得到样例最终的信息矩阵 Λ, 可视化如下:

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


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


参考文献：
VINS技术路线与代码详解 by 五行缺帅wangshuailpp
VINS-Mono论文学习与代码解读——目录与参考 by Manii
VIO-Doc——崔华坤
VINS-mono详细解读 by 极品巧克力