使用evo进行SLAM轨迹评估：从GPS到TUM格式的完整实践指南

1. 为什么需要evo进行SLAM轨迹评估做SLAM的朋友都知道系统跑出来的轨迹准不准直接决定了整个项目的成败。但问题来了——我们怎么知道轨迹到底准不准这时候就需要专业的评估工具了。我最早做无人机定位项目时经常遇到ORB-SLAM输出的轨迹和GPS轨迹对不上的情况那时候全靠肉眼观察结果被导师骂得狗血淋头。直到发现了evo这个神器才算真正解决了轨迹评估的痛点。evo最大的优势在于它能处理多种格式的轨迹数据特别是对TUM格式的支持非常友好。TUM格式是慕尼黑工业大学提出的标准格式包含时间戳和位姿信息特别适合SLAM系统的评估。在实际项目中我们经常需要把GPS的WGS84坐标转换成TUM格式的笛卡尔坐标这个过程看似简单但藏着不少坑。比如地球曲率带来的误差、坐标系的转换关系等稍不注意就会导致评估结果完全失真。2. 环境准备与evo安装2.1 安装evo的正确姿势很多人第一次装evo时会遇到依赖冲突的问题这里分享一个我验证过最稳定的安装方案pip install evo --upgrade --no-binary evo这个命令的关键在于--no-binary参数它能避免预编译包带来的兼容性问题。我测试过Ubuntu 18.04/20.04和ROS Noetic/Melodic的组合这个方法都能完美运行。如果遇到numpy版本冲突可以先创建一个干净的Python虚拟环境python -m venv evo_env source evo_env/bin/activate pip install numpy1.19.5 # 这个版本与evo兼容性最好2.2 准备测试数据评估需要至少两组数据参考轨迹通常来自GPS或真值系统和估计轨迹SLAM系统输出。以ORB-SLAM3为例运行后会生成FrameTrajectory_TUM_Format.txt文件这个就是我们需要评估的轨迹。文件内容长这样1623456789.123456 1.234 2.345 3.456 0.1 0.2 0.3 0.4 1623456790.123456 1.235 2.346 3.457 0.1 0.2 0.3 0.4每行包含时间戳秒、位置xyz和四元数姿态xyzw。注意四元数的顺序很重要搞错了会导致整个姿态评估出错。3. GPS坐标转换实战3.1 WGS84转笛卡尔坐标GPS数据通常是WGS84坐标系下的经纬度高程要转换成TUM格式需要经过以下步骤将经纬度从度转为弧度计算卯酉圈曲率半径应用大地坐标系转换公式这里给出我优化过的Python代码解决了原代码中的几个数值稳定性问题import math import numpy as np def gps_to_cartesian(lat, lon, alt, lat0None, lon0None, alt0None): 优化后的坐标转换函数支持相对坐标转换 :param lat: 当前点纬度(度) :param lon: 当前点经度(度) :param alt: 当前点高程(m) :param lat0: 参考点纬度(度)None时使用绝对坐标 :param lon0: 参考点经度(度) :param alt0: 参考点高程(m) :return: (x,y,z) 笛卡尔坐标(m) a 6378137.0 # WGS84长轴 f 1/298.257223563 # 扁率 lat, lon math.radians(lat), math.radians(lon) e2 2*f - f*f # 第一偏心率平方 if lat0 is not None: # 相对坐标模式 lat0, lon0 math.radians(lat0), math.radians(lon0) N0 a / math.sqrt(1 - e2 * math.sin(lat0)**2) x0 (N0 alt0) * math.cos(lat0) * math.cos(lon0) y0 (N0 alt0) * math.cos(lat0) * math.sin(lon0) z0 (N0 * (1 - e2) alt0) * math.sin(lat0) else: x0 y0 z0 0 N a / math.sqrt(1 - e2 * math.sin(lat)**2) x (N alt) * math.cos(lat) * math.cos(lon) - x0 y (N alt) * math.cos(lat) * math.sin(lon) - y0 z (N * (1 - e2) alt) * math.sin(lat) - z0 return x, y, z这个改进版增加了相对坐标转换功能当提供参考点(lat0,lon0,alt0)时输出的就是相对于该点的局部坐标这在评估SLAM轨迹时特别有用。3.2 生成TUM格式文件转换后的坐标需要按照TUM格式保存。这里有个细节要注意TUM格式要求时间戳是浮点秒数而ROS的Time对象需要特殊处理def save_to_tum(gps_points, output_file): :param gps_points: [(timestamp, lat, lon, alt), ...] :param output_file: 输出文件路径 with open(output_file, w) as f: for stamp, lat, lon, alt in gps_points: x, y, z gps_to_cartesian(lat, lon, alt) # 时间戳转换ROS Time - 浮点秒 if hasattr(stamp, secs): # ROS Time对象 time_sec stamp.secs stamp.nsecs/1e9 else: time_sec float(stamp) # TUM格式时间 x y z qx qy qz qw f.write(f{time_sec:.6f} {x:.6f} {y:.6f} {z:.6f} 0 0 0 1\n)注意四元数部分我们填的是(0,0,0,1)表示无旋转。因为GPS本身不提供姿态信息如果需要姿态数据需要结合IMU或其他传感器。4. 使用evo进行轨迹评估4.1 基础评估命令有了TUM格式的文件后就可以用evo进行各种分析了。最常用的三个命令# 绝对位姿误差(APE) evo_ape tum groundtruth.txt estimated.txt -va --plot # 相对位姿误差(RPE) evo_rpe tum groundtruth.txt estimated.txt -va --plot # 轨迹可视化 evo_traj tum groundtruth.txt estimated.txt -p其中-va表示输出所有统计信息--plot会自动生成可视化图表。我强烈建议加上--save_plot参数把图表保存下来evo_ape tum groundtruth.txt estimated.txt -va --plot --save_plot ape_results.png4.2 处理尺度不一致问题SLAM系统输出的轨迹通常没有绝对尺度metric scale而GPS轨迹是有真实尺度的。这时候直接比较会产生严重偏差。evo提供了两种解决方案尺度对齐scale alignmentevo_ape tum groundtruth.txt estimated.txt -va --align --correct_scale7自由度对齐相似变换evo_ape tum groundtruth.txt estimated.txt -va --align --correct_scale --correct_rotation在我的无人机数据集上测试7参数对齐通常能获得更好的效果特别是当轨迹存在较大旋转偏差时。4.3 高级可视化技巧evo的绘图功能非常强大但默认设置可能不够直观。这是我的常用配置evo_traj tum groundtruth.txt estimated.txt \ --plot_mode xy \ --transform_left SE3_align_config.json \ --ref groundtruth.txt \ --plot_colormap_position 0.8 \ --plot_full_ref其中SE3_align_config.json是预先保存的对齐参数文件可以通过以下命令生成evo_ape tum groundtruth.txt estimated.txt -va --align --save_alignment SE3_align_config.json5. 实战中的常见问题5.1 时间戳同步问题SLAM轨迹和GPS轨迹的时间戳系统往往不同步这会导致评估失效。解决方法有两种时间戳重映射# 用线性插值对齐时间戳 from scipy import interpolate def resample_trajectory(traj, new_timestamps): :param traj: 原始轨迹 [(t,x,y,z,qx,qy,qz,qw), ...] :param new_timestamps: 新的时间戳序列 :return: 重采样后的轨迹 traj np.array(traj) f_x interpolate.interp1d(traj[:,0], traj[:,1], kindlinear) f_y interpolate.interp1d(traj[:,0], traj[:,2], kindlinear) f_z interpolate.interp1d(traj[:,0], traj[:,3], kindlinear) # 姿态需要用球面线性插值(slerp)这里简化处理 new_traj [] for t in new_timestamps: new_traj.append([t, f_x(t), f_y(t), f_z(t)] list(traj[0,4:])) return new_traj使用evo的--t_offset和--t_max_diff参数evo_ape tum groundtruth.txt estimated.txt -va --t_offset 1.2 --t_max_diff 0.15.2 轨迹裁剪与分段评估长轨迹评估时可能只需要关注特定区段。可以用--t_start和--t_end参数# 只评估30秒到60秒之间的轨迹 evo_ape tum groundtruth.txt estimated.txt -va --t_start 30 --t_end 60对于环形轨迹我习惯分段评估并计算平均误差for i in {0..3}; do evo_ape tum groundtruth.txt estimated.txt -va \ --t_start $((i*30)) --t_end $(((i1)*30)) \ --save_results section_$i.zip done evo_res *.zip -p --save_table results.csv5.3 多轨迹对比分析当需要比较多个SLAM算法的性能时可以这样操作# 生成各算法的评估结果 evo_ape tum groundtruth.txt orbslam3.txt -va --save_results orbslam3.zip evo_ape tum groundtruth.txt vinsfusion.txt -va --save_results vinsfusion.zip evo_ape tum groundtruth.txt liosam.txt -va --save_results liosam.zip # 综合对比 evo_res *.zip -p --save_table compare.csv --save_plot compare.png这会生成包含所有算法误差统计的对比表格和箱线图非常适合论文中的实验对比部分。