VINS-Mono和VINS-Mobile是香港科技大学沈劭劼团队开源的单目视觉惯导SLAM方案。是基于优化和滑动窗口的VIO，使用IMU预积分构建紧耦合框架。并且具备自动初始化，在线外参标定，重定位，闭环检测，以及全局位姿图优化功能。
方案最大的贡献是构建了效果很好的融合算法，视觉闭环等模块倒是使用了较为常见的算法。
系列博客将结合课题组发表的paper，从代码层面，逐步剖析系统的各个模块，达到对单目VIO整体的把握，帮助自己理解各类算法，并开发出针对应用场景的视觉惯导SLAM系统。最终目标是使用在AR应用中（Android）。
主要分为五部分
1. 传感器数据处理：
2. 初始化：
3. 基于滑动窗口的非线性优化：
4. 闭环检测：
5. 4自由度全局位姿图优化：
主要依赖的库只有OpenCV， Eigen和Ceres Solver，代码目录如下
核心算法都在feature_tracker和vins_estimator包中。
按照REDEME步骤跑EuRoC/MH_05_difficult.bag录好的数据结果如下：
使用rqt_graph得到系统的node和topic关系：
rosbag将记录好的imu数据和单目相机获取的图像数据分别发布到/imu0和/cam0/image_raw话题；/feature_tracker节点通过订阅/cam0/image_raw话题获取图像数据，/vins_estimator节点通过订阅/imu0话题获取imu数据，同时/feature_tracker节点将提取出的图像特征发布到/feature_tracker/feature话题，由/vins_estimator订阅获取。
因此，/feature_tracker节点负责视觉提取和跟踪，/vins_estimator则是融合系统的主要部分。
为了方便看代码，整理了一下各个部分架构图（更新中）：
系统入口是feature_tracker_node.cpp文件中的main函数
%YAML:1.0
#common parameters
imu_topic: "/imu0"
image_topic: "/cam0/image_raw"
#camera calibration
model_type: PINHOLE
camera_name: camera
image_width: 752
image_height: 480
distortion_parameters:
k1: -2.917e-01
k2: 8.228e-02
p1: 5.333e-05
p2: -1.578e-04
projection_parameters:
fx: 4.616e+02
fy: 4.603e+02
cx: 3.630e+02
cy: 2.481e+02
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
# 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
ex_calib_result_path: "/config/euroc/ex_calib_result.yaml" # If you choose 1 or 2, the extrinsic calibration result will be written vins_folder_path + ex_calib_result_path.
#If you choose 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
extrinsicRotation: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [0, -1, 0,
1, 0, 0,
0, 0, 1]
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix
rows: 3
cols: 1
dt: d
data: [-0.02,-0.06, 0.01]
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
equalize: 1 # if image is too dark or light, trun on equalize to find enough features
fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.2 # accelerometer measurement noise standard deviation. #0.2
gyr_n: 0.02 # gyroscope measurement noise standard deviation. #0.05
acc_w: 0.0002 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.81007 # gravity magnitude
#loop closure parameters
loop_closure: 1 #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly;
#also give the camera calibration file same as feature_tracker node
pattern_file: "/support_files/brief_pattern.yml"
voc_file: "/support_files/brief_k10L6.bin"
min_loop_num: 25该config.yaml文件中的其他参数在vins_estimator_node中被读取，属于融合算法的参数。
ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
前端视觉的算法基本在这个回调函数中，步骤为：
1. 频率控制，保证每秒钟处理的image不多于FREQ；
2. 对于单目：
1）. readImage;
2）. showUndistortion(可选);
3）. 将特征点矫正（相机模型camodocal）后归一化平面的3D点（此时没有尺度信息，3D点p.z=1），像素2D点，以及特征的id，封装成ros的sensor_msgs::PointCloud消息类型;
3. 将处理完的图像信息用PointCloud和Image的消息类型，发布到"feature"和"feature_img"的topic：
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
1. CLAHE(Contrast Limited Adaptive Histogram Equalization)
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));2. Optical Flow(光流追踪)
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);3. 根据匹配点计算Fundamental Matrix， 然后用Ransac剔除不符合Fundamental Matrix的外点
cv::findFundamentalMat(un_prev_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);4. 特征点检测：goodFeaturesToTrack， 使用Shi-Tomasi的改进版Harris corner
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.1, MIN_DIST, mask);特征点之间保证了最小距离30个像素，跟踪成功的特征点需要经过rotation-compensated旋转补偿的视差计算，视差在30个像素以上的特征点才会去参与三角化和后续的优化，保证了所有的特征点质量都是比较高的，同时降低了计算量。