Apollo自动驾驶创建点云地图流程分析

前言

Apollo为了实现点云与惯性导航的融合定位这个最终的功能，开放了一系列的工具，包括点云数据过滤，与GNSS的时间对齐，生成点云地图，使用点云地图与Ins惯性导航系统做融合定位，可视化定位效果，以及对最终的定位结果的评估等等这一些列的工具套件，从整体上来看，这些工具可以分为两个大类别，一是创建点云地图，二是使用点云地图与Ins(惯性导航系统)做融合定位。

创建点云地图

下图描述了创建点云地图的主要流程

创建点云地图是进行点云融合定位的第一步，作为先验性的地图信息参考。
要想创建一个点云地图，需要两个重要的信息，一个是点云信息，另一个是定位信息。
点云信息，来自激光LiDAR，是对环境扫描的点云信息， 包括XYZIT（X,Y,Z,强度,时间戳）， 是相对于车身的相对坐标。
定位信息，来自全球定位导航系统(GNSS)，GNSS融合了GPS+RTK+IMU 的数据，能够精准定位（精度在10cm以内）， 包括Position（位置），Orientation(姿态)和Vel（速度）信息，在创建点云地图时只需要Position和Orientation 信息。
最终点云地图中的每一个点，是包含了准确的定位信息的点，所以，点云地图实际上就是对点云数据与定位数据的一个融合的结果。
接下来我们就来深入的探讨一下，这个融合过程是怎么实现的。

数据采集与解析

首先我们来看数据提取与解析。

    输入数据：使用Cyber_record 录制的数据包
    输出数据： 
		LiDAR 数据
		odometry 数据

要创建某个区域的地图，首先需要对这个区域进行数据采集，主要采集LiDAR和GNSS数据，可以是用cyber_record库对这两个topic消息进行解析，对于LiDAR点云数据，我们需要使用PCL库将每一帧的点云数据其转换成PCD文件并保存，同时也要记录每一帧的索引文件，每行记录的内容是：index（pcd索引值）, timestamp(时间戳), 保存到索引文件pcd_timestamp.txt。
对于Odometry定位数据，也同样提取一份索引文件，没行内容为： index(位置索引), timestamp（时间戳）,位置(
x, y, z) , 姿态( qx, qy, qz, qw) ,方差( std_x, std_y, std_z) , 并保存至文件odometry_loc.txt。
此外，在此次解析过程中，还有三个输出，分别是gnss_loc.txt， lidar_loc.txt和fusion_loc.txt， 这三个文件分别保存融合后的数据输出，所以，在第一次创建时，都为空。
有了pcd_timestamp.txt 和odometry_loc.txt 这两个索引文件以及所有的点云pcd数据，就可以做点云与GNSS时间对齐了。

点云数据与GNSS定位融合

点云数据是由若干帧组成的，我们的激光雷达采集频率是10Hz，即每秒10帧，每一帧是由若干个离散的点组成，目前每一帧原始点大概10万个点左右。为了能够方便有效的对这些点进行计算，通常采用pcd格式的文件对点云数据进行存储，每一帧存储为一个pcd文件。
而在算法层面，知行者中，对于点云的处理主要依赖于PCL库，这个库提供了大部分公开的点云处理算法，在融合过程中使用的关键性的算法，均出自PCL库。
地理位置信息和点云数据是相对独立的，要想把它们融合在一起，就需要找到它们的关联点，唯一能把它们关联起来的，只有时间戳，莫急，我们一步一步往下看。
时间对齐与坐标转换插值

输入： 
    GNSS定位数据索引文件
    点云pcd数据
    激光雷达外参
输出： 
    时间对齐后的位置

GNSS