1 evo
1.1 介绍
evo是一个开源工具，用于处理、评估和比较里程计算法和SLAM算法的轨迹输出。支持以下格式的轨迹：
- 'TUM'轨迹文件
- 'KITTI'位姿文件
- 'EuRoC MAV'（.csv真值和TUM轨迹文件）
- ros/ros2的bag文件,一些位姿话题（具体见evo主页）
项目地址：https://github.com/MichaelGrupp/evo.git
1.2 安装
pip install evo --upgrade --no-binary evo
1.3 使用
evo提供了两类接口，指标类和工具类
指标
- evo_ape 绝对位姿误差
- evo_rpe 相对位姿误差
工具
- evo_traj 用于分析、绘制或导出一个或多个轨迹
- evo_res 用于比较 evo_ape 或 evo_rpe 中的一个或多个结果文件
- evo_fig (实验性工具)用于重新打开序列化图(用--serialize_plot保存)
- evo_config 用于全局设置和配置文件操作
2 使用kitti数据集评估
2.1 kitti格式转rosbag
转换的目的是现在大多数算法使用的都是ros平台
2.1.1 下载kitti格式数据集
kitti中的十个序列对应的raw data关系如下：
00: 2011_10_03_drive_0027
01: 2011_10_03_drive_0042
02: 2011_10_03_drive_0034
03: 2011_09_26_drive_0067
04: 2011_09_30_drive_0016
05: 2011_09_30_drive_0018
06: 2011_09_30_drive_0020
07: 2011_09_30_drive_0027
08: 2011_09_30_drive_0028
09: 2011_09_30_drive_0033
10: 2011_09_30_drive_0034
我们下载2011_09_30_drive_0034这个数据集（05之前的z轴漂移严重，下了好多次又删了，后面才知道😅）其中包含了三个文件，下载来源参考：https://blog.csdn.net/weixin_47552638/article/details/129759500
2.1.2 安装工具
安装下面两个包：
- 转换工具
pip install kitti2bag - 进度条显示工具
pip install tqdm
注：可能由于kitti2bag最初是由python2开发的，我用pip3安装后一直有问题，好久没解决，请确保终端运行的是python2，
也可以参考一下这个，更新一下pip：https://zhuanlan.zhihu.com/p/164889106
2.1.4 改写源码输出位姿
- 适配雷达
因为liosam 要求输入的点云每个点都有ring 信息和相对时间time信息，目前的雷达驱动基本具备这些信息，但是早期的KITTI数据集不具备，所以代码要自己计算一下 ring和time
待补充...
- 输出TUM格式的位姿
待补充...
2.1.3 kitti转rosbag
lio-sam作者在源码LIO-SAM/config/doc/kitti2bag路径下给出了转换方法kitti2bag.py，我改写部分在下面：
unzip 2011_09_30_drive_0034_sync.zip
unzip 2011_09_30_drive_0034_extract.zip
unzip 2011_09_30_calib.zip
python kitti2bag.py -t 2011_09_30 -r 0034 raw_synced .
其中最后一条指令是在解压后的2011_09_30文件夹同一级执行的，执行以上命令后将会输出转换后的rosbag文件kitti_2011_09_30_drive_0034_synced.bag
开启lio-sam并播放同步后的包，得到以下点云图：
2.1.5 使用evo评估
准备评估所需文件：
- 数据集的真值文件（ground truth）
我从 http://t.csdnimg.cn/FhANk 处获取到了kitti全部11个序列的真值文件（已转成tum格式） - 运行lio-sam算法后获取的tum格式位姿文件
3 评估自己数据集
tum格式
每行都有 8 个条目，包含时间戳（以秒为单位）、位置和方向（方向用四元数表示），每个值之间用空格分隔，格式如下：
timestamp x y z q_x q_y q_z q_w
在你想要输出轨迹的代码处加入以下语句（以A-LOAM在laserOdometry.cpp为例）：
//定义文件输出流对象
static std::ofstream tumPose;
tumPose.open("/home/dianhua/loam_ws/src/A-LOAM/data/tumPose.txt", std::ios::app);
tumPose << ros::Time().fromSec(timeSurfPointsLessFlat) << " "
<< t_w_curr.x() << " "
<< t_w_curr.y() << " "
<< t_w_curr.z() << " "
<< q_w_curr.x() << " "
<< q_w_curr.y() << " "
<< q_w_curr.z() << " "
<< q_w_curr.w() << std::endl;
tumPose.close();
注：输出位姿文件路径若用相对路径，输出文件将会在当前用户下的.ros文件夹中，不是很方便，所以这里使用了绝对路径
kitti格式
kitti格式实际上不是真正的轨迹格式，因为它没有时间戳 - 它只包含文本文件中的位姿。这意味着若想要将此格式的两个文件与度量进行比较时必须小心，因为位姿的数量必须完全相同。
文件的每一行都包含 4x4 齐次位姿矩阵（SE(3) 矩阵）的前 3 行，该矩阵被展平为一行，每个值由空格分隔。例如，这个位姿矩阵：
a b c d
e f g h
i j k l
0 0 0 1
将在文件中以如下格式显示：
a b c d e f g h i j k l
示例程序：
Eigen::Quaterniond quaternion(q_w_curr.w(),q_w_curr.x(),q_w_curr.y(),q_w_curr.z());
Eigen::Matrix3d mat = quaternion.toRotationMatrix();
tumPose.open("/home/dianhua/loam_ws/src/A-LOAM/data/mappingPose_kitti.txt", std::ios::app);
tumPose
<< mat(0,0) << " "<< mat(0,1) << " "<< mat(0,2) << " "<< t_w_curr.x() << " "
<< mat(1,0) << " "<< mat(1,1) << " "<< mat(1,2) << " "<< t_w_curr.y() << " "
<< mat(2,0) << " "<< mat(2,1) << " "<< mat(2,2) << " "<< t_w_curr.z() << std::endl;
tumPose.close();
