VINS-MONO学习框架记录（精简版）

写在前面：由于Vins对初学者来说会感觉系统庞大，各部分函数关系复杂，所以本文主要是为了理清系统的框架思路，同时会注重讲解一些比较重点的地方。由于在最开始学习前端和IMU预积分部分的时候还没开始想过写博客，所以这部分的细节后面补充。当然整个系统的细节也会继续完善。（真希望我当时开始学的时候能够看到这样的贴子。。。哈哈哈开玩笑的，第一次写博客讲真是为了自己能够在后面思路更加清晰，也希望能够帮助到初次接触Vins的学者。）
学习中，更新中

VINS-MONO的基本框架

IV: 测量预积分
V：初始化
VI: 紧耦合单目VIO
VII：重定位
VIII：全局位姿图优化

(五个部分：
测量预积分、初始化、后端非线性优化、闭环检测、闭环优化)

IV: 测量预积分

IV1-视觉处理前端（camera部分）

1.代码部分——feature_tracker文件夹

IV2-IMU预积分（IMU部分）

1.代码部分——integration_base.h+imu_factor.h

2.主要作用：利用IMU的测量值来计算PVQ对应的预积分量，并由此计算各自的雅可比矩阵。

V: 初始化（松耦合—initialStructure）

初始化的目的：
恢复出尺度、重力、速度和IMU的bias（加速度bias可忽略）。

步骤：

1.用SFM进行纯视觉估计滑动窗内所有帧的位姿和路标点逆深度；
2.视觉与IMU对齐，恢复出尺度s、重力g、IMU速度v和陀螺仪偏置bg。

v1-若无qbc,则求相机与IMU之间的相对旋转（手眼标定法）

v2-检测IMU的可观性

• 即通过加速度标准差判断IMU是否有充分运动以初始化

v3-相机初始化(Vision-Only SFM)

• 1.根据本质矩阵求解位姿(relativePose)——对极几何、SVD分解得R、T
• 2.三角化特征点(sfm.construct) ——三角测量得s1、s2
• 3.PnP 求解所有图像帧的位姿(cv::solvePnP)
• 4.转换到 IMU 坐标系下
• 5.c0坐标系作为参考系
• 6.不断重复直到恢复出滑窗内的 Features 和相机位姿

v3.1-根据本质矩阵求解位姿(relativePose)——对极几何、SVD分解得R、T

1.代码部分：estimator.cpp中的relativePose()函数；
   其中relativePose()函数包含两个重要函数：getCorresponding()和solveRelativeRT()。

2.上述上三个函数的功能：
estimator.cpp→relativePose()：通过基础矩阵计算当前帧与第l帧之间的R和T；
feature_manager.cpp→getCorresponding()：返回两帧匹配特征点3D坐标；
solve_5pts.cpp→solveRelativeRT()：返回当前帧到参考帧的relative_R和relative_T。

v3.2-三角化特征点(sfm.construct)——三角测量得s1、s2

1.代码部分：在estimator.cpp的initialStructure中调用initial_sfm.cpp的GlobalSFM结构。
2.GlobalSFM结构解析：
(1)GlobalSFM.construct:纯视觉sfm，求解窗口中的所有图像帧的位姿和特征点坐标。
    具体流程：
a.先三角化第l帧（参考帧）与第frame_num-1帧（当前帧）的路标点——因为目前只有这两帧的相对位姿已知；
b.用solveFrameByPnP()函数pnp求解从第l+1开始的每一帧到第l帧的相对位姿，并调用triangulateTwoFrames()函数进行三角化；
c.过程与b相同，只是对象变为从第l-1到第0帧；
d.三角化其他未恢复的特征点；
e.使用cares进行全局BA优化（相机位姿和特征点坐标）。

v3.3- PnP 求解所有图像帧的位姿(cv::solvePnP)
          ——对于所有的图像帧，包括不在滑动窗口中的，提供初始的RT估计

v4-视觉与IMU对齐（visualInitialAlign函数）

• 1.陀螺仪Bias标定
• 2.初始化速度、重力向量和尺度因子
• 3.优化重力
• 4.确定世界坐标系
• 5.将相机坐标系对齐到世界坐标系

v4.1-陀螺仪Bias标定
        (并更新IMU数据，重新计算IMU预积分)

1.代码部分：initial_aligment.cpp中的solveGyroscopeBias()函数。
   实际调用路径：
   estimator.cpp→visualInitialAlign()→initial_aligment.cpp的VisualIMUAlignment()→solveGyroscopeBias()。

2.理论：

       前两个值由SFM可以求得，第三个值为IMU预积分

v4.2-利用IMU平移，初始化速度、重力向量和尺度因子

1.代码部分：initial_aligment.cpp中的LinearAlignment()函数。
   实际调用路径：
   estimator.cpp→visualInitialAlign()→initial_aligment.cpp的VisualIMUAlignment()→LinearAlignment()。

v4.3-利用|gw|已知的先验条件，优化gc0

1.代码部分：initial_aligment.cpp中的RefineGravity()和TangentBasis()函数。
   实际调用：
   这两个函数实际上已经包括在v4.2所调用的LinearAlignment()中，
   也就是LinearAlignment()调用了RefineGravity(),
   而RefineGravity()调用了TangentBasis()。

v4.4-利用gw和gc0确定世界坐标系

1.代码部分：estimator.cpp中的visualInitialAlign()函数的后部分。

v4.5-将相机坐标系对齐到世界坐标系

1.代码部分：estimator.cpp中的visualInitialAlign()函数的后部分。

VI: 紧耦合单目VIO（processImage后部分）

首先看一下estimator.cpp中的processImage()函数：

• 1.关键帧的判断
• 2.将图像数据、时间、临时预积分值存到图像帧imageframe容器中
• 3.如果没有外参则标定IMU到相机的外参
• 4.初始化!
• 5.紧耦合的非线性优化

可见后端优化在processImage()函数第5部分（紧耦合的非线性优化和滑动窗）：

• 5.1 非线性化求解里程计(solveOdometry)
• 5.2故障检测与恢复(failureDetection)
• 5.3 滑动窗口(slideWindow)
• 5.4 移除失败的点(removeFailures)

5.1 非线性化求解里程计(solveOdometry)

solveOdometry()函数就是调用了triangulate(获得最新特征的深度)和optimization(非线性优化和边缘化)两个函数。

optimization()函数：非线性优化和边缘化

（1）紧耦合非线性优化：

a. 声明和引入鲁棒核函数
b. 添加各种待优化量X——位姿优化量
c. 添加各种待优化量X——相机外参
d. 添加各种待优化量X——IMU-image时间同步误差
e. vector2double()
f. 添加各种残差——先验信残差(边缘化残差)
g. 添加各种残差——IMU残差
h1. 添加各种残差——重投影残差
h2. 添加各种残差——回环检测
i.求解
j. double2vector()

（2）边缘化处理：

    情况1：次新帧是关键帧，将边缘化最老帧

1、将上一次先验残差项传递给marginalization_info
2、将第0帧和第1帧间的IMU因子IMUFactor，添加到marginalization_info中
3、将第一次观测为第0帧的所有路标点对应的视觉观测，添加到marginalization_info中
4、计算每个残差，对应的Jacobian，并将各参数块拷贝到统一的内存（parameter_block_data）中
5、多线程构造先验项舒尔补AX=b的结构，在X0处线性化计算Jacobian和残差
6、调整参数块在下一次窗口中对应的位置（往前移一格）

    情况2：次新帧不是关键帧，将边缘化次新帧

1、保留次新帧的IMU测量，丢弃该帧的视觉测量，将上一次先验残差项传递给marginalization_info
2、premargin
3、marginalize
4、调整参数块在下一次窗口中对应的位置（去掉次新帧）

5.2故障检测与恢复(failureDetection)

5.3 滑动窗口(slideWindow)

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

5.4 移除失败的点(removeFailures)

VII: 重定位

addKeyFrame()函数：获取当前帧与回环帧相关信息，优化当前帧

//1.建一个新的图像序列
//2.getVioPose()获取当前帧的位姿vio_P_cur、vio_R_cur并更新updateVioPose
//3.detectLoop()回环检测，返回回环候选帧的索引loop_index；并调用findConnection()进一步判断当前关键帧和闭环候选帧之间的闭环情况
//4.计算当前帧与回环帧的相对位姿，纠正当前帧位姿w_P_cur、w_R_cur；回环得到的位姿和VIO位姿之间的偏移量shift_r、shift_t
//5.获取VIO当前帧的位姿P、R，根据偏移量得到实际位姿并更新updatePose当前帧的位姿P、R
//6.发布path[sequence_cnt]
//7.保存闭环轨迹到VINS_RESULT_PATH

detectLoop()函数：回环检测返回候选帧索引

//1.query()查询字典数据库，得到与每一帧的相似度评分ret
//2.add()添加当前关键帧到字典数据库中
//3.hconcat()通过相似度评分判断是否存在回环候选帧
//4.对于索引值大于50的关键帧才考虑进行回环,即前50帧不回环,返回评分大于0.015的最早的关键帧索引min_index

VII: 位姿图优化（optimize4DoF函数）

由于重力所对应的发平面只能改变x、y、z、yaw，故而是4自由度优化。

一、位姿图优化（pose_graph.cpp的optimize4DoF()函数）

二、位姿存储、加载、合并

1.存储：通过pose_graph_node.cpp的command()函数决定是否存储；通过pose_graph.cpp的savePoseGraph()函数保存位姿图。
2.加载：pose_graph.cpp的loadPoseGraph()函数加载位姿图。
3.合并：pose_graph.cpp的loadKeyFrame()函数合并位姿图。

引用：
学习中看了很多文档，有些没有记下出处的，若有侵权，必定修改。
高博的《SLAM十四讲》
https://blog.csdn.net/iwanderu/article/details/104617829
https://blog.csdn.net/iwanderu/article/details/104617829
https://blog.csdn.net/try_again_later/article/details/104854698