SLAM_ORB-SLAM3(VIO/IMU)适配KITTI数据集，并评估！！！奶妈级教程！！！

柔光的暖阳◎ 2023-01-15 09:27 299阅读 0赞

这几天想用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对齐。

# 2.数据对齐 #

以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秒左右的缺失。

# 3.IMU参数 #

首先是外参，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

# 4.ORBSLAM3读取KITTI数据的代码实现 #

读KITTI数据其实很简单参照stereo\_kitti.cc和stereo\_inertial\_euroc.cc就可以了，代码占篇幅有点大就不贴了，懒癌重症患者请移步：  
stereo\_inertial\_kitti.cc下载

# 5.精度评估 #

评估工具：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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70][]![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 1][]

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20210422102334496.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ==,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 1]: /images/20221022/47ef4136cccc4694bdd8c8ca99edbe19.png