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

下面咱们来对vins-mono地图进行简单测试。

地图保存与加载

vins-mono官方源码地址：https://github.com/HKUST-Aerial-Robotics/VINS-Mono

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

步骤如下：

先跑起来

1.先按照官网配置好

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

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 

这里的YOUR_PATH_TO_DATASET要修改成存放数据集的路径。
此时，如果你看到系统跑起来了，完成了第一步。

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

roslaunch benchmark_publisher publish.launch sequence_name:=MH_01_easy

主要要修改根据使用的数据集，修改数据集名称，这里我们使用的是MH_01。
然后，你就会看到两条轨迹，红色的轨迹是ground truth，绿色的轨迹是vins-mono跑出来的。

修改地图保存的路径

因为我们现在跑的是euroc数据集，所以我们要修改的地方有两处。

1. euroc_config.yaml中的pose_graph_save_path项

pose_graph_save_path: "/home/kk/自己的路径/"

2. euroc_config.yaml中的output_path项

output_path: "/home/kk/自己的路径/"

上述""里填写自己的路径，先创建好该路径，注意最后面有个/，代表着是一个文件夹。

修改完之后，记得在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. euroc_config.yaml的load_previous_pose_graph，置1
2. euroc_config.yaml的fast_relocalization，置1

第一项的意思是使能重新加载地图功能，第二项是快速重定位。（你可以先不改第二项试试看，然后再改，看看有什么不同），修改完之后重新编译catkin_make。

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

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

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

evo测评

evo工具

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

常见的参数如下：

1. evo_config:用于保存配置文件，把自己常用的参数保存为.json文件，避免每次输入。
2. evo_traj:用于绘制轨迹，支持的格式有kitti，eurco，tum 数据集等，也可以用于验证数据是否有效，导出为其他格式等。
3. evo_res:可用于比较指标中的多个结果文件（打印消息和统计消息，绘制结果，将统计信息保存在表内）
4. evo_ape :计算绝对位姿误差
5. evo_rpe:计算相对位姿误差

修改数据格式

这边我只测试了evo_traj，也就是画出它的轨迹。问题来了，vins-mono保存的轨迹没法直接用，因为它既不符合tum数据集的格式，又不符合euroc数据集的格式。那怎么办，改呗。

修改以下文件：

1. 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