这几天想用ORBSLAM3评估一下车载环境VIO的精度，但是ORBSLAM3没有适配KITTI数据集（你们懂的~）。
然后在网上搜了一下还没搜到，只好自己来适配。在此记录一下的走过的辛酸历程，希望能给大家带来帮助。
——————————分割线———————————
1.KITTI数据格式KITTI提供两种数据：
odometry：标定图像calib、彩色图像color、灰度图像gray、轨迹真值poses、激光数据velodyne；
raw：相机、激光、IMU的原始数据。分sync和extract两种，sync是已对齐和矫正的数据，它的IMU数据为10HZ，extract未对齐未校正的原始数据 ，它的IMU数据为100HZ。
做VIO的精度评估肯定需要轨迹真值、图像序列、IMU数据。所以需要raw中的IMU数据与odometry的lolor和poses对齐。
以odometry的07序列为例，其对应raw里的2011_09_30_drive_0027数据包，这个对应关系来自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
因为图像数据是10HZ的，通常1帧图像需要10次IMU记录，即IMU数据需要100HZ，所以我们的目标是将2011_09_30_drive_0027_extract的IMU数据与odometry-07的图像数据通过同一个格式的时间联系起来，具体做法就是把extract中的IMU时间减去sync中图像的第一帧时间。
PS：00序列对应rawdata的imu数据出现了几次持续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];
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
其次是IMU噪声和随机游走，KITTI使用的IMU型号为oxts R3003，参考oxts官网的技术手册可知：
IMU.GyroWalk: 0.003491
IMU.AccWalk: 5.0000e-3
技术手册没提供噪声，用EuRoC的噪声代替：
IMU.NoiseGyro: 1.6968e-04
IMU.NoiseAcc: 2.0000e-3
读KITTI数据其实很简单参照stereo_kitti.cc和stereo_inertial_euroc.cc就可以了，代码占篇幅有点大就不贴了，懒癌重症患者请移步：
stereo_inertial_kitti.cc下载
评估工具：evo
因为OBRSLAM3的VIO需要IMU由足够的加速度才会开始，所以跑出来的轨迹会少了刚开始的几帧，需要把真值的前n-1帧删除再同时左乘inv(Tn)。用MATLAB计算：
poseOri=load('D: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
然后用evo评估：
evo_ape kitti poses.txt traj_kitti.txt -p
Tcv=[Rcv tcv;0 0 0 1];
Tci=Tcv*Tvi
其次是IMU噪声和随机游走，KITTI使用的IMU型号为oxts R3003，参考oxts官网的技术手册可知：
IMU.GyroWalk: 0.003491
IMU.AccWalk: 5.0000e-3
技术手册没提供噪声，用EuRoC的噪声代替：
IMU.NoiseGyro: 1.6968e-04
IMU.NoiseAcc: 2.0000e-3 4.ORBSLAM3读取KITTI数据的代码实现
读KITTI数据其实很简单参照stereo_kitti.cc和stereo_inertial_euroc.cc就可以了，代码占篇幅有点大就不贴了，懒癌重症患者请移步：
stereo_inertial_kitti.cc下载
评估工具：evo
因为OBRSLAM3的VIO需要IMU由足够的加速度才会开始，所以跑出来的轨迹会少了刚开始的几帧，需要把真值的前n-1帧删除再同时左乘inv(Tn)。用MATLAB计算：
poseOri=load('D: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
然后用evo评估：
evo_ape kitti poses.txt traj_kitti.txt -p