CN110009741B - 一种在Unity中生成环境点云地图的方法 - Google Patents

Abstract

本发明本提供一种在Unity中生成环境点云地图的方法，包括以下步骤：初始化将雷达点云分隔成若干个方块体素，对每一个体素进行体素滤波降低数据密度，将体素滤波后生成的所有点云数据变换到世界坐标系下的对应位置，将坐标变换后的点云数据直接读取复制字节数据，保存在输出缓冲区，从而生成点云地图。本发明能够在Unity中根据自定义场景制作出精准点云地图的流程，不但生成的地图数据体积较小，而且也降低了定位匹配的难度。

Description

一种在Unity中生成环境点云地图的方法

技术领域

本发明一种计算机制作精准LiDAR云地图的方法，属于自动驾驶仿真技术领域。

背景技术

自动驾驶汽车的正常运行需要依靠高精地图、实时定位以及障碍物检测等多项技术，很多人都有这样的疑问：如果有了精准的GPS，不就知道了当前的位置，还需要定位吗，其实不然。目前高精度的军用差分GPS在静态的时候确实可以在“理想”的环境下达到厘米级的精度。这里的“理想”环境是指大气中没有过多的悬浮介质而且测量时GPS有较强的接收信号。然而自动驾驶是在复杂的动态环境中行驶，尤其在大城市中，由于各种高大建筑物的阻拦，GPS多路径反射（Multi-Path）的问题会更加明显。这样得到的GPS定位信息很容易就有几十厘米甚至几米的误差。对于在有限宽度上高速行驶的汽车来说，这样的误差很有可能导致交通事故。因此必须要有GPS之外的手段来增强无人车定位的精度。

激光雷达（LiDAR）是一种向指定方向发射激光束并检测返回光束来获取目标位置等数据的装置。自动驾驶领域常用的激光雷达为多线束机械旋转式激光雷达，由于其定位精准，算法实现难度低，不受昼夜影响等诸多优点，成为自动驾驶核心定位与检测设备之一。

利用LiDAR定位的常用方法之一，就是用当前的LiDAR数据匹配场景中的点云数据，如果找到一致则可推断车身的位置与朝向。但由于机械旋转式LiDAR的激光发射单元并不是静止不动的。在无人车行驶的过程中，LiDAR同时以一定的角速度匀速转动，在这个过程中不断地发出激光并收集反射点的信息，以便得到全方位的环境信息。LiDAR在收集反射点距离的过程中也会同时记录下该点发生的时间和水平角度（Azimuth），并且每个激光发射器都有编号和固定的垂直角度，根据这些数据我们就可以计算出所有反射点的坐标。LiDAR每旋转一周收集到的所有反射点坐标的集合就形成了点云。

点云地图是由众多的点云拼接而成，目前点云地图的绘制也是通过LiDAR完成的，安装LiDAR的地图数据采集车在想要绘制点云地图的路线上多次反复行驶并收集点云数据。后期经过人工标注，过滤一些点云图中的错误信息，例如由路上行驶的汽车和行人反射所形成的点，然后再对多次收集到的点云进行对齐拼接形成最终的点云地图。但现有的点云地图生成方法不但成本较高，计算繁琐，而且后期需要人工干预，生成的时间也较长。

发明内容

发明目的：为了克服现有点云地图生成方法所存在的不足，结合Unity引擎的优势，本发明提供一种在Unity引擎中高效生成点云地图的方法。

技术方案：为解决上述技术问题，本发明提供的点云地图生成方法，包括以下步骤：

步骤一、初始化

将雷达点云分隔成若干个方块体素，体素尺寸为voxelSize；

步骤二、对每一个体素进行体素滤波

1）创建NativeMultiHashMap<int3, float4> a，用于存放体素的点索引与点数据；

2）建立IJobParallelFor格式的并行流程，向a中填入点索引与点数据，伪代码如下：

a.add((int3) math.floor(data[index].xyz * voxelSize),data[index]);

其中math.floor()为向下取整函数，data为雷达点云的数据数组，index为点索引；

3）创建NativeArray<float4> b，用于顺序填充体素中索引一致的点；

4）创建NativeArray<int> c，用于记录相同点合并的次数；

5）建立IJobNativeMultiHashMapMergedSharedKeyIndices格式的并行流程，将体素中的点云进行合并，伪代码如下：

ExecuteFirst:b[index] = data[index];

c[index] = 1;

ExecuteNext:b[index]=b[index](c[index]-1)/c[index]+data[index]/c[index];

c[index]++;

6）创建NativeList<int> d，用于存放筛选后的合并过的点索引；

7）建立IJobParallelForFilter格式的筛选方法，并用ScheduleAppend过程填充到d中，伪代码如下：

Return c[index] > 0;

8）创建NativeArray<float4> e，用于存放输出结果；

9）建立IJobParallelFor过程，根据d中索引在b中查找对应元素放入e对应位置，伪代码如下：

e[index] = b[d[index]]；

步骤三、坐标变换

建立IJobParallelFor过程，将体素滤波后生成的所有点云数据变换到世界坐标系下的对应位置；

步骤四、合并帧数据，生成点云地图

将坐标变换后的点云数据直接读取复制字节数据，保存在输出缓冲区，从而生成点云地图。

本发明直接采用Lidar的数据生成地图，避免了在Lidar无法扫到的区域也生成点云的问题，不但生成点云地图高效精准，而且使最终生成的地图数据体积减小，也降低了定位匹配的难度。本发明充分利用了Unity引擎提供的各种多线程处理任务的并行接口，从而实现高性能体验，它不仅能改善帧率，而且特别适合用来处理多个需要长时间运行的任务。

除以上所述的本发明解决的技术问题、构成技术方案的技术特征以及由这些技术方案的技术特征所带来的优点外。为使本发明目的、技术方案和有益效果更加清楚，下面将结合本发明实施例中的附图，对本发明所能解决的其他技术问题、技术方案中包含的其他技术特征以及这些技术特征带来的优点做更为清楚、完整的描述。

附图说明

图1是本发明实施例的流程图；

图2是体素滤波的原理示意图；

图3是本发明实施例中某场景图像数据；

图4是对应图3场景的点云地图数据。

具体实施方式

实施例：

Unity作为通用图形引擎，内部集成了第三方物理引擎，能够满足自动驾驶领域的物理与图像仿真需求。得益于这些优势，我们可以在2018以上版本的Unity中实现高效获取激光雷达点云的新方法，其实施总流程如图1所示，包括以下步骤：

步骤一、初始化。

将图3所示场景的雷达点云分隔成若干个方块体素，体素尺寸为voxelSize；

步骤二、对每一个体素进行体素滤波

激光雷达原始数据每一帧数据量在数万点以上，而且这只是低端设备的数目，且每秒至少10帧，处理这些数据需要相当大算力，进行体素滤波，降低数据密度是很有必要的。

体素滤波原理如图2所示，是在空间中划分一系列方块（体素Voxel），将同在一个方块中的点合并到一个中心点的过程。本实施例中体素滤波的方法具体如下：

1）创建NativeMultiHashMap<int3, float4> a，用于存放体素的点索引与点数据，a的预定义容量为点的数目；

2）建立IJobParallelFor格式的并行流程，向a中填入点索引与点数据，伪代码如下：

a.add((int3) math.floor(data[index].xyz * voxelSize),data[index]);

其中math.floor()为向下取整函数，data为雷达点云的数据数组，index为点索引；

3）创建NativeArray<float4> b，用于顺序填充体素中索引一致的点，长度为点数目；

4）创建NativeArray<int> c，用于记录相同点合并的次数；

5）建立IJobNativeMultiHashMapMergedSharedKeyIndices格式的并行流程，将体素中的点云进行合并，伪代码如下：

ExecuteFirst:b[index] = data[index];

c[index] = 1;

ExecuteNext:b[index]=b[index](c[index]-1)/c[index]+data[index]/c[index];

c[index]++;

由于该过程对NativeMultiHashMap的处理底层将根据相同Key在相同线程顺序执行的原则进行，经过这一步骤后，b中会有合并过的点与大量空位存在（即被合并掉的点的位置）。

6）创建NativeList<int> d，用于存放筛选后的合并过的点索引，容量为点数目；

7）建立IJobParallelForFilter格式的筛选方法，并用ScheduleAppend过程填充到d中，伪代码如下：

Return c[index] > 0;

8）创建NativeArray<float4> e，用于存放输出结果，长度为d长度；

9）建立IJobParallelFor过程，根据d中索引在b中查找对应元素放入e对应位置，伪代码如下：

e[index] = b[d[index]]。

步骤三、坐标变换

建立IJobParallelFor过程，将体素滤波后生成的所有点云数据变换到世界坐标系下的对应位置。

伪代码如下：

globalPoint[index] = math.rotate(lidarRotation, localPoint[index].xyz) + lidarPosition;

步骤四、合并帧数据，生成点云地图

将坐标变换后的点云数据直接读取复制字节数据，保存在输出缓冲区，从而生成点云地图，如图4所示。具体实施时，可使用UnmanagedMemoryStream类进行拷贝。

另外，生成点云地图后，可以采用以下方法将点云地图导出：建立一个新文件，先写入PCD格式文本头，再将缓冲区的数据写入该文件，即可得到点云的PCD文件。

本发明为自动驾驶仿真领域的环境点云地图生成提供了一种全新的思路与方法，具体实现该技术方案的方法和途径很多，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，对附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

Claims (2)

1.一种在Unity中生成环境点云地图的方法，包括以下步骤：

步骤一、初始化

将雷达点云分隔成若干个方块体素，体素尺寸为voxelSize；

步骤二、对每一个体素进行体素滤波

1）创建NativeMultiHashMap<int3, float4> a，用于存放体素的点索引与点数据；

2）建立IJobParallelFor格式的并行流程，向a中填入点索引与点数据，伪代码如下：

a.add((int3) math.floor(data[index].xyz * voxelSize),data[index]);

其中math.floor()为向下取整函数，data为雷达点云的数据数组，index为点索引；

3）创建NativeArray<float4> b，用于顺序填充体素中索引一致的点；

4）创建NativeArray<int> c，用于记录相同点合并的次数；

5）建立IJobNativeMultiHashMapMergedSharedKeyIndices格式的并行流程，将体素中的点云进行合并，伪代码如下：

ExecuteFirst:b[index] = data[index];

c[index] = 1;

ExecuteNext:b[index]=b[index](c[index]-1)/c[index]+data[index]/c[index];

c[index]++;

其中，ExecuteFirst是指对b[index]的第一个元素执行的操作，ExecuteNext是指对b[index]除第一个元素以外的其他元素执行的操作；

6）创建NativeList<int> d，用于存放筛选后的合并过的点索引；

7）建立IJobParallelForFilter格式的筛选方法，并用ScheduleAppend过程填充到d中，伪代码如下：

Return c[index] > 0;

8）创建NativeArray<float4> e，用于存放输出结果；

9）建立IJobParallelFor过程，根据d中索引在b中查找对应元素放入e对应位置，伪代码如下：

e[index] = b[d[index]]；

步骤三、坐标变换

建立IJobParallelFor过程，将体素滤波后生成的所有点云数据变换到世界坐标系下的对应位置；

步骤四、合并帧数据，生成点云地图

将坐标变换后的点云数据直接读取复制字节数据，保存在输出缓冲区，从而生成点云地图；

步骤五、建立一个新文件，先写入PCD格式文本头，再将缓冲区的数据写入该文件，即可得到点云的PCD文件。

2.根据权利要求1的在Unity中生成环境点云地图的方法，其特征在于：步骤四中，使用UnmanagedMemoryStream类进行拷贝将坐标变换后的点云数据直接读取复制字节数据，保存在输出缓冲区。