安装使用rpg_trajectory_evaluation评估工具

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";