主要根据自己的设备,修改自己传感器的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
//安装pip
sudo apt-get update
sudo apt install python-pip
//安装EVO
pip install evo --upgrade --no-binary evo
evo包括两个带尺度的评估工具和四个简单的工具:
Metrics工具
evo_ape:绝对位姿评估工具;
evo_rpe:相对位姿评估工具;
简单工具
evo_traj:轨迹可视化工具
evo_res:结果评估工具
evo_fig:绘图查看工具
evo_config:evo配置工具
通-h
可以查看对应工具的可选参数,包括:轨迹对齐、可视化坐标、尺度对齐等等
运行中画面
可以看到回环检测模块的功能很强大,直接把错误的地图纠正,红色的线即为回环检测模块的位姿估计
生成的轨迹文件
其中,vio.csv 是不带回环的位姿估计器生成的轨迹文件,vio_loop.csv是回环检测模块生成的轨迹文件,另外一个是真值文件
评估结果: