SLAM中evo评估工具（用自己的数据集评估vinsFusion）

配置标题文件修改

主要根据自己的设备，修改自己传感器的ROS话题、陀螺仪的噪声，相机的内参、外参，以及输出路径，以及要开启回环检测等功能：

源码修改

由于VINS-Fusion保存的轨迹格式与evo所使用的格式不同，VINS-Fusion输出的轨迹格式不符合tum数据集的格式。因此需要对源代码就行修改，更改保存轨迹的格式。需要修改三处地方：

第一处

~/vins_fusion_ws/src/VINS-Fusion/vins_estimator/src/utility/visualization.cpp

这个文件功能是：保存不带回环检测的位姿输出并可视化

在void pubOdometry(const Estimator &estimator, const std_msgs::Header &header)函数中：

修改前：

修改为：

ofstream foutC2(VINS_RESULT_PATH, ios::app);
foutC2.precision(9);
foutC2.setf(ios::fixed,  ios::floatfield);
foutC2 << header.stamp.toSec() << " ";
foutC2.precision(6);
foutC2 	<< 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;
foutC2.close();

第二处

~/vins_fusion_ws/src/VINS-Fusion/loop_fusion/src/pose_graph.cpp

这个文件功能是：回环检测的位姿优化

在void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop) 函数中

修改前：

修改为：

ofstream loop_path_file2(VINS_RESULT_PATH, ios::app);
loop_path_file2.setf(ios::fixed,  ios::floatfield);
loop_path_file2.precision(9);
loop_path_file2 << cur_kf->time_stamp << " ";
loop_path_file2.precision(6);
loop_path_file2 << P.x() << " "
                << P.y() << " "
                << P.z() << " "
                << Q.x() << " "
                << Q.y() << " "
                << Q.z() << " "
                << Q.w() << endl;
loop_path_file2.close();

第三处

同样在pose_graph.cpp这个文件夹

~/vins_fusion_ws/src/VINS-Fusion/loop_fusion/src/pose_graph.cpp

在void PoseGraph::updatePath() 函数中

修改前：

修改为：

ofstream loop_path_file2(VINS_RESULT_PATH, ios::app);
loop_path_file2.setf(ios::fixed,  ios::floatfield);
loop_path_file2.precision(9);
loop_path_file2 << (*it)->time_stamp << " ";
loop_path_file2.precision(6);
loop_path_file2 << P.x() << " "
                << P.y() << " "
                << P.z() << " "
                << Q.x() << " "
                << Q.y() << " "
                << Q.z() << " "
                << Q.w() << endl;
loop_path_file2.close();

重新编译工程

cd ~/vinFusion_ws/
catkin_make

安装evo

1、安装命令

//安装pip
sudo apt-get update
sudo apt install python-pip

//安装EVO
pip install evo --upgrade --no-binary evo

2、常用指令

evo包括两个带尺度的评估工具和四个简单的工具：

Metrics工具

evo_ape：绝对位姿评估工具；
evo_rpe：相对位姿评估工具；

简单工具

evo_traj：轨迹可视化工具
evo_res：结果评估工具
evo_fig：绘图查看工具
evo_config：evo配置工具

通-h 可以查看对应工具的可选参数，包括：轨迹对齐、可视化坐标、尺度对齐等等

运行vinsFusion生成位姿估计文件

运行中画面

可以看到回环检测模块的功能很强大，直接把错误的地图纠正，红色的线即为回环检测模块的位姿估计

生成的轨迹文件

其中，vio.csv 是不带回环的位姿估计器生成的轨迹文件，vio_loop.csv是回环检测模块生成的轨迹文件，另外一个是真值文件

使用evo评估轨迹

评估结果：