SLAM： 用测试数据集来测试VINS

目前编写双目VINS代码，发现双目VINS初始化后的尺度存在问题（理想化双目尺度应该为1），所以想着利用无误差的测试集来测试一下自己写的代码。我先在单目VINS下做了测试，下面是我利用github上贺所长的单目测试集在单目vins上测试的过程：

1.单目VINS测试数据集：https://github.com/HeYijia/vio_data_simulation.

（下载后通过mkdir build；cd build; cmake ..; make ; cd ../bin; ./data_gen 命令生成待读取数据，数据存储在bin/里面）

    （1）gener_alldata.cpp :

             A.  createpointslines():   通过这个函数取出设计好的房子图形中的map点，之后所有的图像都能观测到这些map点，且顺  序不变；

            B.  imudata  和 imudata_noise:   只用了imudata；

            C. camdata：  完全由imudata 和map点决定，所以没有误差；

     （2）imudata的生成：imu.cpp

             A. MotionModel:  直接给出imu的线加速度和角加速度 以及imu的R、t；（需要推导）

2.在VINS中新建两个节点分别读取feature数据 和 imu数据：

    （1）其中map点在各帧观测的归一化坐标存储在 vio_data_simulation/bin/keyframe/all_points_x.txt中

          （6维，map3维+1+归一化坐标点2维），

    （2）feature的时间 存储在vio_data_simulation/bin/cam_pose.txt中

           （14维，时间1维+q4维+t3维+陀螺仪加速度3维+线加速度3 维）， 

   （3）imu数据存储在vio_data_simulation/bin/imu_pose.txt（无噪声）

             （ 与cam_pose存储相同，只需要时间和gyro、acc 7维）。

3.节点发布：

      （1）feature发布：注意频率为30

    ros::init(argc, argv, "test_pub_feature");
    ros::NodeHandle n("~");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

    pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
    test_s();// 读取feature 和 time 数据
    int count = 0;
    ros::Rate loop_rate(30);
    while(ros::ok())
    {
      pub_feature(all_points[count],cam_time[count]);
      ros::spinOnce();
      loop_rate.sleep();
      count ++;
    }

      （2）imu发布：注意频率为200(与上面类似)

4.参数修改：projection_parameters + cam to imu 的 R、t<