Vins-fusion gps融合 KITTY数据集测试

下载kitti数据集

下载kitti数据集和真值poses的00.txt以及sequences文件00序列的times.txt，（全网找了好久，最后不得已翻墙从官网down下来的）

代码修改，保存输出数据

先指定输出路径:打开vins-fusion/config/kitti_raw/kitti_10_03_config.yaml或者kitti_raw/kitti_09_30_config.yaml(根据数据集选择具体参数文件),第十行
output_path: “/home/tony-ws1/output/”
这里指明自己需要保存到的地址.

第二步接下来是修改代码使其输出轨迹文件,原本的vins_fusion是无轨迹文件输出的.
globalOptNode.cpp中vio_callback函数

    std::ofstream foutC("/home/tony-ws1/output/vio_global.csv", ios::app);//这里还是文件输出位置
   foutC.setf(ios::fixed, ios::floatfield);
    foutC.precision(0);
    foutC << pose_msg->header.stamp.toSec() * 1e9 << ",";
    foutC.precision(5);
    foutC << global_t.x() << ","
            << global_t.y() << ","
            << global_t.z() << ","
            << global_q.w() << ","
            << global_q.x() << ","
            << global_q.y() << ","
            << global_q.z() << endl;
    foutC.close(); 

改成如下:

    std::ofstream foutC("/home/tony-ws1/output/vio_global.txt", ios::app);//这里还是文件输出位置,最好和前面位置一样
   foutC.setf(ios::fixed, ios::floatfield);
    foutC.precision(0);
    foutC << pose_msg->header.stamp.toSec() << " ";
    foutC.precision(5);
    foutC   << global_t.x() << " "
            << global_t.y() << " "
            << global_t.z() << " "
            << global_q.x() << " "
            << global_q.y() << " "
            << global_q.z() << " "
            << global_q.w() << endl;
    foutC.close(); 

将输出数据格式改成tum文件格式的

运行global_fusion节点

按照github上面来,三个终端,分别是下面三个指令(可能要加上source ~/(你的目录)/devel/setup.bash).

roslaunch vins vins_rviz.launch
rosrun vins kitti_gps_test ~/catkin_ws/src/VINS-Fusion/config/kitti_raw/kitti_10_03_config.yaml YOUR_DATASET_FOLDER/2011_10_03_drive_0027_sync/ 
rosrun global_fusion global_fusion_node

可看到如下显示结果：

EVO显示及评测

指定的文件夹下找到vio_global文件.使用evo分析结果,这里是tum数据格式,故使用

evo_traj tum vio_global.txt -p

也可同时显示出来：

接下来是对比真值,(kitti的真值来自与卫星定位,这里视觉融合卫星定位的结果还与之前kitti的真值对比是不是不合适?，这里暂且参照对比吧)

因为真值是12列的kitti轨迹格式,所以要将真值转换成8列格式的tum轨迹格式
evo里给出了可转换的代码

python kitti_poses_and_timestamps_to_trajectory.py ./result/00.txt ./result/times.txt ./result/KITTI_00_gt.txt

通过该指令完成转换,详细见原博文,转换得到tum格式的真值后,我们可以进行评定,将我们得到的vio_global.txt 与这里的真值进行对比

evo_ape tum vio_global.csv   KITTI_00_gt.txt -a  -p

为了对比，把vio.csv的也与真值做个对比

evo_ape tum vio.csv   KITTI_00_gt.txt -a  -p