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定位信息的更新频率是100Hz，点云的更新频率是10Hz，显然不在一个频段上。也就是说，GNSS在各个时刻与点云的各个时刻是没有重合点的，要想获取激光雷达在某一时刻的位置，必须先要将激光雷达与定位系统做时间对齐，找到激光雷达距离定位系统最近的时间点，在通过激光雷达附近的定位点做插值计算，最终得到激光点云所在的位置信息。

我们先来看下准备工作：

	遍历 odometry_loc.txt 定位文件{
		对Pos(x,y,z)做座标平移+旋转变换得到仿射矩阵
		获取GNSS位置的时间戳作为对齐参考时间 和 方差
 		std::vector<Eigen::Affine3d>* poses
		...
		Eigen::Translation3d trans(Eigen::Vector3d(x, y, z));
      		Eigen::Quaterniond quat(qr, qx, qy, qz);
      		poses->push_back(trans * quat);
      		timestamps→push_back(timestamp);

 		Eigen::Vector3d std;
      		std << std_x, std_y, std_z;
      		stds->push_back(std);
	}

这段简单的处理无疑是必要的准备，它可以得到任意时刻每个点odometry(车所在的位置)的仿射变换矩阵，那么，之后我们只要获取对应时刻的点云局部座标值，乘以这一时刻的仿射矩阵，就可以把点云座标转换到WGS84座标系下，OK:-)

具体到算法上主要有以下几个步骤，

一是, 在座标平移的方向根据时间戳做线性插值，即找到距离第一帧点云数据最近并且在点云时间之前的定位信息时间，也就是保证第一帧pcd的时间在两个连续的pos时间之间, 得到线性插值系数 delta_t = (t – t0)/(t1-t0),  
(
输入位置： odometry， 
输入位置时间戳: odometry pos timestamps
参考时间戳：pcd的时间戳,
double t = (cur_timestamp - ref_timestamp) / (cur_timestamp – pre_timestamp)
)
根据线性插值系数delta_t , 即可算出座标平移x,y,z三个方向上的座标值，

        Eigen::Translation3d re_transd;
        re_transd.x() = pre_transd.x() * t + cur_transd.x() * (1 - t);
        re_transd.y() = pre_transd.y() * t + cur_transd.y() * (1 - t);
        re_transd.z() = pre_transd.z() * t + cur_transd.z() * (1 - t);

二是，在座标旋转方向上做插值计算，为了实现平滑插值的效果，使用的是四元数球面线性插值(slerp)算法, 具体是 
Eigen::Quaterniond res_quatd = pre_quatd.slerp(1 - t, cur_quatd); 

函数来实现的，

out_poses->push_back(re_transd * res_quatd);
…
Eigen::Affine3d pose_tem = out_poses_[i] * velodyne_extrinsic_;

通过时间对齐之后，对点云的每一帧进行座标平移、旋转插值变换，再补上激光雷达的外参，就得到了每一帧点云的pcd在某一时刻的基础仿射变换矩阵，之后就可以从pcd中获取当前帧的每一点的位姿信息，乘以这个放射转换矩阵，最终将点云座标转换到车的全局座标系下(WGS84)。
做好每一帧点云时间对齐之后的索引数据保存至corrected_poses.txt 文件中，内容包括： 帧索引(indexes), 帧时间戳( timestamp), 位置座标(transd.x() transd.y() transd.z()), 姿态四元数( qx qy qz qr )。

创建点云地图

至此，我们准备好了以下几个数据：
点云的每一帧的pcd文件
点云帧索引文件corrected_poses.txt，包含了每一帧索引和当前帧的精确base定位信息
激光雷达的外参

有了这两个数据，我们现在就可以创建地图了。
分为无损地图和有损地图两个部分。

创建无损地图

输入：
		pcd文件路径
		点云索引文件
输出： 
		无损地图配置文件
		无损地图

总结起来，创建地图也只有两个关键的动作
一是点云滤波
二是点云特征平面分割

点云滤波，使用的是体素网格滤波(VoxelGrid)，简单说来的就是将每一帧的点云空间划分成若干个小空间，每个小空间包含了一定数量的点，最终计算出这些点的重心点来代表这个小空间，其他的点被过滤掉，从而实现滤波的作用。此处使用的具体算法是PCL::VoxelGrid算法。
特征平面分割，知行者中，采用的是采样一致(Ransac)算法，通过提取点云特征平面信息，并结合点云的平均高程信息，最终确认点云的地面特征，至此，根据过滤后的结果可以保存此地图数据，为无损地图(lossless map)。

对每一帧做过滤波之后的点云数据被放入到无损地图队列中，等待保存处理。

那么点云地图又是怎么存储这些点云数据的呢，实际上就是做了一个空间上的映射，
首先点云地图使用Node节点来作为管理单元，可以根据当前需要创建点云的总数来创建Node节点总数，然后根据每个点的座标值来计算Node的索引值，这样就建立起点云与存储之间的映射关系了。

最后每个点云数据存放在 LosslessMapMatrix 节点中，但此时还没完，因为对于水平位置x,y和高程z是分开来处理的，高程值z与点云强度值是按照给定的阈值范围内分层次来保存的，也就是说不同范围的高程数据保存在点云地图的不同层上。

而对于有损地图，就是在每个点云地图的Node Cell中，获取平均高程和灰度的方差值来代表当前cell的数据。

至此，点云的有损地图创建完成，也就代表点云地图的创建完成。