1.安装
项目地址 https://github.com/uzh-rpg/rpg_trajectory_evaluation
新建ROS工作空间并安装,注意该包仅支持python2
mkdir catkin_ws
cd catkin_ws
mkdir src
cd src
git clone https://github.com/uzh-rpg/rpg_trajectory_evaluation.git
git clone https://github.com/catkin/catkin_simple.git
cd ..
catkin_make
2.运行示例程序
以euroc数据集的MH_01为例
cd src/rpg_trajectory_evaluation
python2 scripts/analyze_trajectory_single.py results/euroc_mono_stereo/laptop/vio_mono/laptop_vio_mono_MH_01
遇到问题
1.AttributeError: 'module' object has no attribute 'FullLoader'
解决
pip install -U PyYAML
2.OSError: [Errno 2] No such file or directory: 'latex'
解决
sudo apt install texlive-fonts-recommended texlive-fonts-extra
sudo apt install dvipng
第一个包有点大,大概1.4G
3.部分功能使用
1.将euroc数据集中的state_groundtruth_estimate0/data.csv
使用该包中的工具转化为其需要的格式
cd src/rpg_trajectory_evaluation/scripts/dataset_tools
python asl_groundtruth_to_pose.py euroc数据集路径/data.csv
注意其实该工具包的需要格式与TUM格式类似,区别是时间戳的单位不同,时间戳以秒为单位
# timestamp tx ty tz qx qy qz qw
1.403636580013555527e+09 1.258278699999999979e-02 -1.561510199999999963e-03 -4.015300900000000339e-02 -5.131151899999999988e-02 -8.092916900000000080e-01 8.562779200000000248e-04 5.851609599999999523e-01
使用该工具评估vinsmono,首先使用脚本将groundtruth生成对应的格式,也就是上面的这一步,然后修改vinsmono 的输出使其满足上述格式,需要修改三个地方
首先是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;
// foutC.close();
// write result to file 这里写入的是no_loop.csv
ofstream foutC(VINS_RESULT_PATH, ios::app);
foutC.setf(ios::fixed, ios::floatfield);
foutC.precision(12);
foutC << header.stamp.toSec() << " "<< 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;
foutC.close();
然后是pose_graph.cpp
的addKeyFrame
,将注释修改
if (SAVE_LOOP_PATH)
{
// 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;
// loop_path_file.close();
ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
loop_path_file.setf(ios::fixed, ios::floatfield);
loop_path_file.precision(12);
loop_path_file << cur_kf->time_stamp << " "<< P.x() << " "<< P.y() << " "<< P.z() << " " << Q.x() << " "<< Q.y() << " " << Q.z() << " "<< Q.w() << endl;
loop_path_file.close();
}
然后是pose_graph.cpp
的updatePath
,将注释修改
if (SAVE_LOOP_PATH)
{
// 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;
// loop_path_file.close();
ofstream loop_path_file(VINS_RESULT_PATH, ios::app);//
loop_path_file.setf(ios::fixed, ios::floatfield);
loop_path_file.precision(12);
loop_path_file << (*it)->time_stamp << " "<< P.x()<< " "<< P.y() << " "<< P.z() << " "<< Q.x() << " "<<Q.y() << " "<< Q.z() << " "<<Q.w() <<endl;
loop_path_file.close();
}
当然还要修改输出的两个文件格式为txt,原来是csv
在parameters.cpp
的readParameters()
函数中
VINS_RESULT_PATH = OUTPUT_PATH + "/vins_result_no_loop.txt";