SLAM数据集（tum/kitti）轨迹对齐与结果评估

SLAM数据集的轨迹对齐与结果评估

SLAM是一个系统工程，最终的结果是一个实时的地图，因此我们需要对轨迹进行对齐和比对。在深蓝SLAM课程中，提供了ICP对齐的思路，来估计实际与模型的结果偏差。一些常见的数据集，如KITTI，TUM等，也提供了对齐工具和思路。

(1) tum数据集

1. 单目

建立评估文件夹，将得到的KeyFrameTrajectory.txt与数据集里面的groundtruth.txt文件拷贝至评估文件夹下
下载evaluate_ate.py，evaluate_rpe.py

官网介绍这两个文件区别和联系：

After estimating the camera trajectory of the Kinect and saving it to a file, we need to evaluate the error in the estimated trajectory by comparing it with the ground-truth. There are different error metrics. Two prominent methods is the absolute trajectory error (ATE) and the relative pose error (RPE). The ATE is well-suited for measuring the performance of visual SLAM systems. In contrast, the RPE is well-suited for measuring the drift of a visual odometry system, for example the drift per second.

即，ate适用于检测整体的表现，rpe适用于展示不同计量单位下的漂移情况
官网上有这两个文件的详细调用方式，这里提供一种最常用的：
```
python2 evaluate_ate.py --save alignedTrajectory_ate.txt --plot ate.png groundtruth.txt KeyFrameTrajectory.txt
python2 evaluate_rpe.py --fixed_delta --delta_unit s --save alignedTrajectory_rpe.txt --plot rpe.png groundtruth.txt KeyFrameTrajectory.txt
```
（delta_unit替换：“s”: seconds；“m”: meters；“rad”: radians；“deg”: degrees；“f”: frames）
没错，上述算法在python2环境中使用
rgbd_dataset_freiburg1_desk序列下的ate图、rpe图（单位：秒、米、帧）如下：

2. rgbd

基本类似，但是使用了CameraTrajectory.txt文件，这个是更加准确，输入：

python2 evaluate_ate.py --save alignedTrajectory_ate.txt --plot ate.png groundtruth.txt CameraTrajectory.txt
python2 evaluate_rpe.py --fixed_delta --delta_unit s --save alignedTrajectory_rpe.txt --plot rpe.png groundtruth.txt CameraTrajectory.txt

结果如下，可以看出准确率更高，更细密：

3. 其他：

此外，还有一个能够生成点云（generate_pointcloud.py）；
还有一个将点云载入ROS包的python2脚本（add_pointclouds_to_bagfile.py），供ros开发者使用；

(2) KITTI数据集

KITTI数据集比较麻烦，官网上并没有给出一个比较便捷的评测工具（尤其是对于ORBSLAM2输出格式），官网提供的一个评测工具是pykitti，详细的过程参考：https://zhuanlan.zhihu.com/p/76155544

但是，这个需要下全数据集，尤其是80G的velodyne，本人实在没有这个能耐，所以不得已放弃了
正好看到泡泡机器人上推荐了evo，所以就开始借助evo这个评测工具进行评估。

(3) evo工具

安装：github：https://github.com/MichaelGrupp/evo

在目录界面下
```
pip install --editable . --upgrade --no-binary evo
```
跑模型，evo提供一个kitti的模型，使用：
```
cd test/data
evo_traj kitti KITTI_00_ORB.txt KITTI_00_SPTAM.txt --ref=KITTI_00_gt.txt -p --plot_mode=xz
```
如图所示，看到显示了三种图，非常的棒：

注：以上步骤follow的evo的readme文档，如果有问题可以看一下这个

OK，我们从这个demo为入手，详细分析一下如何评估其他KITTI数据集序列

https://github.com/MichaelGrupp/evo/wiki/Formats

这是一个evo的wiki，上面指出kitti数据集真实轨迹是一个4x4的矩阵，这和我们在之前tum上提供的真实轨迹不同，与orbslam2产生的轨迹不同。

事实上，这两者的格式是统一的，都是timestamp x y z q_x q_y q_z q_w格式，evo中称其为tum格式，它表示8个数据；pose位姿更与evo在test过程中使用KITTI_00_gt.txt文件格式不同，后者被称为kitti格式，用12位数据表示；它们之间的转换见图（该图源自evo的wiki）：

注：它们可以使用类似于evo_traj tum xxx.txt --save_as_kitti进行转换

1. 使用双目数据

使用双目时较为简单，只需要新建一个KITTI文件夹，将CameraTrajectory.txt（orbslam2得到的）、02.txt拷贝至该文件夹下，然后输入：

evo_traj kitti CameraTrajectory.txt  --ref=02.txt -p --plot_mode=xz

如果获得ate和rpe，则在KITTI下新建results，输入：

evo_ape kitti 02.txt CameraTrajectory.txt -r full -va --plot --plot_mode xz --save_results results/ape.zip
evo_rpe kitti 02.txt CameraTrajectory.txt -r full -va --plot --plot_mode xz --save_results results/rpe.zip

结果（第一行ape，第二行rpe）：

2. 使用单目数据

单目orbslam仅输出KeyFrameTrajectory.txt，观察这个文件发现它是一个tum格式的文件。由于kitti真实轨迹02.txt格式特殊（kitti），查阅上述的转化变发现它无法直接转换为tum，因此evo提供了一个小工具，这个工具在evo的contrib文件夹下被使用：

新建一个result文件夹，然后终端输入：

mkdir result
cd result

把KITTI数据集下的（pose）02.txt和times.txt文件拷贝到该目录，运行如下命令：

cd ..
python kitti_poses_and_timestamps_to_trajectory.py ./result/02.txt ./result/times.txt ./result/KITTI_02_gt.txt

在result文件夹下得到新的真实轨迹KITTI_02_gt.txt。
然后新建一个KITTI文件夹，将KeyFrameTrajectory.txt（orbslam2得到的）、time.txt、KITTI_02_gt.txt拷贝至该文件夹下，输入：

evo_traj tum KeyFrameTrajectory.txt --ref=KITTI_02_gt.txt -p --plot_mode=xz --correct_scale --align

这里采用了–correct_scale --align 后缀辅助，表示手动对齐。

ape和rpe：

evo_ape tum KITTI_02_gt.txt KeyFrameTrajectory.txt -r full -va --plot --plot_mode xz --correct_scale --align --save_results results/ape.zip
evo_rpe tum KITTI_02_gt.txt KeyFrameTrajectory.txt -r full -va --plot --plot_mode xz --correct_scale --align --save_results results/rpe.zip

evo其他的功能还包括evo_res，可以对evo_ape/evo_rpe中将结果保存为.zip文件对不同的结果进行比较。

可以看出，综合而言，evo工具其实比数据集自带的几个工具使用更加便捷，而且数据更加漂亮，也更加推荐。