1. 从报错信息说起:为什么我的轨迹和真值“对不上”?
最近在折腾ORB-SLAM2,跑完Euroc数据集想看看效果怎么样,结果用evo一评估,直接给我泼了盆冷水。命令行里赫然出现一行报错:found no matching timestamps between CameraTrajectory.txt and gt/MH01_GT.txt with max. time diff 0.01 (s) and time offset 0.0 (s)。翻译过来就是,evo工具在我算法生成的轨迹文件 CameraTrajectory.txt 和数据集提供的真值文件 MH01_GT.txt 之间,一个匹配的时间戳都没找到。允许的最大时间差是0.01秒,时间偏移设的是0,按理说要求已经非常宽松了,但还是匹配不上。
这感觉就像你拿着两张电影票,一张写着“14:30:00”,另一张写着“143000000000”,检票员(evo)当然会告诉你这两张票对不上,看不了电影。我当时第一反应是,是不是我跑的数据序列不对?或者保存轨迹的姿势有问题?于是我用 head 命令分别看了一眼两个文件的前几行。
真值文件(gt/MH01_GT.txt)长这样:
1403715273412143104 4.276342 -2.372546 1.857908 0.419058 -0.113071 -0.070379 0.897665
1403715273412143360 4.276330 -2.372513 1.857904 0.419058 -0.113071 -0.070379 0.897665
而我的ORB-SLAM2输出的轨迹文件(CameraTrajectory.txt)却是这样:
1.40372e+09 4.276342 -2.372546 1.857908 0.419058 -0.113071 -0.070379 0.897665
1.40372e+09 4.276330 -2.372513 1.857904 0.419058 -0.113071 -0.070379 0.897665
乍一看,位姿数据(后面那7个数字)几乎一模一样,说明算法跑出来的位置和姿态是准的。问题就出在第一列,也就是时间戳。真值文件的时间戳是一串长长的整数(如1403715273412143104),而我的轨迹文件时间戳是科学计数法表示的浮点数(如1.40372e+09)。虽然1.40372e+09换算成整数就是1403720000,但这和真值的1403715273412143104差了十万八千里,根本不是一个数量级。evo在内部进行时间戳匹配时,会尝试在允许的微小时间差内(比如0.01秒)寻找最接近的配对,现在这两个数字天差地别,自然就“找不到对象”了。
这个问题的根源,其实在于时间戳的“刻度”或者说单位不统一。在计算机里,时间戳本质上就是一个数字,但这个数字代表多少秒、多少毫秒还是多少纳秒,需要有统一的约定。TUM数据集格式(以及evo工具默认期望的格式)使用的是以秒为单位的浮点数时间戳,并且通常精度很高(小数点后很多位)。而ORB-SLAM2源码中默认输出的时间戳,是直接使用了系统的时间,这个时间的“单位”和“起点”可能和TUM标准不一致,导致了格式上的巨大差异。接下来,我们就得深入ORB-SLAM2的“心脏”——源码,去把这个“刻度”给纠正过来。
2. 深入病灶:定位ORB-SLAM2中的时间戳生成点
要解决问题,首先得知道问题出在哪儿。ORB-SLAM2在运行结束时,通常会调用一个函数来保存相机运动轨迹。根据你运行的是单目、双目还是RGB-D版本,以及你调用的示例程序,这个保存函数可能不同。常见的保存函数有 SaveTrajectoryTUM 和 SaveTrajectoryKITTI。既然我们要评估的是TUM格式的数据集,并且希望输出与TUM真值对齐的轨迹,那么重点就在 SaveTrajectoryTUM 这个函数上。
这个函数通常定义在 ORB_SLAM2/src/System.cc 文件中。它的作用就是遍历SLAM系统在整个运行过程中记录下来的所有关键帧位姿,按照“时间戳 tx ty tz qx qy qz qw”的格式写入文本文件。其中,时间戳 *lT 是一个指向 double 类型时间的指针。问题就出在对这个 *lT 的处理上。
在原始的ORB-SLAM2代码中,SaveTrajectoryTUM 函数里写入时间戳的那行代码大概是这样的:
f << setprecision(6) << (*lT) << " " << setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
这里 setprecision(6) 设置了输出流 f(也就是我们的轨迹文件)的浮点数精度为6位小数。对于后面的位姿数据,又用 setprecision(9) 设置了更高的精度。但关键在于,(*lT) 这个值被直接输出了。
那么 *lT 到底是什么呢?这需要追溯到时间戳的来源。在ORB-SLAM2中,时间戳通常是在图像帧被载入时赋予的。对于从视频文件读取的情况,它可能是帧的索引;对于从数据集读取的情况,它通常直接来自数据集中图像自带的、以某种单位(比如纳秒)表示的时间戳。以Euroc数据集为例,它的图像时间戳就是以纳秒为单位的整数。如果ORB-SLAM2的示例程序(比如 Examples/Monocular/mono_euroc.cc)没有对这个纳秒时间戳进行转换,直接把它当作 double 类型赋给了 *lT,那么一个像 1403715273412143104 这样的纳秒数,以 double 类型存储并输出,就很容易变成 1.40372e+09 这种科学计数法形式,而且数值上丢失了大量精度(因为 double 的有效位数有限,无法精确表示这么大的整数)。
所以,核心矛盾在于:真值文件的时间戳是以秒为单位的小数(例如 1403715273.412143104),而ORB-SLAM2直接输出了未经转换的、以纳秒为单位的原始大整数。两者在数值上相差了10^9倍(1秒 = 10^9纳秒)。我们的任务,就是在保存轨迹的那一刻,把这个纳秒时间戳除以10^9,转换成秒。
3. 手把手修改源码:让时间戳“秒”起来
找到了问题根源,修改起来就目标明确了。我们需要在 System.cc 文件的 SaveTrajectoryTUM 函数中,将时间戳输出行进行修改,把原始的 (*lT) 转换为 (*lT) / 1e9。注意,这里用除法 / 1e9 和乘法 * 1e-9 是等价的,但为了和常见的转换方式一致,我们采用除法。
操作步骤如下:
- 定位文件:用你喜欢的代码编辑器(如VSCode、Sublime Text、甚至vim)打开
ORB_SLAM2/src/System.cc。 - 搜索函数:在文件中搜索
SaveTrajectoryTUM,找到这个函数的定义。 - 找到关键行:在这个函数内部,你会看到一个循环,遍历所有关键帧(
vpKFs)。在循环体内,找到向文件流f写入数据的那一行,它看起来就像上面提到的那段代码。 - 实施修改:将写入时间戳的部分从
(*lT)修改为(*lT) / 1e9。修改后的代码行应如下所示:
非常重要:这里f << setprecision(6) << (*lT) / 1e9 << " " << setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;setprecision(6)是作用于(*lT) / 1e9这个结果的,意味着时间戳会以浮点数形式输出,保留6位小数。这对于TUM格式和evo工具来说是标准且兼容的。 - 保存文件:修改完成后,保存
System.cc。
但是,这里有一个极其关键的细节,也是很多朋友踩坑的地方:ORB-SLAM2的代码结构可能因版本或修改分支而异。我强烈建议你在修改前,先确认你使用的 SaveTrajectoryTUM 函数的具体实现。有些修改版的ORB-SLAM2,或者针对特定数据集调整过的版本,可能已经对时间戳做了其他处理,或者这个函数被定义在了其他地方(比如单独的 Save.cc 文件里)。一个稳妥的方法是,在整个ORB_SLAM2项目目录下全局搜索 SaveTrajectoryTUM 这个字符串,确保你修改的是真正被调用的那个函数。
修改完成后,你需要重新编译ORB-SLAM2。进入你的ORB_SLAM2项目根目录(通常有 build.sh 或 build 文件夹的那个目录),执行:
cd ORB_SLAM2
./build.sh
或者如果你之前是用CMake手动编译的:
cd build
make -j4
等待编译完成,没有报错即可。
4. 验证与测试:让evo“握手”成功
源码改好了,也重新编译了,现在到了检验成果的时候。我们重新运行ORB-SLAM2处理Euroc数据集(例如MH01序列)。运行命令和你之前一样,比如:
./Examples/Monocular/mono_euroc Vocabulary/ORBvoc.txt Examples/Monocular/EuRoC.yaml /path/to/EuRoC/MH01 Examples/Monocular/EuRoC_TimeStamps/MH01.txt
程序运行结束后,会在当前目录生成(或覆盖) CameraTrajectory.txt 文件。
现在,用文本编辑器或者 head 命令再次查看这个新的轨迹文件:
head -n 5 CameraTrajectory.txt
你应该看到时间戳已经变成了类似这样的浮点数:
1403715273.412143 4.276342 -2.372546 1.857908 0.419058 -0.113071 -0.070379 0.897665
1403715273.412143 4.276330 -2.372513 1.857904 0.419058 -0.113071 -0.070379 0.897665
注意,第一列现在是 1403715273.412143,这正好等于真值文件里的 1403715273412143104 / 1e9。虽然由于浮点数精度限制,小数点后的位数可能不会完全一致(真值文件是整数除出来的,可能更长),但只要前几位小数对得上,在evo允许的微小时间差(如0.01秒)内,它们就能成功匹配。
现在,使用evo进行绝对轨迹误差(APE)评估:
evo_ape tum /path/to/EuRoC/MH01/gt/MH01_GT.txt CameraTrajectory.txt -va --plot
如果一切顺利,那个恼人的“found no matching timestamps”错误应该消失了。evo会开始计算匹配到的轨迹点对,并输出APE的统计结果,包括RMSE(均方根误差)、均值、中位数等指标,并且会弹出轨迹对比和误差分布的图表。
进阶验证与技巧:
- 时间戳对齐检查:在正式评估前,可以用
evo_traj命令先可视化一下两条轨迹,并检查时间戳对齐情况:
加上evo_traj tum CameraTrajectory.txt --ref=/path/to/EuRoC/MH01/gt/MH01_GT.txt --plot --plot_mode xy --check_timestamps--check_timestamps参数,evo会在终端输出时间戳匹配的详细信息,帮助你确认匹配是否成功。 - 处理时间偏移:有时候,即使单位对了,算法轨迹和真值轨迹之间可能还存在一个固定的时间延迟或提前(比如传感器数据同步问题)。这时,evo的
--t_offset参数就派上用场了。你可以尝试指定一个时间偏移量,让evo在匹配时考虑这个偏移。不过,对于我们这种因单位错误导致的数量级差异,--t_offset是解决不了的,必须先修正单位。 - 注意其他保存函数:如果你在别的示例程序中调用的是
SaveTrajectoryKITTI来保存轨迹,然后想用KITTI格式评估,那可能不存在这个问题,因为KITTI格式不包含时间戳。但如果你保存了TUM格式却用KITTI评估,或者反过来,也会出错。务必确保评估工具(evo)、轨迹文件格式(tum/kitti)和真值文件格式三者一致。
5. 原理延伸:时间戳在SLAM评估中的核心作用
解决了这个实战问题,我们不妨再深入一点,聊聊时间戳为什么这么重要。在SLAM(同步定位与建图)中,轨迹评估的本质是比较两个时空序列的吻合程度。这里既有空间(位置和姿态)的比对,也有时间(顺序和时刻)的比对。
1. 时间戳是数据关联的“胶水”:当我们说“绝对轨迹误差”时,我们并不是简单地把算法输出的所有位姿和真值的所有位姿随便拉出来算个距离。那样是没有意义的,因为两者可能长度不同、起始点不同、频率不同。我们需要找到同一时刻(或尽可能接近的同一时刻)算法估计的位姿和真实位姿,将它们配对。这个“同一时刻”的判断依据,就是时间戳。evo工具内部会基于你设定的最大时间差阈值(--t_max_diff,默认0.01秒),为每一条轨迹中的每个位姿,在另一条轨迹中寻找时间戳最接近的伙伴,形成配对点集。所有后续的误差计算(平移误差、旋转误差)都是基于这些成功的配对来进行的。如果时间戳格式不对、单位不对,这个配对过程就完全失效了。
2. 不同的时间戳“世界观”:在机器人学和计算机视觉领域,常见的时间戳格式主要有两种“世界观”:
* 秒制浮点数:这是TUM数据集、ROS系统(rosbag)中常见的时间表示法。时间戳是一个双精度浮点数,表示从某个纪元(比如Unix时间戳的1970年1月1日)开始经过的秒数。小数部分表示秒的分数,可以提供纳秒级的理论精度(虽然双精度浮点数的实际有效位数有限)。这种表示法对人类阅读相对友好,也便于进行时间差计算(直接相减就是秒数)。
* 整数纳秒/微秒:许多硬件传感器(如IMU、相机)和原始数据集(如Euroc、KITTI的某些部分)直接记录整数形式的时间戳,单位是纳秒或微秒。这样做可以避免浮点数的精度损失,特别是当时间值非常大时。但直接使用这些大整数进行时间匹配或可视化就不太方便。
我们的修改操作,本质上就是完成了一次从“传感器原始时间世界观”(纳秒整数)到“评估标准时间世界观”(秒制浮点)的坐标转换。理解这一点,以后遇到其他数据集或工具的时间戳问题,你就能举一反三了。
3. 不仅仅是ORB-SLAM2和evo:这个时间戳格式匹配的问题具有普遍性。只要你是在用某个SLAM算法处理带有高精度时间戳的数据集,并且需要用第三方工具(如evo、rpg_trajectory_evaluation)进行评估,就很可能遇到类似问题。解决思路都是一样的:弄清数据源的时间戳单位,弄清评估工具期望的时间戳单位,然后在输出轨迹的环节进行正确的转换。这可能意味着修改SLAM算法的保存函数,也可能意味着在评估前写一个简单的脚本对轨迹文件进行预处理。
6. 避坑指南与更多实用技巧
在实战中,除了上述核心问题,还有一些常见的“坑”和技巧值得分享。
坑1:修改了源码但编译没生效。 这可能是最让人郁闷的情况。请确保:
- 你修改的是正确的源文件,并且保存了。
- 你执行了完整的重新编译。有时候只
make可能不够,如果修改了头文件或者CMakeLists.txt,可能需要清理构建目录(rm -rf build/然后重新mkdir build && cd build && cmake ..)。 - 你运行的是新编译出来的可执行文件,而不是系统其他地方缓存的旧版本。
坑2:时间戳转换后,evo还是报少量不匹配。 这可能是因为:
- 浮点数精度问题:正如前面提到的,纳秒整数除以1e9转换成浮点数时,可能会有极其微小的精度损失。虽然对于0.01秒的匹配阈值来说这通常不是问题,但如果你的数据集时间戳本身频率就很高(如100Hz以上),两个连续帧的时间差本来就很小,微小的精度损失可能导致匹配时排在后面的点更“接近”。可以尝试稍微放宽
--t_max_diff参数,比如设为0.02秒。 - 轨迹起点/终点不一致:算法轨迹和真值轨迹可能不是严格同时开始和结束的。evo在匹配时,会自动寻找两条轨迹时间重叠的部分进行评估。但如果重叠部分很少,或者因为初始化的原因算法前几帧没有输出,也可能导致匹配到的点对很少。可以用
evo_traj的--plot功能直观查看两条轨迹的时间覆盖范围。
技巧1:使用Python脚本进行灵活的轨迹预处理。
如果你不想频繁修改和编译C++源码,或者需要处理多种不同格式的轨迹,写一个Python预处理脚本是更灵活的方法。例如,你可以写一个脚本,读取ORB-SLAM2生成的原始轨迹文件,将其时间戳列除以1e9,然后保存为新的文件供evo使用。使用 numpy 和 pandas 可以非常方便地完成这种操作。
技巧2:理解evo的多种对齐模式。 除了依赖时间戳的严格匹配,evo还提供了其他对齐轨迹的方式,例如:
--align或-a:使用Umeyama算法进行相似变换(旋转、平移、缩放)对齐,这可以消除轨迹之间坐标系不一致或尺度差异的影响。注意:--align通常是在时间戳成功匹配、找到对应点对的基础上进行的进一步空间对齐。它不能替代时间戳匹配。--correct_scale:对于单目SLAM这类存在尺度模糊性的算法,可以尝试估计并纠正一个尺度因子。
在实际评估中,合理的流程是:先确保时间戳匹配(解决本文的问题),然后根据算法特性(单目/双目)决定是否使用 --correct_scale,最后再考虑是否使用 --align 进行精细的空间对齐。不同的对齐方式会显著影响最终的误差指标,在论文或报告中需要明确说明你采用了哪种评估协议。
处理时间戳格式不匹配这个问题,虽然看起来只是一个简单的乘除运算,但它贯穿了从数据理解、源码修改到工具使用的整个SLAM评估流程。把它搞明白,不仅是解决了一个报错,更是打通了算法输出与性能量化之间的关键桥梁。下次再看到evo抱怨时间戳对不上,你就能自信地定位问题,快速解决了。
9448



被折叠的 条评论
为什么被折叠?



