VINS-Mono代码详解 ——— （0）原理框图 + ROS 基础知识 + 代码目录图

一、VINS原理图

1 前端（数据预处理）

1. 相邻帧的光流跟踪：建立当前与滑窗中其他共视关系，即重投影误差约束
2. Shi -Tomas特征点提取：保证一定数量的特征点
3. IMU 预积分：计算当前帧的位姿作为初始值，计算IMU 约束的误差项、协方差和Jacobian

2 后端（滑窗优化）

1. 联合优化视觉重投影误差、IMU运动约束、先验约束

3 初始化

1. 基于视觉信息来计算运动轨迹
2. 基于IMU信息来计算运动轨迹
3. 视觉和IMU松耦合来优化尺度、速度、重力

4 闭环

1. 通过词袋找闭环帧，暴力匹配进行特征点滑窗优化的相对位姿
2. 利用闭环帧的相对位姿进行轨迹四自由度优化

二、ROS 基础知识

1. 运行VINS-Mono

$ roslaunch vins_estimator euroc.launch
$ roslaunch vins_estimator vins_rviz.launch 
$ rosbag play YOUR_PATH_TO_DATASET/MH_03_medium.bag 

1. master —— 节点管理器，负责管理所有节点
2. node —— 节点，相互独立的可执行文件
3. topic—— 主题，节点之间通信的主题
4. rosrun —— 一次运行—个node
5. roslaunch —— 一次运行多个node

  roslaunch  功能包名称  launch文件 [参数设置]

这里roslaunch命令会在对应的功能包中找到名称匹配的launch文件，并执行这个launch文件。如果设置了参数的话，会将参数传入到launch文件中赋给对应的参数。

例如，执行vins_estimator功能包中的euroc.launch文件，命令如下：

roslaunch vins_estimator euroc.launch 

如果在launch文件所在路径下执行该launch文件的话，就不需要加功能包名称了，因为此时是能直接找到launch文件的。

如果运行launch文件时需要设置参数，如下所示。arg为参数名称，value为给参数的赋值。

roslaunch my_file.launch arg:=value

注意：在执行roslaunch命令时，一定要先执行source命令：source …/devel/setup.bash命令，否则找不到对应的包名称。

6. package —— 一个功能包，包含多个node

2. rqt_graph（显示node）

rqt_graph是用图形表示当前活动中的节点与在ROS网络上传输的消息之间的相关性的工具。这对了解当前ROS网络情况非常有用。
使用方法：
在catkin_ws文件夹下打开打开四个终端分别运行以下命令：

roscore
//不要忘记设置环境变量：source devel/setup.bash
source devel/setup.bash && roslaunch vins_estimator euroc.launch 
source devel/setup.bash && roslaunch vins_estimator vins_rviz.launch
source ~/catkin_ws/devel/setup.bash && rosbag play MH_01_easy.bag

重新打开新终端：

sudo dot -c
rqt_graph

node only（模式）

node all（模式）

代码主要包括3个node：feature_tracker（前端），vins_estimator（后端），pose_graph（闭环）：

1. feature_tracker —— 仅负责特征点提取和发布；
2. pose_graph —— 关键帧的选择/位姿图建立/回环检测；
3. vins_estimator —— 的内容最多了，包含了初始化，滑窗，优化等后端内容和IMU预积分等前端内容，并且在这个结点里分出了2个线程。

三、VINS目录结构

sfann@sfann-virtual-machine:~/catkin_ws/src$ tree
.
├── CMakeLists.txt -> /opt/ros/melodic/share/catkin/cmake/toplevel.cmake
└── VINS-Mono├── ar_demo       //一个ar应用demo│   ├── cmake│   │   └── FindEigen.cmake│   ├── CMakeLists.txt│   ├── launch│   │   ├── 3dm_bag.launch│   │   ├── ar_rviz.launch│   │   └── realsense_ar.launch│   ├── package.xml│   └── src│       └── ar_demo_node.cpp├── benchmark_publisher       //发布数据集中参考值(基准值)│   ├── cmake│   │   └── FindEigen.cmake│   ├── CMakeLists.txt│   ├── config│   │   ├── MH_01_easy│   │   │   └── data.csv│   │   ├── MH_02_easy│   │   │   └── data.csv│   │   ├── MH_03_medium│   │   │   └── data.csv│   │   ├── MH_04_difficult│   │   │   └── data.csv│   │   ├── MH_05_difficult│   │   │   └── data.csv│   │   ├── V1_01_easy│   │   │   └── data.csv│   │   ├── V1_02_medium│   │   │   └── data.csv│   │   ├── V1_03_difficult│   │   │   └── data.csv│   │   ├── V2_01_easy│   │   │   └── data.csv│   │   ├── V2_02_medium│   │   │   └── data.csv│   │   └── V2_03_difficult│   │       └── data.csv│   ├── launch│   │   └── publish.launch│   ├── package.xml│   └── src│       └── benchmark_publisher_node.cpp├── camera_model│   ├── cmake│   │   └── FindEigen.cmake│   ├── CMakeLists.txt│   ├── include│   │   └── camodocal│   │       ├── calib      //相机参数标定│   │       │   └── CameraCalibration.h│   │       ├── camera_models            //各类相机模型│   │       │   ├── CameraFactory.h│   │       │   ├── Camera.h│   │       │   ├── CataCamera.h│   │       │   ├── CostFunctionFactory.h│   │       │   ├── EquidistantCamera.h│   │       │   ├── PinholeCamera.h│   │       │   └── ScaramuzzaCamera.h│   │       ├── chessboard              //用于检测棋盘格特征点│   │       │   ├── ChessboardCorner.h│   │       │   ├── Chessboard.h│   │       │   ├── ChessboardQuad.h│   │       │   └── Spline.h│   │       ├── gpl│   │       │   ├── EigenQuaternionParameterization.h│   │       │   ├── EigenUtils.h│   │       │   └── gpl.h│   │       └── sparse_graph│   │           └── Transform.h│   ├── instruction│   ├── package.xml│   ├── readme.md│   └── src│       ├── calib│       │   └── CameraCalibration.cc│       ├── camera_models│       │   ├── Camera.cc│       │   ├── CameraFactory.cc│       │   ├── CataCamera.cc│       │   ├── CostFunctionFactory.cc│       │   ├── EquidistantCamera.cc│       │   ├── PinholeCamera.cc│       │   └── ScaramuzzaCamera.cc│       ├── chessboard│       │   └── Chessboard.cc│       ├── gpl│       │   ├── EigenQuaternionParameterization.cc│       │   └── gpl.cc│       ├── intrinsic_calib.cc        //相机矫正模块主函数│       └── sparse_graph│           └── Transform.cc├── config                      //硬件、系统配置文件│   ├── 3dm│   │   └── 3dm_config.yaml│   ├── AR_demo.rviz│   ├── black_box│   │   └── black_box_config.yaml│   ├── cla│   │   └── cla_config.yaml│   ├── euroc│   │   ├── euroc_config_no_extrinsic.yaml│   │   └── euroc_config.yaml│   ├── extrinsic_parameter_example.pdf│   ├── fisheye_mask_752x480.jpg│   ├── fisheye_mask.jpg│   ├── realsense│   │   ├── realsense_color_config.yaml│   │   ├── realsense_fisheye_config.yaml│   │   └── realsense_zr300│   ├── tum│   │   └── tum_config.yaml│   └── vins_rviz_config.rviz├── docker│   ├── Dockerfile│   ├── Makefile│   └── run.sh├── feature_tracker     //特征提取与特征跟踪 │   ├── cmake│   │   └── FindEigen.cmake   //直接将eigen写在cmake文件夹下，不用再额外下载，避免了在编译时候代码的报错│   ├── CMakeLists.txt│   ├── package.xml        //feature_tracker功能包的信息│   └── src        //核心，系统入口│       ├── feature_tracker.cpp     //特征点跟踪的具体实现│       ├── feature_tracker.h│       ├── feature_tracker_node.cpp      //main()函数,ROS接受图像的回调函数│       ├── parameters.cpp│       ├── parameters.h│       └── tic_toc.h├── LICENCE├── pose_graph│   ├── cmake│   │   └── FindEigen.cmake│   ├── CMakeLists.txt│   ├── package.xml│   └── src              //核心，系统入口│       ├── keyframe.cpp       //关键帧选取、描述子计算与匹配│       ├── keyframe.h│       ├── parameters.h│       ├── pose_graph.cpp        //位姿图的建立与图优化│       ├── pose_graph.h│       ├── pose_graph_node.cpp        //ROS 节点函数，回调函数，主线程│       ├── ThirdParty│       │   ├── DBoW│       │   │   ├── BowVector.cpp│       │   │   ├── BowVector.h│       │   │   ├── DBoW2.h│       │   │   ├── FBrief.cpp│       │   │   ├── FBrief.h│       │   │   ├── FClass.h│       │   │   ├── FeatureVector.cpp│       │   │   ├── FeatureVector.h│       │   │   ├── QueryResults.cpp│       │   │   ├── QueryResults.h│       │   │   ├── ScoringObject.cpp│       │   │   ├── ScoringObject.h│       │   │   ├── TemplatedDatabase.h│       │   │   └── TemplatedVocabulary.h│       │   ├── DUtils│       │   │   ├── DException.h│       │   │   ├── DUtils.h│       │   │   ├── Random.cpp│       │   │   ├── Random.h│       │   │   ├── Timestamp.cpp│       │   │   └── Timestamp.h│       │   ├── DVision│       │   │   ├── BRIEF.cpp│       │   │   ├── BRIEF.h│       │   │   └── DVision.h│       │   ├── VocabularyBinary.cpp│       │   └── VocabularyBinary.hpp│       └── utility│           ├── CameraPoseVisualization.cpp│           ├── CameraPoseVisualization.h│           ├── tic_toc.h│           ├── utility.cpp│           └── utility.h├── README.md├── support_files│   ├── brief_k10L6.bin│   ├── brief_pattern.yml│   ├── image│   │   ├── vins_black.png│   │   └── vins.png│   ├── paper│   │   └── tro_technical_report.pdf│   └── paper_bib.txt└── vins_estimator├── cmake│   └── FindEigen.cmake├── CMakeLists.txt├── launch│   ├── 3dm.launch│   ├── black_box.launch│   ├── cla.launch│   ├── euroc.launch│   ├── euroc_no_extrinsic_param.launch│   ├── realsense_color.launch│   ├── realsense_fisheye.launch│   ├── tum.launch│   └── vins_rviz.launch├── package.xml└── src├── estimator.cpp├── estimator.h├── estimator_node.cpp          //main()函数:多线程 measurement_process、loop_detection、 pose_graph├── factor                        //实现IMU、camera等残差模型，涉及了ceres优化库，Jacobian矩阵│   ├── imu_factor.h│   ├── integration_base.h│   ├── marginalization_factor.cpp│   ├── marginalization_factor.h│   ├── pose_local_parameterization.cpp│   ├── pose_local_parameterization.h│   ├── projection_factor.cpp│   ├── projection_factor.h│   ├── projection_td_factor.cpp│   └── projection_td_factor.h├── feature_manager.cpp       //特征点管理，三角化，关键帧操作├── feature_manager.h├── initial                   //系统初始化，外参标定，SFM│   ├── initial_aligment.cpp│   ├── initial_alignment.h│   ├── initial_ex_rotation.cpp│   ├── initial_ex_rotation.h│   ├── initial_sfm.cpp│   ├── initial_sfm.h│   ├── solve_5pts.cpp│   └── solve_5pts.h├── parameters.cpp         //外部系统设置参数输入├── parameters.h└── utility                //相机显示，四元数等数据转换├── CameraPoseVisualization.cpp├── CameraPoseVisualization.h├── tic_toc.h├── utility.cpp├── utility.h├── visualization.cpp└── visualization.h65 directories, 175 files