Ubuntu 18.04 ——— VINS-Fusion运行与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
####################################

3. 至此VINS-Fusion编译完成，后面需要打开四个shell窗口分别运行ros命令，为了避免每次打开新的shell都需要source，我们将其添加到bash配置文件中

sudo  vi ~/.bashrc
最后一行添加：source ~/catkin_ws/devel/setup.bash
#修改后#source ~/catkfu_ws/devel/setup.bash
保存，退出

2. EVO安装

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

3. 数据集

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

二、VINS-Fusion运行

1.单相机+IMU

以单相机+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

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.cpp中void pubOdometry函数中white result to file部分（第153行左右）

// 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();

修改后：

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();
    }

修改后：

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();

修改后：

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

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：标准差。


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/