使用 evo 工具和 EuRoc 数据集评估 VINS 项目

2019-07-25 约 1000 字预计阅读 2 分钟 - 次阅读

安装evo工具

https://github.com/MichaelGrupp/evo

安装evo工具

1
2
3
sudo apt install python-pip
pip install evo --upgrade --no-binary evo
pip install numpy --upgrade --user

如果出现以下问题

1
2
Could not install packages due to an EnvironmentError: [Errno 13] Permission denied: '/usr/lib/python2.7/dist-packages/dateutil/relativedelta.pyc'
Consider using the `--user` option or check the permissions.

那么运行下面的命令安装

1
2
pip install evo --upgrade --no-binary evo --user
pip install numpy --upgrade --user

evo工具安装和使用可参考https://blog.csdn.net/A_L_A_N/article/details/88708979

VINS MONO

修改visualization.cpp中 void pubOdometry函数中 write result to file部分

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
void pubOdometry(const Estimator &estimator, const std_msgs::Header &header)

     // write result to file 部分修改为以下代码
    //PATH 是保存输出文件的绝对路径，例如 /home/yourusername/vins_mono/ 要保证路径存在
    double turetime = header.stamp.toSec();
        ofstream foutC("/PATH/result.csv", ios::app);
        foutC.setf(ios::fixed, ios::floatfield);
        foutC << turetime << " "
              << 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;
        foutC.close();

修改pose_graph.cpp中if (SAVE_LOOP_PATH)部分

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
 if (SAVE_LOOP_PATH)
    {
     //PATH 是保存输出文件的绝对路径，例如 /home/yourusername/vins_mono/ 要保证路径存在
     ofstream loop_path_file("/PATH/loop_result.csv", ios::app);
        double turetime = cur_kf->time_stamp;
        loop_path_file.setf(ios::fixed, ios::floatfield);
        loop_path_file  << turetime << " "
              << P.x() << " "
              << P.y() << " "
              << P.z() << " "
              << Q.x() << " "
              << Q.y() << " "
              << Q.z() << " "
              << Q.w() << endl;           
        loop_path_file.close();
    }

使用catkin_make重新编译

运行

1
2
3
4
5
6
source ~/catkin_ws/devel/setup.bash
#source /你的工作空间/devel/setup.bash

roslaunch vins_estimator euroc.launch
roslaunch vins_estimator vins_rviz.launch
rosbag play ./xxx.bag #你的EuRoc数据集的位置

使用evo工具进行评估
画轨迹图，例如MH_01_easy_result.csv是你的输出文件。
1
evo_traj tum ./MH_01_easy_result.csv -p --plot_mode=xyz
确定轨迹误差，data.csv是数据集中的groundtruth真值（下载zip格式的数据集，解压后能看到） --save_results result_vins.zip参数是保存结果到文件，可以不需要。
1
evo_ape euroc ./data.csv ./MH_01_easy_result.csv -va --plot --save_results result_vins.zip

VINS FUSION

修改代码，与VINS_Mono相同
重新编译

运行

单目+IMU

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
roslaunch vins vins_rviz.launch

#换成你自己的配置文件的路径
rosrun vins vins_node ~/catkin_ws/src/VINS-FUSION/config/euroc/euroc_mono_config.yaml

#换成你自己的配置文件的路径
(optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/config/euroc/euroc_mono_config.yaml

#输入包文件的路径，或者直接cd到包文件目录
   rosbag play ./MH_01_easy.bag

双目

1
2
3
4
5
6
7
roslaunch vins vins_rviz.launch

#换成你自己的配置文件的路径
rosrun vins vins_node ~/catkin_ws/src/VINS-FUSION/config/euroc/euroc_stereo_config.yaml

#输入包文件的路径，或者直接cd到包文件目录
rosbag play ./MH_01_easy.bag

双目+IMU

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
roslaunch vins vins_rviz.launch

#换成你自己的配置文件的路径
rosrun vins vins_node ~/catkin_ws/src/VINS-FUSION/config/euroc/euroc_stereo_imu_config.yaml

#换成你自己的配置文件的路径
(optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-FUSION/config/euroc/euroc_stereo_imu_config.yaml

#输入包文件的路径，或者直接cd到包文件目录
rosbag play ./MH_01_easy.bag

以上命令中的(optional)部分是开启闭环优化，需要在运行一段时间后执行。运行后会实时看到优化的效果。

评估过程和上面的VINS_Mono相同。