1. 从零开始：理解FAST_LIO与evo评估的实战价值

大家好，我是老张，在机器人定位与建图这个领域摸爬滚打了十来年，从最早的激光SLAM到现在的多传感器融合，踩过的坑比走过的路还多。今天想和大家聊聊一个非常实际的问题：当你手上只有一个录了激光雷达和IMU数据的ROS bag包，怎么才能知道你的SLAM算法跑得到底准不准？很多刚入行的朋友可能会觉得，算法跑起来，地图看起来不错，轨迹也平滑，这就算成了。但说实话，没有量化评估，一切“看起来不错”都可能是自欺欺人。这就好比炒菜只凭感觉放盐，偶尔能成，但想稳定输出大厨水准，必须得有把精准的秤。

这个“秤”，在SLAM领域就是轨迹评估工具。而今天我们要用的组合，是 FAST_LIO 和 evo。FAST_LIO是一个业界公认非常高效、鲁棒的激光雷达-惯性里程计（LIO）框架，它能把激光雷达的精确测距和IMU的高频运动估计紧紧“拧”在一起，输出稳定可靠的位姿轨迹。但FAST_LIO自己不会告诉你它的误差有多大。这时候，就需要 evo 这个神器出场了。它是一个专门用于评估、可视化和分析SLAM轨迹的工具包，能帮你把算法输出的轨迹和“标准答案”（真值轨迹）放在一起，算出像绝对轨迹误差（ATE）、相对位姿误差（RPE）这些硬核指标。

所以，整个流程的逻辑链条非常清晰：用FAST_LIO处理你的原始传感器bag数据，得到算法估计的轨迹；然后用evo工具，将这条估计轨迹与真值轨迹（如果有的话）进行比对分析，用数据说话，给出精度评价。 这个过程对于算法开发、参数调优、传感器选型乃至论文实验都至关重要。接下来，我就手把手带你走一遍这个完整的实战流程，从数据准备到最终出图出数据，保证你跟着做就能出结果。

2. 实战第一步：准备数据与运行FAST_LIO

拿到一个只有/velodyne_points（激光雷达点云）和/imu_data（IMU数据）的bag包，我们第一步就是让FAST_LIO“吃”进去，并让它“吐”出我们需要的里程计信息。这里有几个关键点，处理不好可能算法都跑不起来。

2.1 环境配置与话题重映射

首先，你得确保FAST_LIO已经在你电脑上编译好了。这个步骤网上教程很多，核心就是依赖装对，特别是PCL和Eigen库的版本要匹配。编译成功之后，你会看到在config文件夹下有一堆.yaml配置文件，这些文件是FAST_LIO的“口味说明书”，告诉它你的传感器是什么型号、怎么安装的、噪声水平如何。

对于最常见的Velodyne雷达和普通IMU，我们通常使用config/velodyne.yaml。用编辑器打开它，你会看到类似下面的关键字段：

common:
    lid_topic: "/velodyne_points"
    imu_topic: "/imu_data"
    time_sync_en: false

这里lid_topic和imu_topic就是FAST_LIO期望订阅的话题名称。但问题来了，你录制的bag包里的实际话题名，未必就叫这个。可能你的雷达话题是/lidar/points，IMU话题是/mavros/imu/data。这就是第一个坑：话题名对不上。

解决的办法就是在播放bag包的时候，使用ROS的话题重映射功能。这功能就像个翻译官，告诉系统：“把bag包里叫A的话题，转发给算法时改名叫B”。命令格式如下：

rosbag play your_recorded.bag /原始雷达话题:=/velodyne_points /原始IMU话题:=/imu_data

举个例子，如果你的bag里雷达话题是/rslidar_points，IMU话题是/imu，那么命令就应该是：

rosbag play my_data.bag /rslidar_points:=/velodyne_points /imu:=/imu_data

这样，FAST_LIO就能正确接收到数据了。我强烈建议在播放bag前，先用rosbag info your_recorded.bag命令查看一下bag包里话题的确切名称，避免想当然。

2.2 参数调优初探

在启动FAST_LIO前，扫一眼配置文件里的其他参数也很有必要，特别是对于新手。有两个参数直接影响融合效果和运行速度：

1. cube_len（地图范围）：这个参数定义了ikd-Tree维护的局部地图的边长。如果场景很大（比如跑室外），这个值需要设得大一些，比如1000.0（单位米），否则地图边界可能会切掉远处的点。但设得太大，会增大树结构的维护开销。室内场景一般200.0就足够了。
2. filter_size_map（地图分辨率）：这是下采样网格的边长，决定了插入ikd-Tree的点云密度。值越小，地图越精细，但计算量越大；值太大，地图会过于稀疏，影响匹配精度。通常0.2到0.5米是一个不错的起点。

对于IMU参数（gyr_n，acc_n等），如果你没有对IMU进行过精确的标定，建议先使用配置文件中的默认值。FAST_LIO对IMU噪声参数有一定的鲁棒性，初期可以不用深究。

2.3 运行并记录里程计

环境准备好后，我们打开三个终端，按顺序执行：

终端1：启动FAST_LIO

source devel/setup.bash
roslaunch fast_lio mapping_velodyne.launch

看到终端开始滚动输出信息，特别是出现[ IMU Process ]: 100%和[ Map Process ]: 100%这样的字样，说明节点启动成功，正在等待数据。

终端2：播放bag数据（带重映射）

rosbag play my_data.bag /rslidar_points:=/velodyne_points /imu:=/imu_data -r 1.0

-r 1.0表示以1倍速播放，你可以根据算法处理速度调整，如果处理不过来，可以降低到0.5倍速。

终端3：记录FAST_LIO的输出轨迹 我们最需要的是它发布的里程计话题，默认是/Odometry。启动FAST_LIO后，可以用rostopic list命令确认一下这个话题是否存在。

rosbag record -O fast_lio_odom.bag /Odometry

这个命令会创建一个名为fast_lio_odom.bag的新bag文件，里面只记录了/Odometry这个话题。等终端2的bag播放完毕，在终端3按Ctrl+C停止记录。

至此，你已经成功地从原始传感器数据中，提炼出了FAST_LIO估计的轨迹数据。它被安全地保存在fast_lio_odom.bag里。接下来，就是把它转换成evo能“读懂”的格式。

3. 数据转换：将ROS轨迹“翻译”给evo

FAST_LIO输出的/Odometry话题是ROS的标准nav_msgs/Odometry消息类型。而evo工具主要支持几种特定的轨迹文件格式，最常用的是TUM格式和KITTI格式。这里我们主要用TUM格式，因为它包含时间戳，对于时间对齐和误差分析更友好。

3.1 使用evo_traj进行格式转换

evo提供了一个非常方便的命令evo_traj，可以直接把bag文件里的轨迹话题转换成文本文件。打开终端，执行：

evo_traj bag fast_lio_odom.bag /Odometry --save_as_tum

这条命令做了以下几件事：

1. 读取fast_lio_odom.bag文件。
2. 找到其中名为/Odometry的话题。
3. 将该话题中的所有位姿消息（包含时间戳、位置x y z、姿态四元数x y z w）提取出来。
4. 默认保存为Odometry.tum文件（TUM格式）。

你可以用--save_as_tum指定输出TUM格式，或者用--save_as_kitti指定KITTI格式。我强烈建议使用TUM格式，因为后续的时间对齐操作需要时间戳。转换完成后，用文本编辑器打开Odometry.tum，你会看到类似这样的内容：

1678887661.744277000 1.302 0.854 0.123 0.001 0.002 0.003 0.999
1678887661.754277100 1.305 0.857 0.124 0.001 0.002 0.003 0.999
...

每一行代表一个位姿，数据依次是：时间戳（秒）、位置x、位置y、位置z、四元数x、四元数y、四元数z、四元数w。

3.2 真值轨迹的准备

轨迹评估需要有参照物。如果你有高精度的真值轨迹，比如来自动作捕捉系统（Vicon、OptiTrack）、差分GPS（RTK-GPS）或者经过后处理优化的轨迹（如LOAM、LIO-SAM的输出），那么评估将是最具说服力的。

真值轨迹也需要转换成同样的TUM格式。假设你有一个记录了真值话题/ground_truth的bag文件，转换命令是类似的：

evo_traj bag ground_truth.bag /ground_truth --save_as_tum

这会生成ground_truth.tum文件。

如果没有真值怎么办？ 这是很多实际项目中的常态。别慌，evo依然大有用处。我们可以：

1. 可视化定性分析：单独绘制FAST_LIO的轨迹，观察其平滑度、闭合性（如果起点终点是同一个地方）。一个漂移严重的轨迹，在图上会表现为无法闭合的环。
2. 相对轨迹误差（RPE）：即使没有绝对真值，我们也可以计算相邻位姿之间的相对误差，这能反映里程计的局部一致性。
3. 多算法对比：如果你用不同参数或不同算法（比如FAST_LIO和LIO-SAM）处理了同一段数据，可以将它们的轨迹放在一起比较，虽然不知道谁绝对正确，但可以看出谁更稳定、漂移更小。

4. evo核心操作：可视化、对齐与误差分析

现在，我们手头有了估计轨迹（Odometry.tum）和真值轨迹（ground_truth.tum）。接下来就是让evo大显身手的时候了。

4.1 轨迹可视化：第一印象

首先，我们可以把两条轨迹画在一起，看看大概长什么样：

evo_traj tum Odometry.tum ground_truth.tum -p --plot_mode=xy

-p参数表示绘图，--plot_mode=xy表示绘制二维的XY平面轨迹图（你也可以用xyz画三维图）。

这张图能给你一个直观的感受：两条轨迹的形状是否相似？估计轨迹有没有明显的尺度错误（比如真值走了100米，估计只走了90米）？有没有明显的漂移？这是定性分析的第一步。

4.2 轨迹对齐：统一“起跑线”

直接计算误差前，有一个至关重要的步骤：轨迹对齐。因为FAST_LIO估计的轨迹和真值轨迹，它们的坐标系原点、姿态基准很可能是不一样的。想象一下，两个人从同一个起点出发，但一个面向东，一个面向北，他们记录的轨迹坐标就完全对不上。我们需要通过旋转和平移，将两条轨迹尽可能地对齐到同一个坐标系下。

evo使用Umeyama算法（一种最小二乘法的SE(3)对齐方法）来帮我们自动完成这个步骤。在执行误差分析命令时，使用-a或--align参数即可：

evo_ape tum ground_truth.tum Odometry.tum -a -p --plot_mode=xy

这条命令会先对齐两条轨迹，然后计算绝对位姿误差（APE）。同样，evo_rpe命令用于计算相对位姿误差，也支持-a参数进行对齐。

关于尺度对齐：如果你的传感器配置（比如IMU和雷达外参）不准，可能会导致估计轨迹和真值轨迹之间存在尺度缩放（scale）差异。这时候可以加上-s参数进行七自由度对齐（旋转、平移、尺度）。命令就变成了evo_ape tum ... -a -s。我一般会先不加-s跑一次，如果误差很大且两条轨迹明显一个“大”一个“小”，再加上-s试试。

4.3 误差指标解读：APE与RPE

evo计算的两个核心误差指标，理解它们的含义非常重要：

• 绝对轨迹误差（APE）：把估计轨迹和真值轨迹对齐后，计算每个时间戳上，两个位姿之间的直接差值（主要是位置差）。它反映了轨迹的全局一致性。比如，你绕场跑了一圈回到起点，APE会告诉你最终位置和起点偏差了多少米。这个值越小，说明全局漂移越小。
• 相对轨迹误差（RPE）：它不关心全局位置，而是看一段固定时间或距离内，估计的位移变化和真实的位移变化之间的误差。比如，计算每隔1秒的位移误差。RPE反映的是里程计的局部精度或短期精度。一个局部抖动很大的轨迹，即使最终漂移不大，RPE也可能很高。

在实际项目中，我通常会同时看这两个指标。APE告诉我算法整体漂移控制得怎么样，RPE告诉我算法在运动过程中是否平滑、稳定。运行evo_ape或evo_rpe后，终端会输出一系列统计值：

max	0.567832
mean	0.123456
median	0.098765
min	0.001234
rmse	0.152637
sse	45.678901
std	0.087654

这里最常关注的是均方根误差（RMSE）和平均值（mean）。RMSE对大的误差更敏感，能告诉你误差的整体水平；mean则反映了平均误差。通常我们会以RMSE作为主要评判标准。

4.4 生成专业的评估图表

除了看终端数字，evo生成的图表包含了更丰富的信息。以evo_ape生成的图表为例，通常包含三到四个子图：

1. 轨迹图：显示对齐后的估计轨迹和真值轨迹。
2. 误差随时间变化图：每个时间点的APE值，可以看到误差在哪些时间段突然增大（比如快速旋转、遮挡严重的时候）。
3. 误差随距离变化图：误差随着轨迹长度累积的情况，直观展示漂移趋势。
4. 误差分布直方图：看看误差主要集中在哪个区间，是否符合正态分布。

这些图能帮你快速定位问题。比如，如果误差在某个转弯处剧增，你可能需要检查IMU的角速度噪声参数gyr_n是否设得太小，或者雷达在该角度下的点云是否质量太差。

5. 进阶技巧与避坑指南

掌握了基本流程，下面分享几个我实战中总结的进阶技巧和常见坑点，能帮你节省大量调试时间。

5.1 时间戳同步与补偿

这是一个隐藏得很深的坑。FAST_LIO发布的/Odometry消息的时间戳，是雷达帧结束的时间（lidar_end_time）。而你的真值轨迹（比如来自动作捕捉系统）的时间戳基准可能不同。如果直接计算，会因为时间没对齐而引入巨大误差。

evo提供了手动时间补偿参数-t或--t_offset。例如，如果你发现估计轨迹整体比真值轨迹晚了0.1秒，可以这样计算RPE：

evo_rpe tum ground_truth.tum Odometry.tum -a -t 0.1 --plot_mode=xy

更严谨的做法是，在录制数据时，确保所有设备（雷达、IMU、真值系统）都接入同一个时间服务器（如PTP），或者至少记录下它们之间的固定时间偏移。

5.2 使用evo_config进行个性化绘图

evo默认的绘图风格可能不符合你的论文或报告要求。你可以使用evo_config命令来全局修改设置，或者为单次绘图生成配置文件。

比如，我觉得默认的轨迹线太细，想加粗，并且把真值轨迹改成虚线：

1. 先复制默认配置生成一个本地配置文件：evo_config generate --type plot
2. 这会生成一个plot_config.json文件。用编辑器打开，找到trajectory相关的设置，修改linewidth（线宽）和linestyle（线型，--代表虚线）。
3. 在绘图时指定这个配置文件：evo_traj tum ... -p --plot_mode=xy --cfg plot_config.json

你还可以修改字体大小、颜色、图例位置、保存图片的分辨率和格式（PNG、PDF）等，让图表直接达到出版级水准。

5.3 批量处理与自动化脚本

当你需要测试多组参数、多个数据集时，手动一条条运行命令效率太低。我习惯写一个简单的Bash脚本来自动化这个过程。脚本的核心逻辑是循环不同的参数配置，运行FAST_LIO，记录轨迹，转换格式，最后用evo计算误差并把关键结果（如RMSE）追加到一个日志文件里。

#!/bin/bash
# 这是一个简化示例
DATASETS=("dataset1.bag" "dataset2.bag")
PARAMS=("param_set1.yaml" "param_set2.yaml")

for dataset in "${DATASETS[@]}"; do
  for param in "${PARAMS[@]}"; do
    echo "Processing $dataset with $param ..."
    # 1. 替换FAST_LIO配置文件
    cp $param $FAST_LIO_PATH/config/velodyne.yaml
    # 2. 启动FAST_LIO并处理bag（这里需要更复杂的进程管理，建议用launch文件）
    # 3. 转换轨迹
    evo_traj bag output_odom.bag /Odometry --save_as_tum -o est_${dataset}_${param}.tum
    # 4. 计算APE并提取RMSE
    evo_ape tum ground_truth.tum est_${dataset}_${param}.tum -a --quiet --save_results results/${dataset}_${param}.zip | grep rmse >> results_summary.txt
  done
done

这样，跑一晚上实验，第二天早上就能直接查看汇总的误差表格，快速找到最优参数组合。

5.4 常见问题排查

• FAST_LIO运行时报错或崩溃：首先检查config文件中的lid_topic和imu_topic是否与bag中话题名通过重映射对应上。其次，检查点云消息的frame_id和IMU消息的frame_id，在config文件中lidar2imu_tf的外参矩阵设置是否正确。最后，查看终端报错信息，很可能是点云数据格式（PointCloud2的fields）不匹配，FAST_LIO对不同雷达（Velodyne, Ouster, Livox）有不同处理分支，确保在launch文件或参数中选对了雷达类型。
• evo_traj报错“No messages of desired type found”：这表示在bag文件中没找到指定的话题。请用rosbag info your.bag再次确认话题名称是否正确，特别注意话题名称前面的/符号。
• 轨迹误差大得离谱：首先检查可视化对齐后的图，看两条轨迹形状是否相似。如果形状相似但位置对不上，是-a对齐没做好，可以尝试-as（带尺度对齐）。如果形状都完全不同，那问题可能出在FAST_LIO本身估计错了，需要回头检查传感器数据质量、参数配置，或者bag数据本身是否有问题（比如IMU数据没发布、雷达点云全是NaN值）。
• evo绘图不显示或报字体错误：这是Matplotlib的常见问题。可以尝试在脚本或命令前设置环境变量：export MPLBACKEND=Agg，或者修改Matplotlib的配置文件，使用已安装的字体。

走完这一整套流程，从原始数据到最终的误差数字和图表，你才算真正完成了一次对激光雷达惯性里程计算法的定量评估。这个过程不仅能验证算法性能，更能帮助你深入理解传感器融合中参数的影响，以及如何系统地分析和解决定位漂移问题。记住，好的评估习惯是提升工程能力的捷径。下次当你调优了一个参数感觉“好像变好了”的时候，别忘了用evo跑一下，让数据告诉你真实的答案。