运行VINS_Fusion

注释代码
(visualization、pose_grapf、pose_grapf_node)

编译：

    cd ~/catkin_ws/src
    git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
    cd ../
    catkin_make
    source ~/catkin_ws/devel/setup.bash

运行代码

   // 执行启动文件
    source ~/vins/devel/setup.bash
    roslaunch vins vins_rviz.launch
    // 开环
    source ~/vins/devel/setup.bash
    rosrun vins vins_node ~/vins/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 
    // 闭环优化
    source ~/vins/devel/setup.bash
    (optional) rosrun loop_fusion loop_fusion_node ~/vins/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 
    // 运行ROS bag
    source ~/vins/devel/setup.bash
    rosbag play /home/dyt/compare/MH_01_easy.bag

运行VINS希望输出tum型数据从而使用evo工具进行比较：

1、visualization.cpp中pubOdometry()函数

ofstream foutC(VINS_RESULT_PATH, ios::app);
        foutC.setf(ios::fixed, ios::floatfield);
        foutC.precision(0);
        foutC << header.stamp.toSec() * 1e9 << ",";
        foutC.precision(5);
        foutC << estimator.Ps[WINDOW_SIZE].x() << ","
              << estimator.Ps[WINDOW_SIZE].y() << ","
              << estimator.Ps[WINDOW_SIZE].z() << ","
              << tmp_Q.w() << ","
              << tmp_Q.x() << ","
              << tmp_Q.y() << ","
              << tmp_Q.z() << ","
              << estimator.Vs[WINDOW_SIZE].x() << ","
              << estimator.Vs[WINDOW_SIZE].y() << ","
              << estimator.Vs[WINDOW_SIZE].z() << "," << endl;
         write result to file

修改为：

ofstream foutC(VINS_RESULT_PATH, ios::app);
        foutC.setf(ios::fixed, ios::floatfield);
        foutC.precision(0);
        foutC << header.stamp.toSec() << " ";
        foutC.precision(5);
        foutC << estimator.Ps[WINDOW_SIZE].x() << " "
              << estimator.Ps[WINDOW_SIZE].y() << " "
              << estimator.Ps[WINDOW_SIZE].z() << " "
              << tmp_Q.x() << " "
              << tmp_Q.y() << " "
              << tmp_Q.z() << " "
              << tmp_Q.w() << endl;

2、pose_graph.cpp中的updatePath()函数

ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
            loop_path_file.setf(ios::fixed, ios::floatfield);
            loop_path_file.precision(0);
            loop_path_file << (*it)->time_stamp * 1e9 << ",";
            loop_path_file.precision(5);
            loop_path_file  << P.x() << ","
                  << P.y() << ","
                  << P.z() << ","
                  << Q.w() << ","
                  << Q.x() << ","
                  << Q.y() << ","
                  << Q.z() << ","
                  << endl;

修改为：

ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
            loop_path_file.setf(ios::fixed, ios::floatfield);
            loop_path_file.precision(0);
            loop_path_file << (*it)->time_stamp << " ";
            loop_path_file.precision(5);
            loop_path_file  << P.x() << " "
                            << P.y() << " "
                            << P.z() << " "
                            << Q.x() << " "
                            << Q.y() << " "
                            << Q.z() << " "
                            << Q.w() << endl;

3、pose_graph.cpp文件中addKeyFrame()函数

ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
        loop_path_file.setf(ios::fixed, ios::floatfield);
        loop_path_file.precision(0);
        loop_path_file << cur_kf->time_stamp * 1e9 << ",";
        loop_path_file.precision(5);
        loop_path_file  << P.x() << ","
              << P.y() << ","
              << P.z() << ","
              << Q.w() << ","
              << Q.x() << ","
              << Q.y() << ","
              << Q.z() << ","
              << endl;

修改为：

ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
        loop_path_file.setf(ios::fixed, ios::floatfield);
        loop_path_file.precision(0);
        loop_path_file << cur_kf->time_stamp << " ";
        loop_path_file.precision(5);
        loop_path_file  << P.x() << " "
                        << P.y() << " "
                        << P.z() << " "
                        << Q.x() << " "
                        << Q.y() << " "
                        << Q.z() << " "
                        << Q.w() << endl;

4、pose_graph_node.cpp中的main()函数
修改为txt文件：

VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.txt";

最后编译。

遇到问题：

在运行vins输出轨迹与真值比较时，会发现对于有的数据来说无法对齐。需要查看输出函数时间精度，因为evo是根据时间对齐，当时间精度较低，甚至数个数据对应同一时间时，对齐精度较差。

代码解析：

1、KITTIGPSTest.cpp （KITTI数据集 中 双目 + GPS）
读取GPS的数据，并以 相应的 topic 发出去，为后面的 global_fusion作准备，实现 数据的融合

pubGPS = n.advertise<sensor_msgs::NavSatFix>("/gps", 1000);

2、 KITTIOdomTest.cpp（KITTI数据集 中 双目）

3、rosNodeTest.cpp （EUROC数据集 单目 + IMU）

运行车载KITTI数据：

先提取IMU信息

roscore

source devel/setup.bash
roslaunch vins vins_rviz.launch

 
source  devel/setup.bash  
rosrun vins kitti /home/dyt/VINS/src/VINS-Fusion/config/kitti_raw/kitti_10_03_config1.yaml /home/dyt/2011_10_03/2011_10_03_drive_0042_sync/