依赖库安装与 ROS安装说明:本次运行过程是在以前安装的VINS-Mono环境基础之上,详细的依赖库安装过程见我的另一篇博客Ubuntu18.04 —— 安装环境及运行Vins_mono(2022年)
#由于我以前装过 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
######################################
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
####################################
sudo vi ~/.bashrc
最后一行添加:source ~/catkin_ws/devel/setup.bash
#修改后#source ~/catkfu_ws/devel/setup.bash
保存,退出
详细的安装过程见我的另一篇博客Ubuntu 18.04 ——— VIO算法评测工具EVO的安装与使用
Euroc数据集:数据集采用MH_01_easy.bag。
#打开第一个终端,运行下面的命令
//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
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
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
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
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
由于VINS-Fusion保存的轨迹格式与EVO所使用的格式不同,VINS-Fusion输出的轨迹格式不符合tum数据集和euroc数据集的格式。因此需要对源代码就行修改,更改保存轨迹的格式。
源代码位置: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();
修改后:
修改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();
}
修改后:
修改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();
修改后:
修改、设置完成之后,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
####################################
在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数据集
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/