vins_fusion入门系列-数据的读取

vins_fusion入门系列-数据的读取

VINS_FUSION功能介绍

港科大在2019年1月12号发布了Vins-fusion，从2017年发布的Vins-mono:单目+IMU，这次晋级了vins-fusion，demo中主要给出了四个版本：
（1）单目+imu
（2）纯双目
（3）双目+imu
（4）VIO+GPS
地址为：https://github.com/HKUST-Aerial-Robotics/VINS-Fusion
双目SLAM对比与之前单目的slam，主要一个好处在于初始化过程中，可以静止进行初始化。另一方面由于尺度信息不一定完全依靠IMU,因此不会造成尺度不可关的情况。但是由于视觉误匹配等各种原因，造成实际上双目的精度会比单目来的差一丢丢。不过鲁棒性上考虑，双目明显优于单目。

程序入口

１．定义内容

运行程序时，首先进入的是主程序rosNodeTest.cpp
里边主要定义了 估计器、 缓存器 、 获取传感器数据的函数 和 一个主函数
vins_estimator:: 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)

２．程序执行

main（）函数中：
a.读取配置文件参数 readParameter()
b.订阅了四个话题，分别是imu消息，两个相机图像，和feature_tracker所提供的跟踪光流点
c.同时开启了一个新线程sync_process。线程的作用：如果图像buffer里面有数据的话,读入数据并且添加到estimator中。为什么不在相机图像的回调函数就input，我的理解在于对于双目的话，能够检测同步问题，能够将同样时间戳的两帧图片同时放入estimator中。所以对于Imu以及feature直接在回调函数中进行添加。

/*
    ros::Subscriber subscribe (const std::string &topic, uint32_t queue_size, void(*fp)(M), const TransportHints &transport_hints=TransportHints())
    第一个参数是订阅话题的名称；
    第二个参数是订阅队列的长度；（如果收到的消息都没来得及处理，那么新消息入队，旧消息就会出队）；
    第三个参数是回调函数的指针，指向回调函数来处理接收到的消息！
    第四个参数：似乎与延迟有关系，暂时不关心。（该成员函数有13重载）
    */
    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);

    std::thread sync_thread{sync_process}; //创建sync_thread线程，指向sync_process，这里边处理了processMeasurements的线程
    ros::spin(); // 用于触发topic, service的响应队列

２.1　imu_callback

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

// 输入imu的msg信息，进行解算并把imu数据输入到estimator
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
    double t = imu_msg->header.stamp.toSec();//讲头文件中的时间戳转换为s
    double dx = imu_msg->linear_acceleration.x;
    double dy = imu_msg->linear_acceleration.y;
    double dz = imu_msg->linear_acceleration.z;
    double rx = imu_msg->angular_velocity.x;
    double ry = imu_msg->angular_velocity.y;
    double rz = imu_msg->angular_velocity.z;
    Vector3d acc(dx, dy, dz);
    Vector3d gyr(rx, ry, rz);
    estimator.inputIMU(t, acc, gyr);
    return;
}

２.2 feature_callback

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

// 把特征点的点云msg输入到estimator
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
    map>>> featureFrame;
    // 数据格式为feature_id camera_id xyz_uv_velocity

    // 把点云中所有的点
    for (unsigned int i = 0; i < feature_msg->points.size(); i++)
    {
        int feature_id = feature_msg->channels[0].values[i];
        int camera_id = feature_msg->channels[1].values[i];
        double x = feature_msg->points[i].x;
        double y = feature_msg->points[i].y;
        double z = feature_msg->points[i].z;
        double p_u = feature_msg->channels[2].values[i];
        double p_v = feature_msg->channels[3].values[i];
        double velocity_x = feature_msg->channels[4].values[i]; //特征点的像素速度
        double velocity_y = feature_msg->channels[5].values[i];
        if(feature_msg->channels.size() > 5)
        {
            double gx = feature_msg->channels[6].values[i];
            double gy = feature_msg->channels[7].values[i];
            double gz = feature_msg->channels[8].values[i];
            pts_gt[feature_id] = Eigen::Vector3d(gx, gy, gz); 
            //printf("receive pts gt %d %f %f %f\n", feature_id, gx, gy, gz);
        }
        ROS_ASSERT(z == 1);//FIXME: 
        Eigen::Matrix xyz_uv_velocity;
        xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
        featureFrame[feature_id].emplace_back(camera_id,  xyz_uv_velocity);
            //emplace_back能就地通过参数构造对象，不需要拷贝或者移动内存，相比push_back能更好地避免内存的拷贝与移动，使容器插入元素的性能得到进一步提升。
    }
    double t = feature_msg->header.stamp.toSec();
    estimator.inputFeature(t, featureFrame);
    return;
}

２.３sync_process

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

// 从两个主题中提取具有相同时间戳的图像
// 并将图像输入到估计器中
void sync_process()
{
    while(1)
    {
        if(STEREO)//如果是双目
        {
            cv::Mat image0, image1;
            std_msgs::Header header;
            double time = 0;
            m_buf.lock();
            if (!img0_buf.empty() && !img1_buf.empty())
            {
                double time0 = img0_buf.front()->header.stamp.toSec();//front 返回值为队列中的第一个元素，也就是最早、最先进入队列的元素
                double time1 = img1_buf.front()->header.stamp.toSec();
                // 0.003s sync tolerance
                if(time0 < time1 - 0.003)
                {
                    img0_buf.pop(); //将队列中最靠前位置的元素拿掉
                    printf("throw img0\n");
                }
                else if(time0 > time1 + 0.003)
                {
                    img1_buf.pop();
                    printf("throw img1\n");
                }
                else
                // 这里的话就是时间对其了的话
                {
                    time = img0_buf.front()->header.stamp.toSec();
                    header = img0_buf.front()->header;
                    image0 = getImageFromMsg(img0_buf.front());
                    img0_buf.pop();
                    image1 = getImageFromMsg(img1_buf.front());
                    img1_buf.pop();
                    //printf("find img0 and img1\n");
                }
            }
            m_buf.unlock();
            if(!image0.empty())
                estimator.inputImage(time, image0, image1);
        }
        else
        {
            cv::Mat image;
            std_msgs::Header header;
            double time = 0;
            m_buf.lock();
            if(!img0_buf.empty())
            {
                time = img0_buf.front()->header.stamp.toSec();
                header = img0_buf.front()->header;
                image = getImageFromMsg(img0_buf.front());
                img0_buf.pop();
            }
            m_buf.unlock();
            if(!image.empty())
                estimator.inputImage(time, image);
        }

        std::chrono::milliseconds dura(2);
        std::this_thread::sleep_for(dura);
    }
}