简介面向使用VINS-Fusion等视觉惯性导航系统、需要KITTI数据集标准基准位姿用于算法评测与轨迹对比的研究人员和开发者。资源针对KITTI原始数据完整整理了从00到10共十一个序列的基准真值位姿与原始时间戳文件并给出了转换到TUM格式的位姿结果可直接配合TUM工具集或evo评估套件使用方便将VINS等SLAM系统输出的轨迹与真值进行对比、计算相对与绝对误差。压缩包共包含47个文件核心内容包括34个txt格式的位姿与时间戳文本、11张各序列真实轨迹效果图、1张数据集序列对应关系说明图以及1个Python脚本整体压缩后仅3.54MB下载与解压都很轻量。其中Python脚本实现了将原始poses与时间戳自动转换为TUM轨迹格式供各类工具直接读取轨迹图则直观展示每个序列的行驶路径便于定位数据对应关系。目前已有3734人学习下载对正在调试VINS-Fusion、需要标准基准验证定位精度的读者具有较强的实用价值。 做视觉SLAM或者LiDAR SLAM精度评估的同学基本都绕不开KITTI数据集。我最近在折腾KITTI raw data的时候遇到一个非常典型的痛点官方给的groundtruth是oxts格式的GPS/IMU组合导航数据时间戳是UTC字符串位姿分散在经纬度、姿态角、速度这些字段里面根本不方便直接喂给evo或者TUM评估工具链。花了好几天时间把整个流程整理通顺踩了一堆坑这篇就把“如何从KITTI raw data拿到干净的groundtruth并转成TUM标准的位姿文件”这件事从头到尾讲清楚。文章适合正在用KITTI做SLAM/里程计评估的同学尤其是需要精确groundtruth和时间戳对齐、想用evo做轨迹误差分析的小组。内容会覆盖三个核心问题oxts文件里到底装了什么时间戳应该怎么精确转换以及如何正确地把oxts位姿换算到相机坐标系并输出为TUM格式。后面还会附上我实测过程中遇到的几个坑和排查方法。1. 先把groundtruth的家底摸清楚oxts不只是经纬度1.1 文件结构与28个字段分别代表什么打开KITTI raw data任意一个序列比如2011_09_26/2011_09_26_drive_0002_sync里面会有image_00、image_01、velodyne_points、oxts等文件夹。真正用于基准轨迹的groundtruth存放在oxts目录下结构是这样的oxts/ data/ 0000000000.txt 0000000001.txt ... dataformat.txt timestamps.txtdata目录里每个txt文件对应一帧组合导航数据每一行是一串空格分隔的浮点数总共有28个字段。第一次打开dataformat.txt的时候感觉被信息淹没了其实整理一下就清楚了前3个是经纬度高程中间是姿态角后面是速度、加速度、角速度和定位状态。字段位置和含义大致如下表完整说明以官方dataformat.txt为准字段序号含义单位1-3纬度lat、经度lon、海拔alt度、米4-6横滚roll、俯仰pitch、偏航yaw弧度7-9前向vf、左侧vl、向上vu速度m/s10-15车辆坐标系与传感器坐标系的加速度分量m/s²16-21角速度分量rad/s22-23位置精度、速度精度米、m/s24-28导航状态、卫星数、定位模式等-这里有个容易忽略的点oxts的坐标系定义是前-左-上x轴指向车头方向y轴指向车辆左侧z轴指向上方。后文所有坐标变换都要围绕这个定义展开很多转换结果对不上的原因就是这里从一开始就搞错了。1.2 注意区分raw data和Odometry数据集的groundtruth很多人会把KITTI raw data和KITTI Odometry的groundtruth混为一谈。Odometry数据集也就是00-10序列里直接提供了pose/00.txt这类文件每行12个数可以直接拼成3x4变换矩阵那个就是左目相机在世界坐标系下的绝对位姿非常干净。但Odometry数据集不提供原始传感器数据也没有GPS/IMU的完整观测时间戳只有一个简单的times.txt只有相对秒数没有绝对UTC时间。而raw data的优势在于传感器种类齐全、时间戳完整包含100Hz的oxts数据和10Hz的相机/LiDAR数据。代价就是groundtruth需要你自己从oxts里“挖”出来并做坐标变换。印象里很多教程只讲Odometry数据集的pose文件怎么读很少讲raw data这条线。如果你需要用到视频帧、点云和真实GPS轨迹的对应关系或者想评估前端在原始传感器数据上的表现就必须把raw data这套流程搞清楚。2. 时间戳的处理从UTC字符串到能直接计算的浮点秒2.1 时间戳文件与sync机制oxts/timestamps.txt的格式长这样2011-09-26 13:02:54.997343637 2011-09-26 13:02:55.014351424 2011-09-26 13:02:55.031359211这个时间是以UTC格式保存的精确到纳秒频率为100Hz。注意这里有个细节如果使用_sync后缀的数据oxts/timestamps.txt已经被降采样到10Hz并与相机、激光雷达对齐了如果使用raw data不带_sync那么oxts是100Hz原始频率需要自己和相机时间戳对齐。实际做SLAM评估我一般直接用_sync数据省去对齐的麻烦。但如果你需要做高精度时间插值那就是另外一个话题了。对于转TUM格式我们只需要_sync数据里的10Hz时间戳因为TUM格式本身不强制要求时间连续均匀。2.2 精确转换为Unix时间戳TUM格式的第一列需要一个浮点秒常见的是Unix时间戳。直接拿Python的datetime.strptime解析会出问题因为KITTI时间戳的纳秒部分是9位数字而普通%f只处理微秒精度时间戳第6位之后会被截断。虽然对于视觉SLAM的10Hz数据来说微秒级误差几乎不影响评估但从严谨角度还是推荐手动解析。我实际使用的解析方法是这样from datetime import datetime, timezone def kitti_timestamp_to_unix(ts_str: str) - float: # 拆出秒和纳秒部分 base_str, nano_str ts_str.strip().split(.) base_dt datetime.strptime(base_str, %Y-%m-%d %H:%M:%S) # 视为UTC时间不转本地时区 base_ts base_dt.replace(tzinfotimezone.utc).timestamp() # 纳秒部分可能需要左补零到9位这里按实际位数处理 nano int(nano_str.ljust(9, 0)) return base_ts nano / 1e9有一个细节值得注意datetime.strptime解析出来的base_dt不带时区信息因此必须显式指定tzinfotimezone.utc再调用timestamp()否则Python会按照系统本地时区解释这个时间导致偏移几个小时的错误。我在切换电脑时踩过这个坑换了一台配置了UTC时区的机器后轨迹时间整体偏移8小时评估工具根本对不上。2.3 TUM格式对时间戳的具体要求TUM格式的每行结构是timestamp tx ty tz qx qy qz qw其中timestamp的单位是秒一般用Unix浮点时间TUM官方RGB-D数据集用的是类似1305031453.724000这种格式。KITTI的UTC时间戳转成浮点秒后直接作为第一列即可。很多人会在这一步犯迷糊到底该用相对时间还是绝对时间我的建议是统一用绝对Unix时间戳。原因是evo、rpg_trajectory_evaluation这些工具都支持自动时间对齐绝对时间戳能避免自己手动做相对时间偏移而且后续如果要融合其他传感器数据绝对时间戳更不容易乱。当然如果你只需要相对位姿序列做SLAM评估用第一帧归零的相对时间也完全可以关键是保持一致。3. 完整实操把oxts转换成TUM格式位姿3.1 坐标系关系与变换链先理清坐标系变换链这是整个转换过程最核心的部分也是出错率最高的地方。KITTI raw data涉及的坐标系主要有世界坐标系一般取第一帧IMU位置为原点IMU坐标系前-左-上Velodyne激光雷达坐标系前-左-上相机坐标系以cam0为例X向右Y向下Z向前从oxts数据直接计算得到的是IMU在世界坐标系下的位姿。而我们在SLAM评估里通常需要的是相机在世界坐标系下的位姿。所以至少需要一次坐标系变换T_cam_world T_cam_imu * T_imu_world其中T_imu_world由oxts的经纬度和姿态角计算T_cam_imu来自标定文件。KITTI raw data的标定文件有calib_imu_to_velo.txt和calib_velo_to_cam.txt两者相乘就能得到T_cam_imu。从文件内容看calib_velo_to_cam.txt给出的是T_cam_velo所以T_cam_imu T_cam_velo * T_velo_imu而T_velo_imu就是calib_imu_to_velo.txt里的变换矩阵。注意不要搞反方向calib_imu_to_velo.txt里直接给的是从IMU到Velodyne的变换所以它就是T_velo_imu不需要再求逆。读标定文件时官方给的是R3x3矩阵按行存储和平移T3x1向量。我习惯把它们拼成4x4齐次矩阵再参与运算。3.2 经纬度转局部平面坐标从oxts里的lat/lon/alt到世界坐标系的平移量最常用的做法是投影到UTM坐标系然后以第一帧为原点做差分。KITTI官方devkit里用的就是这个思路。用pyproj实现非常简洁import pyproj import numpy as np # 根据KITTI序列所在区域选择UTM zone卡尔斯鲁厄大约在zone 32 transformer pyproj.Transformer.from_crs(EPSG:4326, EPSG:32632, always_xyTrue) def latlon_to_utm(lat, lon): easting, northing transformer.transform(lon, lat) return easting, northing以第一帧为原点之后每一帧的平移量为x easting - easting0 y northing - northing0 z alt - alt0这里有个细节这里的x对应东向y对应北向z对应高度。但IMU坐标系是前-左-上当车行进方向不一定朝东时直接用这个(x,y,z)作为位姿平移其实已经是“世界坐标系”下的坐标了。后文构建的旋转矩阵要保证和这个平移匹配。换句话说(x,y,z)描述的是IMU在世界坐标系ENU下的位置姿态四元数描述的是IMU相对于世界坐标系的旋转两者之间不需要再做额外旋转。如果不想引入UTM的zone依赖也可以做一个等距近似x (lon - lon0) * np.pi / 180.0 * R * np.cos(np.radians(lat0)) y (lat - lat0) * np.pi / 180.0 * R z alt - alt0对于KITTI数据这种几公里范围的短序列两种方法差不了太多。不过既然有现成工具我建议直接用UTM省心也便于扩展。3.3 姿态角转四元数并组合变换矩阵oxts给的是roll、pitch、yaw。KITTI官方在这块的定义是旋转按ZYX顺序也就是先绕Z轴转yaw再绕Y轴转pitch最后绕X轴转roll。构建旋转矩阵时用def oxts_to_rotation_matrix(roll, pitch, yaw): Rx np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]]) Ry np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]]) Rz np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]]) return Rz Ry Rx然后把这个旋转矩阵转成四元数。手写转换容易出错我一般直接用scipy.spatial.transform.Rotationfrom scipy.spatial.transform import Rotation R_imu_world oxts_to_rotation_matrix(roll, pitch, yaw) q_imu_world Rotation.from_matrix(R_imu_world).as_quat() # 返回 [x, y, z, w]注意scipy返回的是[x, y, z, w]TUM格式也是qx qy qz qw正好对应上。组合起来就是IMU在世界坐标系下的4x4变换矩阵T_imu_world np.eye(4) T_imu_world[:3, :3] R_imu_world T_imu_world[:3, 3] [x, y, z]如果只需要IMU位姿到这里其实就可以输出TUM了。但大多数人想评估的是相机轨迹所以继续往下做相机变换。3.4 输出TUM格式文件最终代码框架我整理成一个可运行的脚本流程import numpy as np from scipy.spatial.transform import Rotation # 伪代码循环读取oxts和timestamps with open(output.tum, w) as f: for i in range(len(oxts_data)): # 1. 从oxts读取经纬度、姿态角 # 2. 经纬度转局部坐标 (x, y, z) # 3. 姿态角转旋转矩阵再转四元数 # 4. 组装 T_imu_world # 5. 如果有标定 T_cam_world T_cam_imu T_imu_world # 6. 从 T_cam_world 提取平移和四元数 # 7. 写入timestamp x y z qx qy qz qw pass这里的输出文件可以直接被evo读取。比较典型的评估命令是evo_ape tum groundtruth.tum estimated.tum -a -s其中-a表示进行Umeyama轨迹对齐-s表示尺度对齐适用于单目SLAM这类有尺度不确定性的情况。如果估计轨迹已经解决了尺度问题就不需要-s。4. 实战中容易踩的坑与排查方法4.1 标定参数是相对哪个相机的KITTI raw data有4个相机cam0和cam1是灰度相机cam2和cam3是彩色相机。通常做视觉SLAM用的是左侧灰度相机cam0而Odometry数据集的groundtruth对应的也是cam0。但calib_velo_to_cam.txt里只有一个相机变换具体对应哪一个需要结合calib_cam_to_cam.txt来看。我的经验是如果只想快速得到能用的相机位姿直接用calib_velo_to_cam.txt的变换给cam0即可因为KITTI官方的preprocessing pipeline中cam0是基准相机激光雷达标定就是针对cam0做的。如果发现输出的轨迹在方向上比图像内容偏了90度多半是T_cam_imu构造不对。排查方法很简单取第一帧把相机坐标系的原点投影到图像中心附近再看相机Z轴是不是指向场景前方。只要姿态大致符合图像观测就说明旋转方向是对的。4.2 时间戳精度丢失导致轨迹对不上最开始我直接用datetime.strptime(ts, %Y-%m-%d %H:%M:%S.%f)解析KITTI时间戳结果在纳秒位被截断本以为是小事但用evo做时间对齐时发现偶尔出现误匹配。后来改成手动解析纳秒并补零到9位问题就消失了。另外一个时间戳相关的坑是时区问题。KITTI的时间戳是UTC时间如果系统时区是东八区base_dt.timestamp()会自动转成本地时间再计算秒数导致整体时间戳偏移8小时。排查时如果发现时间戳差值总是一个固定的小时数优先检查时区。4.3 轨迹方向、尺度对不上如果你转换出来的轨迹和官方Odometry的groundtruth放在一起看形状很像但方向相反多半是坐标系手性问题。IMU坐标系是x前y左z上相机坐标系是x右y下z前如果不做T_cam_imu变换轨迹会呈现“镜像”效果。尺度对不上则更常见。由于UTM坐标和ENU坐标都是米制理论上不应该有尺度问题。如果你发现轨迹形状一致但整体被放大了检查一下lat/lon到UTM的投影是不是用了错误的zone或者经纬度转弧度的公式漏掉了np.pi/180系数。排查这类问题我推荐一个简单办法把第一帧位姿设为原点第二帧位姿打印出来对照IMU实际朝向和大概几米的位移一眼就能看出坐标和旋转有没有问题。4.4 用哪个频率的oxts数据如果使用不带_sync的raw dataoxts频率是100Hz而相机只有10Hz。直接把100Hz的IMU位姿全部输出为TUM文件虽然看起来更平滑但实际评估SLAM轨迹时估计轨迹是10Hzgroundtruth是100Hz时间对齐时更多的匹配候选反而会增加误匹配概率。所以我建议统一用_sync数据此时oxts已经降采样到10Hz并且和相机帧对齐。如果你的传感器融合方案需要更高频率的IMU那再单独处理raw oxts记得不要把100Hz的轨迹直接和10Hz的视觉轨迹混在一个TUM文件里。5. 最后再分享一个小技巧我在转完TUM格式之后通常还会顺手做一步验证用evo把groundtruth轨迹画出来看它是不是贴合对应的车道走向。KITTI的raw data大多采集于卡尔斯鲁厄城区轨迹形状应该是有规律的沿街转弯如果画出来是一团乱麻或者朝某个方向飞出去那基本就是坐标或旋转矩阵出了问题没必要继续往下跑评估流程。另外有一个容易踩但很好解决的细节多个序列的oxts第一帧坐标各不相同如果要把多个序列放在同一个世界坐标系下比较记得不要把每个序列都独立归零到各自第一帧而是保留绝对UTM坐标或者用每个序列的GPS参考点统一变换。否则跨序列对比误差会非常难看。总的来说把KITTI raw data的groundtruth转成TUM格式并不是一个多复杂的工程但涉及时间解析、坐标系变换、四元数约定这些细节每一步都可能埋雷。希望你看了这篇之后能少走一些我走过的弯路一次跑通。本文还有配套的精品资源点击获取
简介面向使用VINS-Fusion等视觉惯性导航系统、需要KITTI数据集标准基准位姿用于算法评测与轨迹对比的研究人员和开发者。资源针对KITTI原始数据完整整理了从00到10共十一个序列的基准真值位姿与原始时间戳文件并给出了转换到TUM格式的位姿结果可直接配合TUM工具集或evo评估套件使用方便将VINS等SLAM系统输出的轨迹与真值进行对比、计算相对与绝对误差。压缩包共包含47个文件核心内容包括34个txt格式的位姿与时间戳文本、11张各序列真实轨迹效果图、1张数据集序列对应关系说明图以及1个Python脚本整体压缩后仅3.54MB下载与解压都很轻量。其中Python脚本实现了将原始poses与时间戳自动转换为TUM轨迹格式供各类工具直接读取轨迹图则直观展示每个序列的行驶路径便于定位数据对应关系。目前已有3734人学习下载对正在调试VINS-Fusion、需要标准基准验证定位精度的读者具有较强的实用价值。 做视觉SLAM或者LiDAR SLAM精度评估的同学基本都绕不开KITTI数据集。我最近在折腾KITTI raw data的时候遇到一个非常典型的痛点官方给的groundtruth是oxts格式的GPS/IMU组合导航数据时间戳是UTC字符串位姿分散在经纬度、姿态角、速度这些字段里面根本不方便直接喂给evo或者TUM评估工具链。花了好几天时间把整个流程整理通顺踩了一堆坑这篇就把“如何从KITTI raw data拿到干净的groundtruth并转成TUM标准的位姿文件”这件事从头到尾讲清楚。文章适合正在用KITTI做SLAM/里程计评估的同学尤其是需要精确groundtruth和时间戳对齐、想用evo做轨迹误差分析的小组。内容会覆盖三个核心问题oxts文件里到底装了什么时间戳应该怎么精确转换以及如何正确地把oxts位姿换算到相机坐标系并输出为TUM格式。后面还会附上我实测过程中遇到的几个坑和排查方法。1. 先把groundtruth的家底摸清楚oxts不只是经纬度1.1 文件结构与28个字段分别代表什么打开KITTI raw data任意一个序列比如2011_09_26/2011_09_26_drive_0002_sync里面会有image_00、image_01、velodyne_points、oxts等文件夹。真正用于基准轨迹的groundtruth存放在oxts目录下结构是这样的oxts/ data/ 0000000000.txt 0000000001.txt ... dataformat.txt timestamps.txtdata目录里每个txt文件对应一帧组合导航数据每一行是一串空格分隔的浮点数总共有28个字段。第一次打开dataformat.txt的时候感觉被信息淹没了其实整理一下就清楚了前3个是经纬度高程中间是姿态角后面是速度、加速度、角速度和定位状态。字段位置和含义大致如下表完整说明以官方dataformat.txt为准字段序号含义单位1-3纬度lat、经度lon、海拔alt度、米4-6横滚roll、俯仰pitch、偏航yaw弧度7-9前向vf、左侧vl、向上vu速度m/s10-15车辆坐标系与传感器坐标系的加速度分量m/s²16-21角速度分量rad/s22-23位置精度、速度精度米、m/s24-28导航状态、卫星数、定位模式等-这里有个容易忽略的点oxts的坐标系定义是前-左-上x轴指向车头方向y轴指向车辆左侧z轴指向上方。后文所有坐标变换都要围绕这个定义展开很多转换结果对不上的原因就是这里从一开始就搞错了。1.2 注意区分raw data和Odometry数据集的groundtruth很多人会把KITTI raw data和KITTI Odometry的groundtruth混为一谈。Odometry数据集也就是00-10序列里直接提供了pose/00.txt这类文件每行12个数可以直接拼成3x4变换矩阵那个就是左目相机在世界坐标系下的绝对位姿非常干净。但Odometry数据集不提供原始传感器数据也没有GPS/IMU的完整观测时间戳只有一个简单的times.txt只有相对秒数没有绝对UTC时间。而raw data的优势在于传感器种类齐全、时间戳完整包含100Hz的oxts数据和10Hz的相机/LiDAR数据。代价就是groundtruth需要你自己从oxts里“挖”出来并做坐标变换。印象里很多教程只讲Odometry数据集的pose文件怎么读很少讲raw data这条线。如果你需要用到视频帧、点云和真实GPS轨迹的对应关系或者想评估前端在原始传感器数据上的表现就必须把raw data这套流程搞清楚。2. 时间戳的处理从UTC字符串到能直接计算的浮点秒2.1 时间戳文件与sync机制oxts/timestamps.txt的格式长这样2011-09-26 13:02:54.997343637 2011-09-26 13:02:55.014351424 2011-09-26 13:02:55.031359211这个时间是以UTC格式保存的精确到纳秒频率为100Hz。注意这里有个细节如果使用_sync后缀的数据oxts/timestamps.txt已经被降采样到10Hz并与相机、激光雷达对齐了如果使用raw data不带_sync那么oxts是100Hz原始频率需要自己和相机时间戳对齐。实际做SLAM评估我一般直接用_sync数据省去对齐的麻烦。但如果你需要做高精度时间插值那就是另外一个话题了。对于转TUM格式我们只需要_sync数据里的10Hz时间戳因为TUM格式本身不强制要求时间连续均匀。2.2 精确转换为Unix时间戳TUM格式的第一列需要一个浮点秒常见的是Unix时间戳。直接拿Python的datetime.strptime解析会出问题因为KITTI时间戳的纳秒部分是9位数字而普通%f只处理微秒精度时间戳第6位之后会被截断。虽然对于视觉SLAM的10Hz数据来说微秒级误差几乎不影响评估但从严谨角度还是推荐手动解析。我实际使用的解析方法是这样from datetime import datetime, timezone def kitti_timestamp_to_unix(ts_str: str) - float: # 拆出秒和纳秒部分 base_str, nano_str ts_str.strip().split(.) base_dt datetime.strptime(base_str, %Y-%m-%d %H:%M:%S) # 视为UTC时间不转本地时区 base_ts base_dt.replace(tzinfotimezone.utc).timestamp() # 纳秒部分可能需要左补零到9位这里按实际位数处理 nano int(nano_str.ljust(9, 0)) return base_ts nano / 1e9有一个细节值得注意datetime.strptime解析出来的base_dt不带时区信息因此必须显式指定tzinfotimezone.utc再调用timestamp()否则Python会按照系统本地时区解释这个时间导致偏移几个小时的错误。我在切换电脑时踩过这个坑换了一台配置了UTC时区的机器后轨迹时间整体偏移8小时评估工具根本对不上。2.3 TUM格式对时间戳的具体要求TUM格式的每行结构是timestamp tx ty tz qx qy qz qw其中timestamp的单位是秒一般用Unix浮点时间TUM官方RGB-D数据集用的是类似1305031453.724000这种格式。KITTI的UTC时间戳转成浮点秒后直接作为第一列即可。很多人会在这一步犯迷糊到底该用相对时间还是绝对时间我的建议是统一用绝对Unix时间戳。原因是evo、rpg_trajectory_evaluation这些工具都支持自动时间对齐绝对时间戳能避免自己手动做相对时间偏移而且后续如果要融合其他传感器数据绝对时间戳更不容易乱。当然如果你只需要相对位姿序列做SLAM评估用第一帧归零的相对时间也完全可以关键是保持一致。3. 完整实操把oxts转换成TUM格式位姿3.1 坐标系关系与变换链先理清坐标系变换链这是整个转换过程最核心的部分也是出错率最高的地方。KITTI raw data涉及的坐标系主要有世界坐标系一般取第一帧IMU位置为原点IMU坐标系前-左-上Velodyne激光雷达坐标系前-左-上相机坐标系以cam0为例X向右Y向下Z向前从oxts数据直接计算得到的是IMU在世界坐标系下的位姿。而我们在SLAM评估里通常需要的是相机在世界坐标系下的位姿。所以至少需要一次坐标系变换T_cam_world T_cam_imu * T_imu_world其中T_imu_world由oxts的经纬度和姿态角计算T_cam_imu来自标定文件。KITTI raw data的标定文件有calib_imu_to_velo.txt和calib_velo_to_cam.txt两者相乘就能得到T_cam_imu。从文件内容看calib_velo_to_cam.txt给出的是T_cam_velo所以T_cam_imu T_cam_velo * T_velo_imu而T_velo_imu就是calib_imu_to_velo.txt里的变换矩阵。注意不要搞反方向calib_imu_to_velo.txt里直接给的是从IMU到Velodyne的变换所以它就是T_velo_imu不需要再求逆。读标定文件时官方给的是R3x3矩阵按行存储和平移T3x1向量。我习惯把它们拼成4x4齐次矩阵再参与运算。3.2 经纬度转局部平面坐标从oxts里的lat/lon/alt到世界坐标系的平移量最常用的做法是投影到UTM坐标系然后以第一帧为原点做差分。KITTI官方devkit里用的就是这个思路。用pyproj实现非常简洁import pyproj import numpy as np # 根据KITTI序列所在区域选择UTM zone卡尔斯鲁厄大约在zone 32 transformer pyproj.Transformer.from_crs(EPSG:4326, EPSG:32632, always_xyTrue) def latlon_to_utm(lat, lon): easting, northing transformer.transform(lon, lat) return easting, northing以第一帧为原点之后每一帧的平移量为x easting - easting0 y northing - northing0 z alt - alt0这里有个细节这里的x对应东向y对应北向z对应高度。但IMU坐标系是前-左-上当车行进方向不一定朝东时直接用这个(x,y,z)作为位姿平移其实已经是“世界坐标系”下的坐标了。后文构建的旋转矩阵要保证和这个平移匹配。换句话说(x,y,z)描述的是IMU在世界坐标系ENU下的位置姿态四元数描述的是IMU相对于世界坐标系的旋转两者之间不需要再做额外旋转。如果不想引入UTM的zone依赖也可以做一个等距近似x (lon - lon0) * np.pi / 180.0 * R * np.cos(np.radians(lat0)) y (lat - lat0) * np.pi / 180.0 * R z alt - alt0对于KITTI数据这种几公里范围的短序列两种方法差不了太多。不过既然有现成工具我建议直接用UTM省心也便于扩展。3.3 姿态角转四元数并组合变换矩阵oxts给的是roll、pitch、yaw。KITTI官方在这块的定义是旋转按ZYX顺序也就是先绕Z轴转yaw再绕Y轴转pitch最后绕X轴转roll。构建旋转矩阵时用def oxts_to_rotation_matrix(roll, pitch, yaw): Rx np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]]) Ry np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]]) Rz np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]]) return Rz Ry Rx然后把这个旋转矩阵转成四元数。手写转换容易出错我一般直接用scipy.spatial.transform.Rotationfrom scipy.spatial.transform import Rotation R_imu_world oxts_to_rotation_matrix(roll, pitch, yaw) q_imu_world Rotation.from_matrix(R_imu_world).as_quat() # 返回 [x, y, z, w]注意scipy返回的是[x, y, z, w]TUM格式也是qx qy qz qw正好对应上。组合起来就是IMU在世界坐标系下的4x4变换矩阵T_imu_world np.eye(4) T_imu_world[:3, :3] R_imu_world T_imu_world[:3, 3] [x, y, z]如果只需要IMU位姿到这里其实就可以输出TUM了。但大多数人想评估的是相机轨迹所以继续往下做相机变换。3.4 输出TUM格式文件最终代码框架我整理成一个可运行的脚本流程import numpy as np from scipy.spatial.transform import Rotation # 伪代码循环读取oxts和timestamps with open(output.tum, w) as f: for i in range(len(oxts_data)): # 1. 从oxts读取经纬度、姿态角 # 2. 经纬度转局部坐标 (x, y, z) # 3. 姿态角转旋转矩阵再转四元数 # 4. 组装 T_imu_world # 5. 如果有标定 T_cam_world T_cam_imu T_imu_world # 6. 从 T_cam_world 提取平移和四元数 # 7. 写入timestamp x y z qx qy qz qw pass这里的输出文件可以直接被evo读取。比较典型的评估命令是evo_ape tum groundtruth.tum estimated.tum -a -s其中-a表示进行Umeyama轨迹对齐-s表示尺度对齐适用于单目SLAM这类有尺度不确定性的情况。如果估计轨迹已经解决了尺度问题就不需要-s。4. 实战中容易踩的坑与排查方法4.1 标定参数是相对哪个相机的KITTI raw data有4个相机cam0和cam1是灰度相机cam2和cam3是彩色相机。通常做视觉SLAM用的是左侧灰度相机cam0而Odometry数据集的groundtruth对应的也是cam0。但calib_velo_to_cam.txt里只有一个相机变换具体对应哪一个需要结合calib_cam_to_cam.txt来看。我的经验是如果只想快速得到能用的相机位姿直接用calib_velo_to_cam.txt的变换给cam0即可因为KITTI官方的preprocessing pipeline中cam0是基准相机激光雷达标定就是针对cam0做的。如果发现输出的轨迹在方向上比图像内容偏了90度多半是T_cam_imu构造不对。排查方法很简单取第一帧把相机坐标系的原点投影到图像中心附近再看相机Z轴是不是指向场景前方。只要姿态大致符合图像观测就说明旋转方向是对的。4.2 时间戳精度丢失导致轨迹对不上最开始我直接用datetime.strptime(ts, %Y-%m-%d %H:%M:%S.%f)解析KITTI时间戳结果在纳秒位被截断本以为是小事但用evo做时间对齐时发现偶尔出现误匹配。后来改成手动解析纳秒并补零到9位问题就消失了。另外一个时间戳相关的坑是时区问题。KITTI的时间戳是UTC时间如果系统时区是东八区base_dt.timestamp()会自动转成本地时间再计算秒数导致整体时间戳偏移8小时。排查时如果发现时间戳差值总是一个固定的小时数优先检查时区。4.3 轨迹方向、尺度对不上如果你转换出来的轨迹和官方Odometry的groundtruth放在一起看形状很像但方向相反多半是坐标系手性问题。IMU坐标系是x前y左z上相机坐标系是x右y下z前如果不做T_cam_imu变换轨迹会呈现“镜像”效果。尺度对不上则更常见。由于UTM坐标和ENU坐标都是米制理论上不应该有尺度问题。如果你发现轨迹形状一致但整体被放大了检查一下lat/lon到UTM的投影是不是用了错误的zone或者经纬度转弧度的公式漏掉了np.pi/180系数。排查这类问题我推荐一个简单办法把第一帧位姿设为原点第二帧位姿打印出来对照IMU实际朝向和大概几米的位移一眼就能看出坐标和旋转有没有问题。4.4 用哪个频率的oxts数据如果使用不带_sync的raw dataoxts频率是100Hz而相机只有10Hz。直接把100Hz的IMU位姿全部输出为TUM文件虽然看起来更平滑但实际评估SLAM轨迹时估计轨迹是10Hzgroundtruth是100Hz时间对齐时更多的匹配候选反而会增加误匹配概率。所以我建议统一用_sync数据此时oxts已经降采样到10Hz并且和相机帧对齐。如果你的传感器融合方案需要更高频率的IMU那再单独处理raw oxts记得不要把100Hz的轨迹直接和10Hz的视觉轨迹混在一个TUM文件里。5. 最后再分享一个小技巧我在转完TUM格式之后通常还会顺手做一步验证用evo把groundtruth轨迹画出来看它是不是贴合对应的车道走向。KITTI的raw data大多采集于卡尔斯鲁厄城区轨迹形状应该是有规律的沿街转弯如果画出来是一团乱麻或者朝某个方向飞出去那基本就是坐标或旋转矩阵出了问题没必要继续往下跑评估流程。另外有一个容易踩但很好解决的细节多个序列的oxts第一帧坐标各不相同如果要把多个序列放在同一个世界坐标系下比较记得不要把每个序列都独立归零到各自第一帧而是保留绝对UTM坐标或者用每个序列的GPS参考点统一变换。否则跨序列对比误差会非常难看。总的来说把KITTI raw data的groundtruth转成TUM格式并不是一个多复杂的工程但涉及时间解析、坐标系变换、四元数约定这些细节每一步都可能埋雷。希望你看了这篇之后能少走一些我走过的弯路一次跑通。本文还有配套的精品资源点击获取
