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-34. 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::Ptrcv::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 ni0; nivstrImageLeft.size(); ni) { // 图像处理 cv::Mat imLeft cv::imread(vstrImageLeft[ni], cv::IMREAD_UNCHANGED); cv::Mat imRight cv::imread(vstrImageRight[ni], cv::IMREAD_UNCHANGED); // IMU数据处理 vectorORB_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_del1,:),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(in_del,:),4,3); 0 0 0 1]; temp Tinv * Ti; T_total(i,:) reshape(temp(1:3,1:4),1,12); end5.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激励在平坦路段初始化可能会失败尺度一致性长时间运行后可能出现轻微的尺度漂移闭环检测在重复场景中可能出现误匹配建议多次运行取平均结果并对不同序列分别评估以获得更全面的性能分析。
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-34. 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::Ptrcv::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 ni0; nivstrImageLeft.size(); ni) { // 图像处理 cv::Mat imLeft cv::imread(vstrImageLeft[ni], cv::IMREAD_UNCHANGED); cv::Mat imRight cv::imread(vstrImageRight[ni], cv::IMREAD_UNCHANGED); // IMU数据处理 vectorORB_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_del1,:),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(in_del,:),4,3); 0 0 0 1]; temp Tinv * Ti; T_total(i,:) reshape(temp(1:3,1:4),1,12); end5.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激励在平坦路段初始化可能会失败尺度一致性长时间运行后可能出现轻微的尺度漂移闭环检测在重复场景中可能出现误匹配建议多次运行取平均结果并对不同序列分别评估以获得更全面的性能分析。
