无人驾驶之KITTI数据集介绍与应用（二）——官方matlab代码使用与解读

本系列博客旨在介绍无人驾驶领域中颇负盛名的KITTI公开数据集，首先整体介绍该数据集的由来、数据组织方式、官方开发工具的使用，重点详细介绍其中对于Object、Tracking和raw data的数据使用，主要分享了我在使用这些数据集时开发的一些工具，也介绍了Gihub上一些开源工具的使用，帮助大家在没有真实无人驾驶车辆时如何利用好KITTI数据集来实现和验证自己的算法。

【文章列表】
无人驾驶之KITTI数据集介绍与应用（一）——数据组织方式介绍
无人驾驶之KITTI数据集介绍与应用（二）——官方matlab代码使用与解读

上一篇博文主要简单介绍了整个KITTI数据集的采集平台和数据集文件组织方式，为了能够更好的使用这个数据集，KITTI官方也提供了C++和MATLAB的一些开发工具和数据集必要说明，以devkit名命名，其中的readme.txt则包含了对相应数据集的解释说明，本博文就来详细介绍一下不同数据集的文件说明和开发代码的简单使用。

1. raw data开发工具及解释

raw data数据的开发工具可以从该网站进行下载，下载解压之后，你会得到如下所示文件及文件夹：

其中cpp文件夹中主要放置了tracklets.h头文件，用来解析raw data中部分提供了tracklet_labels.xml标签文件的数据，matlab文件夹下就是主要的开发工具了，包括对KITTI数据集的载入、解析以及丰富的可视化工具，readme.txt需要详细阅读，这个文件详细讲述了数据集的组织结构和使用方法。

这一小节主要就简单解释一下readme.txt中部分内容，然后利用提供的matlab工具进行一些可视化操作。

1.1 readme文件解读

1.1.1 数据同步

在实际使用KITTI raw data数据集时，我们一般使用同步和矫正的数据 （即synced+rectified data），在所有采集的传感器设备中，激光雷达的采集频率最低（约10Hz），因此最终同步出来的所有其他传感器数据（相机、GPS、IMU）频率也都大约为10Hz；此外，相机图像原始尺寸为1382x512，经过矫正之后图像尺寸变成了1242x375，相机的视场角大约为90度。

1.1.2 激光雷达数据

为了节约存储空间，每一帧激光雷达扫描结果都使用如下代码被保存为.bin格式的二进制文件：

stream = fopen (dst_file.c_str(),"wb");
fwrite(data,sizeof(float),4*num,stream);
fclose(stream);

保存的数据中包括每一个点的x,y,z和I四个数据，其中I表示反射强度，从.bin文件中读取所有点云代码如下：

  // allocate 4 MB buffer (only ~130*4*4 KB are needed)
  int32_t num = 1000000;
  float *data = (float*)malloc(num*sizeof(float));

  // pointers
  float *px = data+0;
  float *py = data+1;
  float *pz = data+2;
  float *pr = data+3;

  // load point cloud
  FILE *stream;
  stream = fopen (currFilenameBinary.c_str(),"rb");
  num = fread(data,sizeof(float),num,stream)/4;
  for (int32_t i=0; i<num; i++) {
    point_cloud.points.push_back(tPoint(*px,*py,*pz,*pr));
    px+=4; py+=4; pz+=4; pr+=4;
  }
  fclose(stream);

这个代码为我们在实际代码中读取这些bin文件提供了重要参考，后面我会进行详细介绍。

1.1.3 GPS/IMU位姿数据

主要提供以下30个数值：
- lat: latitude of the oxts-unit (deg)
- lon: longitude of the oxts-unit (deg)
- alt: altitude of the oxts-unit (m)
- roll: roll angle (rad), 0 = level, positive = left side up (-pi…pi)
- pitch: pitch angle (rad), 0 = level, positive = front down (-pi/2…pi/2)
- yaw: heading (rad), 0 = east, positive = counter clockwise (-pi…pi)
- vn: velocity towards north (m/s)
- ve: velocity towards east (m/s)
- vf: forward velocity, i.e. parallel to earth-surface (m/s)
- vl: leftward velocity, i.e. parallel to earth-surface (m/s)
- vu: upward velocity, i.e. perpendicular to earth-surface (m/s)
- ax: acceleration in x, i.e. in direction of vehicle front (m/s^2)
- ay: acceleration in y, i.e. in direction of vehicle left (m/s^2)
- az: acceleration in z, i.e. in direction of vehicle top (m/s^2)
- af: forward acceleration (m/s^2)
- al: leftward acceleration (m/s^2)
- au: upward acceleration (m/s^2)
- wx: angular rate around x (rad/s)
- wy: angular rate around y (rad/s)
- wz: angular rate around z (rad/s)
- wf: angular rate around forward axis (rad/s)
- wl: angular rate around leftward axis (rad/s)
- wu: angular rate around upward axis (rad/s)
- posacc: velocity accuracy (north/east in m)
- velacc: velocity accuracy (north/east in m/s)
- navstat: navigation status
- numsats: number of satellites tracked by primary GPS receiver
- posmode: position mode of primary GPS receiver
- velmode: velocity mode of primary GPS receiver
- orimode: orientation mode of primary GPS receiver
可以利用matlab程序中的oxts = loadOxtsliteData('2011_xx_xx_drive_xxxx')来解析GPS/IMU数据，然后利用pose = convertOxtsToPose(oxts)程序将oxts数据转换为米制单位位姿，用一个4x4的刚体变换矩阵表示。

1.1.4 传感器标定

这部分内容对于想要利用KITTI中多个传感器数据进行传感器融合相关算法开发至关重要。
raw data数据集中提供了三个标定文件，分别是：
calib_cam_to_cam.txt——相机到相机之间的标定：
-S_xx：表示图像矫正前的尺寸——宽 高
-K_xx：表示相机xx图像矫正前的标定矩阵，3x3大小
-D_xx：矫正前相机xx的1x5畸变向量
-R_xx：相机xx的3x3旋转矩阵（相机外参）
-T_xx：相机xx的3x1平移向量（相机外参）
-S_rect_xx：矫正后相机xx图像尺寸
-R_rect_xx：3x3的矫正旋转矩阵用来使图像平面共面
-P_rect_xx：矫正后3x4的投影矩阵（表示将矫正后相机00下坐标转到相机xx下图像像素的投影矩阵）
注意： 在使用同步矫正数据集时，我们一般只会用到P_rect_xx矩阵和R_rect_00矩阵。（其实有关这些矫正矩阵的具体含义，我也不是很清楚，如果有更加熟悉的人，可以留言评论，多谢！）

calib_velo_to_cam.txt——激光雷达到相机之间的对准：
-R：3x3的旋转矩阵
-T：3x1的平移向量
R|T共同组成一个刚体变换矩阵，用来将激光雷达坐标系下的一个点转换到左侧灰度相机（即cam00）坐标系下。在开发激光雷达和相机融合相关算法时会用到。

calib_img_to_velo.txt——GPS/IMU到激光雷达之间的对准：
-R：3x3的旋转矩阵
-T：3x1的平移向量

1.1.5 转换示例

此处示例如何利用上述标定结果将激光雷达坐标系下点云转换到相机图像中。
假设激光雷达坐标系下一个点的齐次坐标为$X=[X,Y,Z,1{]}^{\prime }$，其对应到相机xx图像上的像素点齐次坐标为$Y=[u,v,1]$，则转换关系式如下公式所示：

Y = P_rect_xx * R_rect_00 * (R|T)_velo_to_cam * X

其中矩阵的含义为：

• P_rect_xx (3x4): rectfied cam 0 coordinates -> image plane
• R_rect_00 (4x4): cam 0 coordinates -> rectified cam 0 coord.
• (R|T)_velo_to_cam (4x4): velodyne coordinates -> cam 0 coordinates

1.1.6 Tracklet Labels

对于部分raw data中数据，KITTI对其中出现在相机视野范围内的有效目标（主要是Car、Van、Truck、Pedestrian、Person、Cyclist、Tram等）进行了人工标注，可以作为其中目标的检测或跟踪的真值。

每一个tracklet保存了目标三维包围盒的高度、宽度和长度以及目标在激光雷达坐标系下的旋转和平移量等信息，结果保存为一个xml格式文件，可以用提供的cpp/tracklets.h文件进行解析：

#include "tracklets.h"
Tracklets *tracklets = new Tracklets();
if (!tracklets->loadFromFile(filename.xml))
   <throw an error>
<do something with the tracklets>
 delete tracklets;

1.2 matlab代码使用示例

通过1.1节中对readme.txt文件进行详细解释，我们应该大概了解了KITTI中raw data数据中的各文件的含义和作用。KITTI官方还提供了有用的matlab代码，可以用来解析数据集进行可视化。

1.2.1 run_demoVelodyne.m

该文件用来示例如何将激光雷达点云投影到相机图像上实现传感器融合的目的。

首先使用Matlab（我使用的是Matlab 2014b版本）打开run_demoVelodyne.m文件，修改你下载好的KITTI raw data数据集所在路径，例如我修改的结果是：

然后运行该程序，得到如下结果：

从图可以看出，程序将一帧完整的在相机视野范围内的点云通过KITTI提供的标定矩阵，投影到相机图像上了，从而为某些图像像素赋予了点云深度信息。投影结果中不同颜色表示点云的不同距离，颜色越红，表示目标离雷达坐标系原点越近。

1.2.2 run_demoTracklets.m

该文件用来示例如何读取并解析tracklets文件获得标注目标三维信息，并将这些三维信息结果投影到相机图像上，形成2D/3D包围框。

同样，首先打开run_demoTracklets.m文件，修改其中表示数据集路径的变量，如图：

然后运行代码，得到结果：

注：所有有关Matlab代码的具体解释此处不再呈现了，大家可以阅读其中的注释和源码进行理解，后续我会给出我在使用C++代码对KITTI数据集进行解析利用的代码，供大家参考使用。