ORB-SLAM3适配KITTI数据集详细操作

前言

由于ORBSLAM3没有提供KITTI双目+IMU的适配，因此需要自己适配。这里主要是自己做个整理，参考了如下文章：

SLAM学习日志：ORBSLAM3(VIO/IMU)适配KITTI数据集，并评估！！！奶妈级教程！！！_orbslam3 yolo kitti-CSDN博客

ORB_SLAM2、ORB_SLAM3跑EuRoc&KITTI数据集及evo评估_stereo_inertial_euroc.cc_KleineFive的博客-CSDN博客

1、KITTI数据集下载

odometry：标定图像calib、彩色图像color、灰度图像gray、轨迹真值poses、激光数据velodyne；
raw：相机、激光、IMU的原始数据。分sync和extract两种，sync是已对齐和矫正的数据，它的IMU数据为10HZ，extract未对齐未校正的原始数据 ，它的IMU数据为100HZ。
因此需要raw中的IMU数据与odometry的gray/color和poses对齐。以下是odometry和raw的对应关系（来自KITTI开发工具包devkit_odometry的readme.txt文件）：

Nr.     Sequence name     Start   End
---------------------------------------
00: 2011_10_03_drive_0027 000000 004540
01: 2011_10_03_drive_0042 000000 001100
02: 2011_10_03_drive_0034 000000 004660
03: 2011_09_26_drive_0067 000000 000800
04: 2011_09_30_drive_0016 000000 000270
05: 2011_09_30_drive_0018 000000 002760
06: 2011_09_30_drive_0020 000000 001100
07: 2011_09_30_drive_0027 000000 001100
08: 2011_09_30_drive_0028 001100 005170
09: 2011_09_30_drive_0033 000000 001590
10: 2011_09_30_drive_0034 000000 001200

以下以06序列为例，首先在官网The KITTI Vision Benchmark Suite的odometry标签下下载22GB的gray数据和4MB的groundtruth，然后根据上面的对应关系在raw data标签的Residential下下载2011_09_30_drive_0020的extract和sync数据（即unsynced和synced） 。这里建议设置好各个数据的路径，以方便使用。

2、ORB-SLAM3配置

首先要编写stereo_inertial_kitti文件，可参考stereo_inertial_kitti.cc_orbslam3跑kitti资源-CSDN文库

并在CMakeLists.txt文件加入可执行：

add_executable(stereo_inertial_kitti
        Examples/Stereo-Inertial/stereo_inertial_kitti.cc)
target_link_libraries(stereo_inertial_kitti ${PROJECT_NAME})

kitti格式无时间戳，需输出文件与真值文件行数完全相同，加了IMU之后的结果文件由于开始无imu数据导致结果行数与真值结果行数不一致，无法在evo中进行比较，因此需将stereo_inertial_kitti中的SaveTrajectoryKITTI函数更换为SaveTrajectoryTum,使最终结果保存为tum格式，方便后续评估。

同时还要将真值(groundtruth)文件也改为TUM格式，这里是将06序列下的06.txt和times.txt文件复制到evo/contrib下，然后在该文件夹下执行：

python3 kitti_poses_and_timestamps_to_trajectory.py 06.txt times.txt kitti_06_tum.txt

得到kitti_06_tum.txt文件后面使用。

此外，需要计算IMU外参并添加至yaml配置文件，KITTI提供的是IMU到激光和激光到相机的外参，可以通过matlab计算：

%imu外参
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];
Rvi=reshape(Rvi,3,3)';
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];
Rcv=reshape(Rcv,3,3)';
tcv=[-7.137748e-03 -7.482656e-02 -3.336324e-01]';
Tcv=[Rcv tcv;0 0 0 1];
Tci=Tcv*Tvi;
Tic=inv(Tci);

Tic就是T_b_c1，将计算的结果加入KITTI04-12.yaml中，要加入的同时还有IMU噪声和随机游走：

#--------------------------------------------------------------------------------------------
# IMU Parameters
#--------------------------------------------------------------------------------------------
# Transformation from camera 0 to body-frame (imu)
IMU.T_b_c1: !!opencv-matrix
  rows: 4
  cols: 4
  dt: f
  data: [0.00781297503487685,-0.00427919893727802,0.999960328830788,1.14389870575543,
        -0.999859471889100,-0.0148680470554297,0.00774856027467805,-0.312718471250709,
        0.0148342962199183,-0.999880225756999,-0.00439476150137299,0.726546045493979,
        0.0,0.0,0.0,1.000000000000000]

IMU.GyroWalk: 0.003491
IMU.AccWalk: 5.0000e-3
IMU.NoiseGyro: 1.6968e-04
IMU.NoiseAcc: 2.0000e-3
IMU.Frequency: 100.0

3、时间对齐

因为图像数据是10HZ的，通常1帧图像需要10次IMU记录，即IMU数据需要100HZ，所以要将2011_09_30_drive_0020_extract的IMU数据与odometry-06的图像数据通过同一个格式的时间联系起来，即把extract中的IMU时间减去sync中图像的第一帧时间。

具体做法是将extract的oxts/timestamps.txt文件导入matlab，注意输出格式选为列向量，导入格式为日期时间，自定义为：yyyy-MM-dd HH:mm:ss.SSSSSSSSS，然后进行如下处理：

%imu和图像数据对齐，timestampsextract为导入的extract的IMU数据
t1(:,1)=posixtime(timestampsextract(:,1));
%这里的时间为sync中图像的第一帧时间
timestampsextract(:,2)=datetime('2011-09-30 12:08:26.403042304');
t2(:,1)=posixtime(timestampsextract(:,2));
extract_vec(:,1)=t1(:,1)-t2(:,1);
%检查变量，将负数的时间设为零
negative_num=1;
extract_vec(1:negative_num,1)= 0.0;
filename = "times_imu100hz_06.txt";
save(filename,"extract_vec","-ascii");

得到times_imu100hz_06.txt文件

4、运行ORB-SLAM3

首先别忘了重新./build.sh，运行前确定好各个文件的路径，然后运行ORB-SLAM3视觉-惯性模式：

./Examples/Stereo-Inertial/stereo_inertial_kitti ./Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/KITTI04-12.yaml ../Dataset/KITTI/kitti-odometry/data_odometry_gray/dataset/sequences/06 ../Dataset/KITTI/kitti-raw/Residential/2011_09_30/2011_09_30_drive_0020_extract/oxts/data ../Dataset/KITTI/kitti-odometry/data_odometry_gray/dataset/sequences/06/times_imu100hz_06.txt

此外，也运行纯视觉模式作为参照：

./Examples/Stereo/stereo_kitti ./Vocabulary/ORBvoc.txt ./Examples/Stereo/KITTI04-12.yaml ../Dataset/KITTI/kitti-odometry/data_odometry_gray/dataset/sequences/06

5、进行evo评估

进行evo评估：

evo_traj tum --ref=kitti_06_tum.txt traj_kitti_v_tum.txt traj_kitti_vi_tum.txt -p --plot_mode=xz -a

根据轨迹来看，直线行驶的时候视觉惯性在有些地方要比纯视觉好一点，但视觉惯性在转弯和回环的时候表现一般，具体的分析以后再做。