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并播放同步后的包,得到以下点云图:
![](https://img2024.cnblogs.com/blog/1437041/202403/1437041-20240326100648244-1898717707.png)
2.1.5 使用evo评估
准备评估所需文件:
- 数据集的真值文件(ground truth)
我从 http://t.csdnimg.cn/FhANk 处获取到了kitti全部11个序列的真值文件(已转成tum格式) - 运行lio-sam算法后获取的tum格式位姿文件
![](https://img2024.cnblogs.com/blog/1437041/202403/1437041-20240326144952530-2128203808.png)
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();