Ubuntu 18.04 ——— VINS-Fusion运行与EVO的评测与使用

Ubuntu 18.04 ——— VINS-Fusion运行与EVO的评测与使用

  • 一、运行环境搭建
    • 1. VINS-Fusion安装
      • 1. 创建ros工作空间
      • 2. 编译VINS-Fusion
    • 2. EVO安装
    • 3. 数据集
  • 二、VINS-Fusion运行
    • 1.单相机+IMU
    • 2.Monocualr camera + IMU
    • 3.Stereo cameras + IMU
    • 4.Stereo cameras
    • 5.Stereo cameras + GPS
    • 6.VINS-Fusion代码修改
      • 1.visualization.cpp(第一处)
      • 2.pose_graph.cpp(第二处)
      • 3.globalOptNode.cpp(第三处)
    • 7. 重新编译
    • 8.运行代码获得轨迹信息
  • 三、EVO评测
  • 参考文献

一、运行环境搭建

依赖库安装与 ROS安装说明:本次运行过程是在以前安装的VINS-Mono环境基础之上,详细的依赖库安装过程见我的另一篇博客Ubuntu18.04 —— 安装环境及运行Vins_mono(2022年)

1. VINS-Fusion安装

1. 创建ros工作空间

#由于我以前装过 VINS-Mono这里我将文件名修改一下;如果你是第一次安装VINS-Fusion以前没有装过VINS-Mono可不做修改
mkdir -p ~/catkin_ws/src              
cd ~/catkin_ws/src
catkin_init_workspacek

##############修改后##################
mkdir -p ~/catkfu_ws/src              
cd ~/catkfu_ws/src
catkin_init_workspace
######################################

2. 编译VINS-Fusion

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

##############修改后##################
cd ~/catkfu_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
cd ../
catkin_make
source ~/catkfu_ws/devel/setup.bash
####################################
  1. 至此VINS-Fusion编译完成,后面需要打开四个shell窗口分别运行ros命令,为了避免每次打开新的shell都需要source,我们将其添加到bash配置文件中
sudo  vi ~/.bashrc
最后一行添加:source ~/catkin_ws/devel/setup.bash
#修改后#source ~/catkfu_ws/devel/setup.bash
保存,退出

Ubuntu 18.04 ——— VINS-Fusion运行与EVO的评测与使用_第1张图片

2. EVO安装

详细的安装过程见我的另一篇博客Ubuntu 18.04 ——— VIO算法评测工具EVO的安装与使用

3. 数据集

Euroc数据集:数据集采用MH_01_easy.bag。

二、VINS-Fusion运行

1.单相机+IMU

Ubuntu 18.04 ——— VINS-Fusion运行与EVO的评测与使用_第2张图片
以单相机+IMU为例,分别打开四个shell窗口,运行命令

#打开第一个终端,运行下面的命令
//roscore  //上面我们配置了bash文件就不需要每次打开新的shell都需要source!!!!
 
#打开第二个终端,分别运行下面的命令
//source ~/catkin_ws/devel/setup.bash
roslaunch vins vins_rviz.launch
 
#打开第三个终端,分别运行下面的命令
//source ~/catkin_ws/devel/setup.bash
rosrun vins vins_node src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml
#可选择运行,执行回环检测
(optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml 
#MH_01_easy.bag所在文件夹下在打开第四个终端,分别运行下面的命令
//source ~/catkin_ws/devel/setup.bash
rosbag play MH_01_easy.bag

Ubuntu 18.04 ——— VINS-Fusion运行与EVO的评测与使用_第3张图片

2.Monocualr camera + IMU

roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/vins-fusion/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml 
(optional) rosrun loop_fusion loop_fusion_node ~/vins-fusion/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml 
rosbag play ~/SLAM/test_data/MH_01_easy.bag

3.Stereo cameras + IMU

roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/vins-fusion/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 
(optional) rosrun loop_fusion loop_fusion_node ~/vins-fusion/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 
rosbag play ~/SLAM/test_data/MH_01_easy.bag

4.Stereo cameras

roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/vins-fusion/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml 
(optional) rosrun loop_fusion loop_fusion_node ~/vins-fusion/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml 
rosbag play ~/SLAM/test_data/MH_01_easy.bag

5.Stereo cameras + GPS

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

6.VINS-Fusion代码修改

由于VINS-Fusion保存的轨迹格式与EVO所使用的格式不同,VINS-Fusion输出的轨迹格式不符合tum数据集和euroc数据集的格式。因此需要对源代码就行修改,更改保存轨迹的格式。

1.visualization.cpp(第一处)

源代码位置:vins_estimator/src/utility/visualization.cppvoid pubOdometry函数中white result to file部分(第153行左右)
Ubuntu 18.04 ——— VINS-Fusion运行与EVO的评测与使用_第4张图片

// write result to file
	double turetime = header.stamp.toSec();
        ofstream foutC("自己的路径/src/VINS-Fusion-master/data/result.csv", ios::app);
        foutC.setf(ios::fixed, ios::floatfield);
        foutC << turetime << " ";
        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;
        foutC.close();

修改后:

Ubuntu 18.04 ——— VINS-Fusion运行与EVO的评测与使用_第5张图片

2.pose_graph.cpp(第二处)

改pose_graph.cpp中if(SAVE_LOOP_PATH)部分(第203行左右)

 if (SAVE_LOOP_PATH)
    {
        ofstream loop_path_file("自己的路径/src/VINS-Fusion-master/data/loop_result.csv", ios::app);
	double turetime = cur_kf->time_stamp;
        loop_path_file.setf(ios::fixed, ios::floatfield);
        loop_path_file << turetime << " ";
        loop_path_file  << P.x() << " "
              << P.y() << " "
              << P.z() << " "
              << Q.x() << " "
              << Q.y() << " "
              << Q.z() << " "
              << Q.w() << endl;
        loop_path_file.close();
    }

修改后:

Ubuntu 18.04 ——— VINS-Fusion运行与EVO的评测与使用_第6张图片

3.globalOptNode.cpp(第三处)

修改globalOptNode.cpp中write result to file部分:(第143行左右)

 // write result to file
    std::ofstream foutC("自己的路径/src/VINS-Fusion-master/data/vio_global.csv", 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.w() << " "
            << global_q.x() << " "
            << global_q.y() << " "
            << global_q.z() << endl;
    foutC.close();

修改后:

Ubuntu 18.04 ——— VINS-Fusion运行与EVO的评测与使用_第7张图片

7. 重新编译

修改、设置完成之后,vins_mono代码重新编译catkin_make

cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash

##############修改后##################
cd ~/catkfu_ws
catkin_make
source ~/catkfu_ws/devel/setup.bash
####################################

8.运行代码获得轨迹信息

catkin_ws文件夹下打开打开四个终端分别运行以下命令:

//没有添加环境变量的不要忘记设置环境变量:source devel/setup.bash
roslaunch vins_estimator euroc.launch //这是运行euroc数据集的三个启动节点.
roslaunch vins_estimator vins_rviz.launch //启动rviz
rosbag play MH_01_easy.bag//bag数据集

三、EVO评测

euroc数据集evo只支持tum格式的绘制,使用euroc格式转tum格式的工具把他转成tum格式;
MH_01_easy的ASL Dataset Format格式,所以当我们下载完成后,解压,进入到该文件下的state_groundtruth_estimate0文件夹中,我们会发现里面有一个data.csv文件

state_groundtruth_estimate0/文件夹下打开终端,输入以下命令:

evo_traj euroc data.csv --save_as_tum

Ubuntu 18.04 ——— VINS-Fusion运行与EVO的评测与使用_第8张图片

2。因为上面三个csv文件已经是tum格式的了,所以直接运行命令(在自己的路径/src/VINS-Fusion-master/data/文件夹下

"自己的路径/src/VINS-Fusion-master/data/***.csv"
#这里需要自己新建VINS-Fusion-master/data/文件夹,如果不新建的话有可能路径位姿可能保存不成功!!!!
#重新运行程序,待地图跑完之后,在运行 roslaunch vins_estimator euroc.launch 的terminal中,输入 “s” ,并按下回车键Enter,等待地图保存,我电脑花了20秒左右的时间。然后会出现下列信息:

pose graph path: /home/kk/happy/pose_graph_map/
pose graph saving...
save pose graph time: 22.858210 s
save pose graph finish
you can set 'load_previous_pose_graph' to 1 in the config file to reuse it next time

evo_ape tum data.tum loop_result.csv -va --plot

一定要进行轨迹对齐!!!
结果:

APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)

       max	0.415883
      mean	0.155404
    median	0.133628
       min	0.013710
      rmse	0.180707
       sse	59.399797
       std	0.092222

其中:

max:表示最大误差;
mean:平均误差;
median:误差中位数;
min:最小误差;
rmse:均方根误差;
sse:和方差、误差平方和;
std:标准差。
Ubuntu 18.04 ——— VINS-Fusion运行与EVO的评测与使用_第9张图片
Ubuntu 18.04 ——— VINS-Fusion运行与EVO的评测与使用_第10张图片
evo工具主要有如下六个常用命令:

evo_ape - 用于评估绝对位姿误差;
evo_rpe- 用于评估相对位姿误差;
evo_traj - 这个主要是用来画轨迹、输出轨迹文件、转换数据格式等功能;
evo_res- 比较来自evo_ape或evo_rpe生成的一个或多个结果文件的工具;
evo_fig - (实验)工具,用于重新打开序列化图(使用–serialize_plot保存);
evo_config - 这个主要用于evo工具全局设置和配置文件操作。

参考文献

https://www.freesion.com/article/7413495661/

你可能感兴趣的:(VIO,slam,ubuntu,自动驾驶)