SLAM中evo评估工具(用自己的数据集评估vinsFusion)

目录:

      • 配置标题文件修改
      • 源码修改
        • 第一处
        • 第二处
        • 第三处
        • 重新编译工程
      • 安装evo
        • 1、安装命令
        • 2、常用指令
      • 运行vinsFusion生成位姿估计文件
      • 使用evo评估轨迹

配置标题文件修改

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

image-20220906222444974

源码修改

由于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)函数中:

修改前:

SLAM中evo评估工具(用自己的数据集评估vinsFusion)_第1张图片

修改为:

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) 函数中

修改前:

SLAM中evo评估工具(用自己的数据集评估vinsFusion)_第2张图片

修改为:

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() 函数中

修改前:

SLAM中evo评估工具(用自己的数据集评估vinsFusion)_第3张图片

修改为:

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 可以查看对应工具的可选参数,包括:轨迹对齐、可视化坐标、尺度对齐等等

SLAM中evo评估工具(用自己的数据集评估vinsFusion)_第4张图片

运行vinsFusion生成位姿估计文件

SLAM中evo评估工具(用自己的数据集评估vinsFusion)_第5张图片

运行中画面

SLAM中evo评估工具(用自己的数据集评估vinsFusion)_第6张图片

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

生成的轨迹文件

SLAM中evo评估工具(用自己的数据集评估vinsFusion)_第7张图片

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

使用evo评估轨迹

SLAM中evo评估工具(用自己的数据集评估vinsFusion)_第8张图片

评估结果:

SLAM中evo评估工具(用自己的数据集评估vinsFusion)_第9张图片

你可能感兴趣的:(SLAM,SLAM,VINS,vinsFusion,evo评估)