1.vinsfusion-从数据流看程序

本节介绍，图像数据和惯导数据是如何传入程序中的，以及之后的图像数据格式转化、和惯导数据的初步处理。
图像数据流
1.订阅话题数据：

     // 订阅左图
    ros::Subscriber sub_img0 = n.subscribe(IMAGE0_TOPIC, 100, img0_callback);
    // 订阅右图
    ros::Subscriber sub_img1 = n.subscribe(IMAGE1_TOPIC, 100, img1_callback);

2.img0_callback、img1_callback回调函数将接受到的图像存储到图像队列

queue<sensor_msgs::ImageConstPtr> img0_buf;
queue<sensor_msgs::ImageConstPtr> img1_buf;

3.getImageFromMsg将img0_buf和img1_buf数据转换成cv：：mat格式返回。

4.sync_process函数
从两个图像队列中取出最早的一帧，并从队列删除，双目要求两帧时差不得超过0.003s，同时将图像输入到估计器中。

estimator.inputImage(time, image0, image1);

5.总结
上述过程算是图像的预处理过程，主要任务接收存储图像并转换图像数据格式，确定图像和时间的数据结构，传入estimator估计器进行处理。

6.补充

  ros::Subscriber sub_feature = n.subscribe("/feature_tracker/feature", 2000, feature_callback);

订阅帧跟踪的特征点，主要是feature_tracker的跟踪信息，包括相机归一化坐标系的x、y、z和像素坐标系坐标和像素坐标系的速度，然后放入estimator估计器中。

  xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
        featureFrame[feature_id].emplace_back(camera_id,  xyz_uv_velocity);

主要函数的调用顺序为：

    // 订阅一帧跟踪的特征点
    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);
    //sync_process图像格式转换，按时间轴确定数据结构，放入估计器
     std::thread sync_thread{sync_process};

随后的处理过程在sync_process函数进入estimator.inputImage中开始

2.imu数据流：
1.imu_callback中订阅imu信息

// 订阅IMU
    ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());

//&imu_msg订阅数据的数据地址
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
  .....
  estimator.inputIMU(t, acc, gyr);
  }

2.通过estimator估计器处理并将数据填充到accBuf和gyrBuf中，

  accBuf.push(make_pair(t, linearAcceleration));
    gyrBuf.push(make_pair(t, angularVelocity));

3.执行vins_estimator/src/estimator/estimator.cpp中inputIMU函数的fastPredictIMU
得到latest_time，latest_Q，latest_P，latest_V，latest_acc_0，latest_gyr_0变量

void Estimator::fastPredictIMU(double t, Eigen::Vector3d linear_acceleration, Eigen::Vector3d angular_velocity)
{
    double dt = t - latest_time;
    latest_time = t;
    // 前一帧加速度（世界系）
    Eigen::Vector3d un_acc_0 = latest_Q * (latest_acc_0 - latest_Ba) - g;
    // 更新旋转Q
     Eigen::Vector3d un_gyr = 0.5 * (latest_gyr_0 + angular_velocity) - latest_Bg;
    latest_Q = latest_Q * Utility::deltaQ(un_gyr * dt);
    //更新加速度
    Eigen::Vector3d un_acc_1 = latest_Q * (linear_acceleration - latest_Ba) - g;
    Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
    // 更新位置P
    latest_P = latest_P + dt * latest_V + 0.5 * dt * dt * un_acc;
    // 更新速度V
    latest_V = latest_V + dt * un_acc;
    latest_acc_0 = linear_acceleration;
    latest_gyr_0 = angular_velocity;

4.通过pubLatestOdometry函数发布

pub_latest_odometry.publish(odometry);

发布的话题名字为：imu_propagate

 pub_latest_odometry = n.advertise<nav_msgs::Odometry>("imu_propagate", 1000);