ORBSLAM3实战：手把手教你将KITTI数据集适配VIO/IMU模式，并完成精度评估-尧图网络科技

1. KITTI数据集与ORBSLAM3适配背景

最近在做一个车载视觉惯性里程计（VIO）的项目，需要评估ORBSLAM3在KITTI数据集上的表现。但实际操作中发现，ORBSLAM3官方并没有直接支持KITTI数据集的VIO/IMU模式评估。这让我不得不自己动手解决这个问题，经过几天的摸索和调试，终于成功实现了适配。下面就把我的完整解决方案分享给大家，希望能帮助到有同样需求的朋友。

KITTI数据集是自动驾驶领域最常用的基准数据集之一，包含了丰富的传感器数据。但它的IMU数据格式与ORBSLAM3要求的格式存在差异，需要进行专门的处理。具体来说，KITTI提供了两种数据格式：odometry和raw。odometry数据已经过标定和对齐，但IMU采样率只有10Hz；而raw数据中的extract版本保留了原始100Hz的IMU数据，但需要我们自己进行时间对齐。

2. 数据准备与对齐

2.1 数据集下载与结构分析

首先需要从KITTI官网下载以下数据：

odometry数据（包含标定文件、彩色图像和轨迹真值）
raw数据中的sync和extract版本（包含IMU原始数据）

以07序列为例，odometry的07序列对应raw数据中的2011_09_30_drive_0027数据包。这个对应关系可以在KITTI开发工具包devkit_odometry的readme.txt文件中找到。

2.2 时间对齐处理

由于图像数据是10Hz，而我们需要100Hz的IMU数据，因此要使用extract版本的IMU数据。关键是要将IMU时间戳与图像时间戳对齐。具体步骤如下：

从sync数据中获取第一帧图像的时间戳t0
对extract中的每个IMU数据点，将其时间戳减去t0
确保IMU数据与图像数据的时间对应关系正确

这里有个需要注意的地方：00序列的IMU数据存在几处约1秒的数据缺失，使用时需要特别注意。

3. IMU参数配置

3.1 外参计算

ORBSLAM3需要相机到IMU的外参矩阵，而KITTI提供的是IMU到激光雷达和激光雷达到相机的外参。我们可以通过以下MATLAB代码计算所需的外参：

Rvi = [9.999976e-01 7.553071e-04 -2.035826e-03 -7.854027e-04 9.998898e-01 -1.482298e-02 2.024406e-03 1.482454e-02 9.998881e-01]; tvi = [-8.086759e-01 3.195559e-01 -7.997231e-01]'; Tvi = [Rvi tvi; 0 0 0 1]; Rcv = [7.027555e-03 -9.999753e-01 2.599616e-05 -2.254837e-03 -4.184312e-05 -9.999975e-01 9.999728e-01 7.027479e-03 -2.255075e-03]; tcv = [-7.137748e-03 -7.482656e-02 -3.336324e-01]'; Tcv = [Rcv tcv; 0 0 0 1]; Tci = Tcv * Tvi; % 相机到IMU的变换矩阵

3.2 IMU噪声参数设置

KITTI使用的是oxts R3003 IMU，参考其技术手册可以获取以下参数：

陀螺仪随机游走：0.003491
加速度计随机游走：5.0000e-3

由于技术手册没有提供噪声参数，我们可以参考EuRoC数据集的设置：

陀螺仪噪声：1.6968e-04
加速度计噪声：2.0000e-3

4. ORBSLAM3代码适配

4.1 数据读取实现

我们需要创建一个新的数据读取类，可以参考ORBSLAM3自带的stereo_kitti.cc和stereo_inertial_euroc.cc实现。主要需要完成以下功能：

读取KITTI图像序列
读取并解析IMU数据
实现时间戳同步
加载相机和IMU参数

关键代码结构如下：

class ImageGrabber { public: ImageGrabber(ORB_SLAM3::System* pSLAM):mpSLAM(pSLAM){} void GrabImage(const sensor_msgs::ImageConstPtr& msg); void GrabImu(const sensor_msgs::ImuConstPtr& imu_msg); ORB_SLAM3::System* mpSLAM; bool mbClahe; cv::Ptr<cv::CLAHE> mClahe = cv::createCLAHE(3.0, cv::Size(8, 8)); }; int main(int argc, char **argv) { // 初始化ORB-SLAM3系统 ORB_SLAM3::System SLAM(argv[1], argv[2], ORB_SLAM3::System::IMU_STEREO, true); // 创建图像和IMU抓取器 ImageGrabber igb(&SLAM); // 设置KITTI数据集路径 string pathToSequence = argv[3]; // 加载图像和IMU数据 LoadImages(pathToSequence, vstrImageLeft, vstrImageRight, vTimestamps); LoadIMU(pathToSequence, vImuMeas, vTimestampsImu); // 主循环处理数据 for(size_t ni=0; ni<vstrImageLeft.size(); ni++) { // 图像处理 cv::Mat imLeft = cv::imread(vstrImageLeft[ni], cv::IMREAD_UNCHANGED); cv::Mat imRight = cv::imread(vstrImageRight[ni], cv::IMREAD_UNCHANGED); // IMU数据处理 vector<ORB_SLAM3::IMU::Point> vImuMeas; while(vTimestampsImu[imuPointer]<=vTimestamps[ni]) { vImuMeas.push_back(ORB_SLAM3::IMU::Point(...)); imuPointer++; } // 跟踪 SLAM.TrackStereo(imLeft, imRight, vTimestamps[ni], vImuMeas); } // 关闭系统 SLAM.Shutdown(); return 0; }

4.2 参数文件配置

需要创建一个YAML配置文件，包含以下关键参数：

%YAML:1.0 # Camera parameters Camera.type: "PinHole" Camera1.fx: 718.856 Camera1.fy: 718.856 Camera1.cx: 607.1928 Camera1.cy: 185.2157 Camera1.k1: 0.0 Camera1.k2: 0.0 Camera1.p1: 0.0 Camera1.p2: 0.0 Camera2.fx: 718.856 Camera2.fy: 718.856 Camera2.cx: 607.1928 Camera2.cy: 185.2157 Camera2.k1: 0.0 Camera2.k2: 0.0 Camera2.p1: 0.0 Camera2.p2: 0.0 # IMU parameters IMU.NoiseGyro: 1.6968e-04 IMU.NoiseAcc: 2.0000e-03 IMU.GyroWalk: 3.491e-03 IMU.AccWalk: 5.0000e-03 IMU.frequency: 100.0 # Extrinsic parameters Tbc: !!opencv-matrix rows: 4 cols: 4 dt: f data: [ 0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, 0.0, 0.0, 0.0, 1.0 ]

5. 精度评估与结果分析

5.1 轨迹对齐处理

由于ORBSLAM3的VIO需要IMU有足够的加速度才会开始初始化，导致输出的轨迹比真值少了开头的若干帧。我们需要对真值轨迹进行相应的裁剪和对齐。可以使用以下MATLAB代码处理：

poseOri = load('poses.txt'); [m,n] = size(poseOri); n_del = 23; % 根据实际情况调整 Tinv = [reshape(poseOri(n_del+1,:),4,3)'; 0 0 0 1]; R = Tinv(1:3,1:3); for i = 1:10 R = 0.5 * (inv(R') + R); % 正交化处理 end R = R'; t = Tinv(1:3,4); tnew = -R * t; Tinv = [R tnew; 0 0 0 1]; for i = 1:m-n_del Ti = [reshape(poseOri(i+n_del,:),4,3)'; 0 0 0 1]; temp = Tinv * Ti; T_total(i,:) = reshape(temp(1:3,1:4)',1,12); end

5.2 使用evo工具评估

安装evo工具后，可以使用以下命令进行评估：

evo_ape kitti poses.txt traj_kitti.txt -p --plot --save_results results/ORBSLAM3_KITTI.zip

评估指标包括：

绝对位姿误差(APE)
相对位姿误差(RPE)
轨迹长度
速度误差等

5.3 典型结果分析

在实际测试中，ORBSLAM3在KITTI数据集上表现良好，但有几个需要注意的地方：

初始化阶段：由于需要足够的IMU激励，在平坦路段初始化可能会失败
尺度一致性：长时间运行后可能出现轻微的尺度漂移
闭环检测：在重复场景中可能出现误匹配

建议多次运行取平均结果，并对不同序列分别评估以获得更全面的性能分析。

资讯详情