vins-mono保存、重载地图、evo工具测试

1940阅读 0评论2020-04-22 iibull
分类:其他平台

https://blog.csdn.net/Hanghang_/article/details/104535370


vins-mono保存、重载地图、evo工具测试
地图保存与加载
先跑起来
修改地图保存的路径
保存地图
重载地图
evo测评
evo工具
修改数据格式
使用evo绘制轨迹
与双目ORB_SLAM2进行对比


下面咱们来对vins-mono地图进行简单测试。
地图保存与加载
vins-mono官方源码地址:


首先你得把vins-mono跑通,才有后续。按照官网的步骤执行到第三步


步骤如下:


先跑起来
1.先按照官网配置好


cd ~/catkin_ws/src
git clone .git
cd ../
catkin_make
source ~/catkin_ws/devel/setup.bash
1
2
3
4
5
2.打开三个terminal,记得都要先执行 source ~/catkin_ws/devel/setup.bash
然后执行下列:


 roslaunch vins_estimator euroc.launch 
 roslaunch vins_estimator vins_rviz.launch
 rosbag play YOUR_PATH_TO_DATASET/MH_01_easy.bag 
1
2
3
这里的YOUR_PATH_TO_DATASET要修改成存放数据集的路径。
此时,如果你看到系统跑起来了,完成了第一步。




如果想看到轨迹的同时,看到数据集的ground truth,我们可以再打开一个terminal,执行:


roslaunch benchmark_publisher publish.launch sequence_name:=MH_01_easy
1
主要要修改根据使用的数据集,修改数据集名称,这里我们使用的是MH_01。
然后,你就会看到两条轨迹,红色的轨迹是ground truth,绿色的轨迹是vins-mono跑出来的。




修改地图保存的路径
因为我们现在跑的是euroc数据集,所以我们要修改的地方有两处。


euroc_config.yaml中的pose_graph_save_path项
pose_graph_save_path: "/home/kk/自己的路径/"
1
euroc_config.yaml中的output_path项
output_path: "/home/kk/自己的路径/"
1
上述""里填写自己的路径,先创建好该路径,注意最后面有个/,代表着是一个文件夹。


修改完之后,记得在ros工作空间中重新编译一下catkin_make。


保存地图
重新运行程序,待地图跑完之后,在运行 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
1
2
3
4
5
恭喜你,保存成功。


重载地图
首先要修改两处地方:


euroc_config.yaml的load_previous_pose_graph,置1
euroc_config.yaml的fast_relocalization,置1
第一项的意思是使能重新加载地图功能,第二项是快速重定位。(你可以先不改第二项试试看,然后再改,看看有什么不同),修改完之后重新编译catkin_make。


重新运行程序,会发现先加载了地图,颜色是黄色的。






下面是我的实验结果:
左边的是仅仅加载了地图,右边是使用了快速重定位,截图的角度有点不同,分辨不出哪个更好。这个留给读者去实验了。


原本我以为再保存多几次地图,效果会更好……这想法太天真了。事实证明轨迹只会和地图重合,而不会接近ground truth,想想确实也是,两者无关。


evo测评
evo工具
evo工具用过没?一个评测SLAM的工具,可以比较不同SLAM的算法精度,轨迹等等。evo支持好几种数据集的格式,tum、euroc等等。怎么装?github找去。


常见的参数如下:


evo_config:用于保存配置文件,把自己常用的参数保存为.json文件,避免每次输入。
evo_traj:用于绘制轨迹,支持的格式有kitti,eurco,tum 数据集等,也可以用于验证数据是否有效,导出为其他格式等。
evo_res:可用于比较指标中的多个结果文件(打印消息和统计消息,绘制结果,将统计信息保存在表内)
evo_ape :计算绝对位姿误差
evo_rpe:计算相对位姿误差
修改数据格式
这边我只测试了evo_traj,也就是画出它的轨迹。问题来了,vins-mono保存的轨迹没法直接用,因为它既不符合tum数据集的格式,又不符合euroc数据集的格式。那怎么办,改呗。


修改以下文件:


visualization.cpp中pubOdometry()函数
  // write result to file
        ofstream foutC(VINS_RESULT_PATH, ios::app);
        foutC.setf(ios::fixed, ios::floatfield);
        foutC.precision(0);
        foutC << header.stamp.toSec() * 1e9 << ",";
        foutC.precision(5);
        foutC << estimator.Ps[WINDOW_SIZE].x() << ","
              << estimator.Ps[WINDOW_SIZE].y() << ","
              << estimator.Ps[WINDOW_SIZE].z() << ","
              << tmp_Q.w() << ","
              << tmp_Q.x() << ","
              << tmp_Q.y() << ","
              << tmp_Q.z() << ","
              << estimator.Vs[WINDOW_SIZE].x() << ","
              << estimator.Vs[WINDOW_SIZE].y() << ","
              << estimator.Vs[WINDOW_SIZE].z() << "," << endl;
         write result to file
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
改成:


  ofstream foutC(VINS_RESULT_PATH, ios::app);
        foutC.setf(ios::fixed, ios::floatfield);
        foutC.precision(0);
        foutC << header.stamp.toSec() << " ";
        foutC.precision(5);
        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;
1
2
3
4
5
6
7
8
9
10
11
12
pose_graph.cpp中的updatePath()函数
            ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
            loop_path_file.setf(ios::fixed, ios::floatfield);
            loop_path_file.precision(0);
            loop_path_file << (*it)->time_stamp * 1e9 << ",";
            loop_path_file.precision(5);
            loop_path_file  << P.x() << ","
                  << P.y() << ","
                  << P.z() << ","
                  << Q.w() << ","
                  << Q.x() << ","
                  << Q.y() << ","
                  << Q.z() << ","
                  << endl;
1
2
3
4
5
6
7
8
9
10
11
12
13
改成:


  ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
            loop_path_file.setf(ios::fixed, ios::floatfield);
            loop_path_file.precision(0);
            loop_path_file << (*it)->time_stamp << " ";
            loop_path_file.precision(5);
            loop_path_file  << P.x() << " "
                            << P.y() << " "
                            << P.z() << " "
                            << Q.x() << " "
                            << Q.y() << " "
                            << Q.z() << " "
                            << Q.w() << endl;
1
2
3
4
5
6
7
8
9
10
11
12
pose_graph.cpp文件中addKeyFrame()函数
        ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
        loop_path_file.setf(ios::fixed, ios::floatfield);
        loop_path_file.precision(0);
        loop_path_file << cur_kf->time_stamp * 1e9 << ",";
        loop_path_file.precision(5);
        loop_path_file  << P.x() << ","
              << P.y() << ","
              << P.z() << ","
              << Q.w() << ","
              << Q.x() << ","
              << Q.y() << ","
              << Q.z() << ","
              << endl;
1
2
3
4
5
6
7
8
9
10
11
12
13
改成:


 ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
        loop_path_file.setf(ios::fixed, ios::floatfield);
        loop_path_file.precision(0);
        loop_path_file << cur_kf->time_stamp << " ";
        loop_path_file.precision(5);
        loop_path_file  << P.x() << " "
                        << P.y() << " "
                        << P.z() << " "
                        << Q.x() << " "
                        << Q.y() << " "
                        << Q.z() << " "
                        << Q.w() << endl;
1
2
3
4
5
6
7
8
9
10
11
12
pose_graph_node.cpp中的main()函数
原本是csv文件,改成txt。
        VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.txt";
1
好了,修改完成,重新编译catkin_make。


使用evo绘制轨迹
重新运行程序,会发现在刚刚保存地图的路径,生成了一个文件:


vins_result_loop.txt
1
经过我们上面的修改,该文件是符合tum格式的,虽然我们使用的是euroc数据集,但evo只支持tum格式的绘制,它提供了euroc格式转tum格式的工具。


首先我们打开数据集的state_groundtruth_estimate0/文件夹,会发现有一个文件:
data.csv。这是一个euroc格式的文件,我们首先要把他转成tum格式。输入以下命令:


evo_traj euroc data.csv --save_as_tum
1
生成data.tum


好了,接下来就可以绘制轨迹了!在当前目录下输入:


evo_traj tum ../../../../YOUR_MAP_PATH/vins_result_loop.txt  --ref=data.tum -p --plot_mode=xyz --align --correct_scale
1
记得把YOUR_MAP_PATH修改成你的地图路径。


真是beautiful。


其中虚线代表ground truth,蓝线代表vins的轨迹。


与双目ORB_SLAM2进行对比
ORB_SLAM2的轨迹在每次运行都会生成姿态轨迹,貌似不用修改文件,小伙伴们自己去试吧,这里只给你看看结果。


命令:(自行修改路径)


evo_traj tum VINS_MAP_PATH/vins_result_loop.txt ORB_SLAM2_PATH/ORB_SLAM2-master/CameraTrajectory.txt --ref=data.tum -p --plot_mode=xyz --align --correct_scale
1
结果:










详细的对比可以点击此处。


参考:
https://blog.csdn.net/learning_tortosie/article/details/83182258
https://blog.csdn.net/houlianfeng/article/details/79626657#commentBox
%E5%BC%80%E6%BA%90SLAM%E6%96%B9%E6%A1%88%E8%AF%84%E4%BB%B7%E4%B8%8E%E6%AF%94%E8%BE%83_3442830_csdn.html
上一篇:搭建 ROS VINS 测试环境.
下一篇:ubuntu 跑 realsense 环境搭建方法