VINS-FUSION代码超详细注释（VIO部分）/VIO入门(1)

0 前言

目前网络上对于VINS-mono的代码已经有很多讲解和注释了，但是对于VINS-FUSION（以下简称VF）的注释还是很少的，刚好本人最近也正在学习VIO的相关知识，所以对VF按照程序执行顺序进行了十分详细的注释，同时为了和大家进行交流学习，所以把相关注释代码进行开源。因水平有限，错误肯定不少，还请各位大佬们指正。

前期准备

1. C++。
   没有c++基础的同学建议先观看北大的c++课程：c++程序设计

2. 视觉SLAM14讲。
   经典书籍，这个不必过多介绍。

3. VIO基础知识。
   深蓝学院有贺博和高博讲的VIO课程，可以自行百度。
   另外，崔华坤也有一系列的博文对VIO进行讲解。
   【泡泡读者来稿】VINS 论文推导及代码解析（一）
   【泡泡读者来稿】VINS 论文推导及代码解析（二）
   【泡泡读者来稿】VINS 论文推导及代码解析（三）
   【泡泡读者来稿】VINS 论文推导及代码解析（四）
   

本次工作

我首先一步步的把代码全部注释了，十分的详细，对于C++和OpenCV的一些操作也进行了详细的注释，对于刚入门的同学应该还是有帮助的。之后我将代码开源，并写了相应的博客进行讲解。

开源程序:

https://github.com/kuankuan-yue/VINS-FUSION-leanrning.git

相应博客：

VINS-FUSION代码超详细注释（VIO部分）/VIO入门(1)
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(2)
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(3)
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(4)

1 程序入口rosNodeTest.cpp

1.1 定义内容

运行程序时，首先进入的是主程序rosNodeTest.cpp
里边主要定义了 估计器、 缓存器 、 获取传感器数据的函数 和 一个主函数

// 获得左目的message
void img0_callback(const sensor_msgs::ImageConstPtr &img_msg)
// 获得右目的message
void img1_callback(const sensor_msgs::ImageConstPtr &img_msg)
// 从msg中获取图片，返回值cv::Mat，输入是当前图像msg的指针
cv::Mat getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg)
// 从两个主题中提取具有相同时间戳的图像
// 并将图像输入到估计器中
void sync_process()
// 输入imu的msg信息，进行解算并把imu数据输入到estimator
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
// 把特征点的点云msg输入到estimator
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
// 是否重启estimator，并重新设置参数
void restart_callback(const std_msgs::BoolConstPtr &restart_msg)
// 是否使用IMU
void imu_switch_callback(const std_msgs::BoolConstPtr &switch_msg)
// 相机的开关
void cam_switch_callback(const std_msgs::BoolConstPtr &switch_msg)
int main(int argc, char **argv)

1.2 程序执行

1.2.1 获取参数并设置参数

具体的方法在函数

主函数中，主要是执行以下各个步骤订阅ROS信息，然后进行处理

readParameters(config_file);// 读取参数
estimator.setParameter();// 设置参数

ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_feature = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
ros::Subscriber sub_img0 = n.subscribe(IMAGE0_TOPIC, 100, img0_callback);
ros::Subscriber sub_img1 = n.subscribe(IMAGE1_TOPIC, 100, img1_callback);
ros::Subscriber sub_restart = n.subscribe("/vins_restart", 100, restart_callback);
ros::Subscriber sub_imu_switch = n.subscribe("/vins_imu_switch", 100, imu_switch_callback);
ros::Subscriber sub_cam_switch = n.subscribe("/vins_cam_switch", 100, cam_switch_callback);

其中有几个比较重要的函数，在下方进行说明。

1.2.2 imu_callback

其中imu_callback中订阅imu信息，并将器填充到accBuf和gyrBuf中，之后执行了fastPredictIMU和pubLatestOdometry

fastPredictIMU使用上一时刻的姿态进行快速的imu预积分

// 使用上一时刻的姿态进行快速的imu预积分
// 用来预测最新P,V,Q的姿态
// -latest_p,latest_q,latest_v,latest_acc_0,latest_gyr_0 最新时刻的姿态。这个的作用是为了刷新姿态的输出，但是这个值的误差相对会比较大，是未经过非线性优化获取的初始值。
void Estimator::fastPredictIMU(double t, Eigen::Vector3d linear_acceleration, Eigen::Vector3d angular_velocity)

pubLatestOdometry构建一个odometry的msg并发布

//构建一个odometry的msg并发布
void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, double t)

1.2.3 feature_callback

feature_callback的作用是获取点云数据，之后填充featureFrame，并把featureFrame通过inputFeature输入到estimator，且填充了featureBuf

1.2.4 sync_process

之后通过

std::thread sync_thread{sync_process}; //创建sync_thread线程，指向sync_process，这里边处理了measurementpross的线程

进入sync_process进行处理

// 从两个主题中提取具有相同时间戳的图像
// 并将图像输入到估计器中
void sync_process()

该函数中，首先对是否双目进行判断。
如果是双目，需要检测同步问题。对双目的时间进行判断，时间间隔小于0.003s的话则使用getImageFromMsg将其输入到image0和image1变量之中。之后estimator.inputImage。
如果是弹幕，则直接estimator.inputImage

结束

至此，主程序rosNodeTest.cpp就已经全部运行完了，之后会跳到其他各个文件中进行图片的处理，优化等等，详情请查看之后的博客。