如何从KITTI数据集GPS数据生成视觉里程计真值位姿？

问题 #

KITTI视觉里程计任务序列提供真值位姿，但未包含IMU数据，仅原始数据序列带有含IMU信息的oxts数据。我尝试将oxts中的GPS数据转换为ENU坐标系生成相对轨迹，用来和VO真值位姿对比验证，但转换后的轨迹与真值无法对齐，求解决办法。

解决方案 #

1. 严格对齐坐标系转换基准点 #

• 必须以序列第一帧的经纬度海拔作为ENU坐标系的原点，和KITTI VO真值的相对位姿基准保持一致，不能随意选择其他点。
• 转换需严格遵循WGS84椭球参数完成经纬度到ECEF（地心地固坐标系）的转换，再转ENU，核心公式如下：
  1. 计算地球曲率半径：

     N = a / sqrt(1 - e² * sin²(lat_rad))
     

     其中a=6378137m（WGS84长半轴），e²=0.00669437999014（偏心率平方）
  2. 计算ECEF坐标：

     X = (N + alt) * cos(lat_rad) * cos(lon_rad)
     Y = (N + alt) * cos(lat_rad) * sin(lon_rad)
     Z = (N*(1-e²) + alt) * sin(lat_rad)
     

  3. 从ECEF转ENU，以第一帧ECEF坐标为原点，旋转矩阵基于第一帧经纬度：

     R = [ -sin(lon0)   cos(lon0)        0      ]
         [ -sin(lat0)*cos(lon0) -sin(lat0)*sin(lon0) cos(lat0) ]
         [  cos(lat0)*cos(lon0)  cos(lat0)*sin(lon0) sin(lat0) ]
     

     ENU坐标 = R * (当前ECEF坐标 - 原点ECEF坐标)

2. 完成帧与时间戳的同步匹配 #

• KITTI VO序列帧和原始oxts数据的时间戳并非严格一一对应，需做时间戳插值匹配：
  • 提取VO序列每帧的时间戳（来自times.txt）、oxts每帧的时间戳（来自文件命名）
  • 对每个VO帧，找到相邻的两个oxts帧，通过线性插值得到对应时刻的GPS/IMU数据，保证每帧VO都有对应的ENU位姿。

3. 修正航向角的坐标系定义差异 #

• KITTI oxts的yaw角定义为0指向东，逆时针为正，但VO真值的航向是0指向车辆前方（X轴），逆时针为正，且VO坐标系为X向前、Y向左、Z向上。
• 需将oxts的yaw转换为VO坐标系下的航向：先将oxts yaw转为北向为0的航向（yaw_north = pi/2 - yaw_oxts），再结合roll和pitch构建从ENU到VO车身坐标系的旋转矩阵，确保姿态与VO真值坐标系一致。

4. 处理GPS噪声与轨迹平滑 #

• oxts中的GPS原始数据存在噪声，直接转换的轨迹会有抖动，可通过滑动窗口滤波或结合oxts内的IMU数据（ax/ay/az、wx/wy/wz）做预积分，平滑轨迹后再与VO真值对比。
• KITTI VO真值是视觉SLAM优化后的平滑结果，GPS原始数据的波动会导致对齐偏差，平滑后再进行对比更合理。

5. 用官方工具验证转换正确性 #

• 可自行实现或使用KITTI官方的oxts2pose工具生成轨迹，与VO真值的poses.txt对比，用KITTI的evaluate_odometry工具计算ATE/RPE，确认转换后的轨迹误差在合理范围内。

内容的提问来源于stack exchange，提问作者Roy Amoyal