【SLAM轨迹误差】rpg_trajectory_evaluation

下载地址：https://www.zora.uzh.ch/id/eprint/175991/1/IROS18_Zhang.pdf

Z. Zhang and D. Scaramuzza, "A Tutorial on Quantitative Trajectory

Evaluation for Visual(-Inertial) Odometry," 2018 IEEE/RSJ

International Conference on Intelligent Robots and Systems (IROS),

Madrid, Spain, 2018, pp. 7244-7251, doi: 10.1109/IROS.2018.8593941.

安装python2 的pip

wget https://bootstrap.pypa.io/pip/2.7/get-pip.py
// 或者curl https://bootstrap.pypa.io/pip/2.7/get-pip.py --output get-pip.py
sudo python2 get-pip.py
sudo apt-get install python-dev

安装成功之后可以查看

pf@pf-NUC12WSKi7:~$ pip -V
pip 20.3.4 from /home/pf/.local/lib/python2.7/site-packages/pip (python 2.7)

安装依赖

pip install numpy
pip install matplotlib
pip install colorama
pip install ruamel.yaml
sudo apt install texlive-fonts-recommended texlive-fonts-extra
sudo apt install dvipng
pip install PyYAML==5.1

下载编译如下

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 

简单评测

source devel/setup.bash
rosrun rpg_trajectory_evaluation analyze_trajectory_single.py <result_folder>

直接将 <result_folder>替换为iTest文件夹即可，文件夹中放置真值和对应的估计值如下：

├── analyze_trajectories_config
│   ├── euroc_vio_mono_stereo.yaml
│   └── euroc_vislam_mono.yaml
├── CMakeLists.txt
├── iTest
│   ├── stamped_groundtruth.txt
│   └── stamped_traj_estimate.txt

深入说明：

数据格式：

• stamped_groundtruth.txt: groundtruth poses with timestamps
• stamped_traj_estimate.txt: estimated poses with timestamps
• (optional) eval_cfg.yaml: specify evaluation parameters
• (optional) start_end_time.yaml: specify the start and end time (in seconds) for analysis.， 0 to N-1

*** stamped_groundtruth.txt😗*

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 ...

scripts/dataset_tools 文件下的脚本可以转换以上数据格式 (EuRoC style, ROS bag)

参数说明：
eval_cfg.yaml

trajectory alignment (used in absolute errors):

• align_type:
  • sim3: a similarity transformation (for vision-only monocular case)
  • se3: a rigid body transformation (for vision-only stereo case)
  • posyaw: a translation plus a rotation around gravity (for visual-inertial case)
  • none: do not align the trajectory
• align_num_frames: the number of poses (starting from the beginning) that will be used in the trajectory alignment. -1 means all poses will be used.

If this file does not exist, trajectory alignment will be done using sim3 and all the poses.

Start and end times

start_end_time.yaml can specify the following (according to groundtruth time):

• start_time_sec: only poses after this time will be used for analysis
• end_time_sec: only poses before this time will be used for analysis

If this file does not exist, analysis be done for all the poses in stamped_traj_estimate.txt.

rosrun 
rpg_trajectory_evaluation 
analyze_trajectories.py  
 euroc_vislam_mono.yaml 
 --output_dir=./results/euroc_vislam_mono 
 --results_dir=./results/euroc_vislam_mono 
 --platform laptop 
 --odometry_error_per_dataset 
 --plot_trajectories 
 --rmse_table 
 --rmse_boxplot 
 --mul_trials=10