一种基于多线激光雷达点云的多尺度静态地图构建方法与流程

1.本发明属于无人系统定位建图领域，具体涉及一种基于多线激光雷达点云的多尺度静态地图构建系统


背景技术：

2.随着人工智能与机器人技术的不断发展与进步，应用场景不断丰富与多样化，特别是无人驾驶的快速商业化，服务机器人的快速产业化以及三维环境重建等应用，对相关技术有了更加严格的要求，包括地图的高准确度以及高可用性。地图的建立和无人系统自身的定位是相辅相成的，而slam(simultaneous localization and mapping)技术则是目前实现这一目标的有效途径，slam技术经过漫长的发展，从早期的依赖于单线激光雷达并采用滤波类算法定位的二维地图建立，再到如今通过多线激光雷达和图优化定位的三维地图建立。无人系统感知的是动态与静态结合的现实环境，所建立的地图也不免会有动态物体诸如移动的行人和车辆，经过slam系统所生成的环境地图，最终会在静态地图中留下一条没有意义的轨迹，点云图中的动态对象点被渲染为点云图中的“重影”轨迹，通常会重叠并遮盖道路标记，交通标志和其他重要静态特征的视图，很难从中提取静态特征。建立的地图的最终目的是能够帮助人或机器进行下一步的理解和利用环境，而动态环境则是需要实时感知的。因此滤除动态目标重影轨迹点云，并重新构建静态地图是十分必要的。
3.环境感知的所依赖的传感器，主要有激光雷达和视觉相机，激光雷达作为无人系统重要的环境感知传感器，一直发挥着不可替代的作用，它可以高频率的精确扫描环境而不受光线的影响，同时拥有探测距离远可靠性高等特点，所得到的点云相对于深度相机，具有强度信息而没有颜色信息。而视觉相机特别包含深度信息的深度相机、tof相机、双目相机，优点是成本低同时能够善于捕捉场景的纹理，进而有效的完成目标识别语义分割等任务；但是感知的深度信息十分有限，都对光线敏感，难以在黑暗等光线收到严重干扰的场景中有效的进行环境感知。
4.三维激光slam系统首先对环境感知的原始数据进行处理消除运动畸变，对imu数据进行预积分，通过icp帧间匹配算法依赖原始插帧的位姿进行前端匹配，根据位姿和原始点计算的目标点，迭代当前帧的全部目标点，进行初步帧间点云的配准。后端利用图优化使得当前帧位姿变换后的目标点与下一帧相应点的总代价最小化，从而得到最优位姿和最佳匹配地图。尽管通过后端优化能够尽可能的减少误差，但随着一帧一帧的误差不断累积，误差逐步被放大，利用回环时的闭环特性进行二次配准则能够有效的消除累积误差，建立精确地图。但动态目标的运动轨迹仍不可避免的保留在了静态地图中。
5.现有的动态目标去除包括在线去除和离线去除，在线去除在slam系统中植入动态去除方法，显然极大的增加了运算复杂度，降低了slam的实时性，与slam轻量化的发展方向不符，同时难以保持高精度去除。离线去除则是根据slam系统得到的最终精确位姿和地图进行地图后处理。包括基于点云分割的动态物体去除，通过深度学习使得该方法依赖于人工标注的训练集；基于动态目标检测的动态物体去除则首先对每帧点云进行3d目标检测而
后重建静态地图。
6.当前技术存在的缺点如下：
7.1、使用原始的slam系统，则完全不考虑动态目标，在扫描并建立地图的过程中，移动的行人和车辆等动态物体会随着帧间累积，在地图中形成重影轨迹点云，严重影响点云地图的进一步利用，这是建立静态地图不需要的冗余信息，对于人机对陌生环境的认知存在干扰，甚至遮挡了有用的静态信息。
8.2、嵌入slam的在线动态去除算法会使得slam实时性下降，影响slam定位建图效率
9.3、采用语义分割和3d目标检测的动态去除方法一般使用深度学习无法保证全场景的泛化性
10.因此，本领域研究人员致力于设计一种新的静态地图构建系统，并克服上述缺陷。


技术实现要素：

11.有鉴于现有技术的上述缺陷，本发明的目的是提供一种基于多线激光雷达点云的多尺度静态地图构建系统。本发明通过利用多线激光雷达所采集的雷达点云数据和激光slam系统输出的点云帧所对应的位姿，进行采样和投影，并对投影图像进行数字图像处理，最终分离动态目标点与静态目标点，同时利用图像金字塔序列对错误标记的动态目标点进行还原，最终得到误差最小化的静态点云地图，完成多尺度静态点云地图的构建。具有运算速度快、动态目标去除精度高、最大程度还原静态地图的特点。
12.相关术语解释如下：
13.多线激光雷达：区别于传统的单线激光雷达，即发射一条激光扫描线；多线激光雷达有16线、32线、64线等多种规格，发射多条激光扫描线，因此能够感知三维环境。
14.点云：一种逻辑数据结构，描述为点的集合，每个点则包括x、y、z坐标以及激光强度等信息
15.图像金字塔：图像金字塔一种以多分辨率来解释图像的有效但概念简单的结构，也是图像多尺度表达的一种，一幅图像的金字塔是一系列以金字塔形状排列的分辨率逐步降低，且来源于同一张原始图的图像集合。其通过梯次向下采样获得，直到达到某个终止条件才停止采样。
16.slam：(simultaneous localizationand mapping)同步定位与建图，一种利用激光雷达或相机作为绝对位姿传感器，利用imu或轮式里程计作为相对位姿传感器；通过位姿和地图相互优化相互弥补从而实现同时完成定位与地图建立的技术。
17.icp：(iterative closest point)迭代最近点算法，分别在带匹配的目标点云p和源点云q中，按照一定的约束条件，找到最邻近点(pi，qi)，然后计算出最优匹配参数r和t，使得误差函数最小。误差函数为e(r，t)为：其中n为最邻近点对的个数，pi为目标点云p中的一点，qi为源点云q中与pi对应的最近点，r为旋转矩阵，t为平移向量。
18.kdtree：一种基于树的树形数据结构，能够利用该结构实现对点云的高效查找，并找到距离某点距离阈值为x以内的全部点。
19.octree：八叉树，一种基于树的数据结构，八叉树结构通过对三维空间的几何实体
进行体元剖分，每个体元具有相同的时间和空间复杂度，通过循环递归的划分方法对大小为(2 nx 2 n x 2 n)的三维空间的几何对象进行剖分，从而构成一个具有根节点的方向图。
20.本发明的技术方案：
21.一种基于多线激光雷达点云的多尺度静态地图构建系统，其特征在于：包括如下步骤：
22.步骤1、利用多线激光雷达获取道路行驶数据输出每一帧的带有时间戳的激光点云，在工控机下通过三维激光slam输出每帧的姿态数据。
23.其中，预设的参数有：
24.v
image_vfov
为二维图像的预设方向视场
25.v
image_hfov
为二维图像的水平方向视场
26.d
downsample_size
为局部帧与全局地图的点云下采样参数
27.s
multi_resolution_list
为图像金字塔的各个预设尺度序列
28.n
parallel_cores
为三维点云投影至二维图像的并发线程数
29.r
keyframe_range
关键帧的选取范围
30.m
extrinsic_lidar
雷达的外参矩阵
31.步骤2、动态目标去除系统按照时间戳读入用户指定范围内的激光点云帧，提取关键帧以及相应关键帧的姿态数据。
32.步骤3、为加速计算同时提高内存利用率，对关键帧的点云进行降采样处理。
33.步骤4、在全局坐标系下，利用每帧点云的姿态信息，将全部关键帧点云拼接成原始地图。
34.步骤5、建立多尺度下的图像金字塔，迭代金字塔的每一层图像。
35.步骤6、针对每一层图像，将局部坐标系下的每一帧关键帧三维点云按照指定尺度投影至指定大小的二维图像。
36.步骤7、在像素级粒度下，计算两帧图像的差与实验得出的阈值进行比较，从而确定动态目标的位置。
37.步骤8、将相应二维图像中动态目标的位置映射回三维点云中点云的索引，对动态点与静态点分别进行标记并提取。
38.步骤9、将标记为动态点的点云子集作为图像金字塔下一次迭代的全部点云，循环往复，直至遍历图像金子塔的全部尺度，将动态点的错误识别率降至最低，从而构建精准的静态点云地图。
39.进一步的是，步骤1中，所述激光雷达位于机器人或车辆的正中心上方，每一帧激光点云都包括多条激光线扫描后接收到的强度信息以及相对于激光接收器的相对笛卡尔坐标。所使用的slam系统可以是任意以多线激光雷达数据和九轴imu数据为输入，能够输出每帧激光点云对应位姿的三维多线激光slam系统。
40.进一步的是，步骤2包括：
41.步骤2.1、根据存储了激光点云扫描帧的已经持久化的文件路径读取或订阅实时扫描到的异步通信的消息队列scanlist，按照预设参数r
keyframe_range
读取关键帧存入工控机内存。
42.步骤2.2、读取激光slam系统输出的各个激光点云扫描帧所对应的经过迭代优化的位姿估计，该位姿是雷达的局部坐标系下的位姿，得到雷达坐标系到当前位姿的变换矩阵集合p
direct
。
43.步骤2.3、对2.2中读取的全部位姿进行求逆得到当前位姿到雷达坐标系的变换矩阵集合p
reverse
44.进一步的是，步骤3包括：
45.对于步骤2中输入的每一帧激光点云扫描帧，根据预设的d
downsample_size
下采样参数，通过在点云数据中创建三维体素栅格，然后用每个体素的重心来近似表达体素中的其它点，进而实现对关键帧的降采样处理，便于后续处理速度，同时减少内存占用。最终得到下采样后的关键帧集合s
local_after
46.进一步的是，步骤4包括：
47.步骤4.1、从步骤2读入的关键帧点云和位姿中，根据索引依次提取对应的关键帧s
local_after
和关键帧对应的位姿t
direct
48.步骤4.2、使用外参变换矩阵，将s
local_after
从雷达坐标系变换到主体baselink坐标系得到s
local_after_then
49.步骤4.3、使用4.1中读取的t
direct
将位姿从主体baselink坐标系变换到全局坐标系下得到s
global_i
50.步骤4.4、根据索引，通过迭代将全部的s
global_i
进行拼接，得到原始的全局地图p
global
51.步骤4.5、根据预设的下采样参数d
downsample_size
对原始全局地图p
global
进行下采样，此时地图过于大，点云数据庞大，不适合用步骤3中的传统体素栅格进行下采样，而应该使用更复杂的数据结构八叉树，通过基于八叉树的体素滤波实现降采样，最终得到下采样后的全局地图p
global_after
52.进一步的是，步骤5包括：
53.图像金字塔是图像中多尺度表达的一种，最主要用于图像的分割，是一种以多分辨率来解释图像的有效但概念简单的结构。图像金字塔最初用于机器视觉和图像压缩，一幅图像的金字塔是一系列以金字塔形状排列的分辨率逐步降低，且来源于同一张原始图的图像集合。其通过梯次向下采样获得，直到达到某个终止条件才停止采样。金字塔的底部是待处理图像的高分辨率表示，而顶部是低分辨率的近似。将一层一层的图像比喻成金字塔，层级越高，则图像越小，分辨率越低
54.根据预设的s
multi_resolution_list
，该预设的参数取决于雷达的分辨率，体现在激多线光雷达的线数。自顶向下的采集多分辨率，构建图像金字塔序列，包含不同尺度的图像分辨率参数，并输出到步骤6中进行分别迭代。
55.进一步的是，步骤6包括：
56.步骤6.1、根据步骤5中的图像金字塔，自底向上遍历图像分辨率进行迭代，根据激光雷达垂直方向上所感知的角度以及水平方向的视场范围，我们预设了参数v
image_vfov
，v
image_hfov
，将当前的尺度ri*v
image_vfov
得到垂直方向的图像像素点个数n
num_row
，再将ri*v
image_hfov
得到水平方向上图像像素点的个数n
num_col
57.步骤6.2、遍历s
local_after
中的每个关键帧，根据n
num_row
和n
num_col
初始化图像矩阵mlocal_after
58.步骤6.3、针对每个关键帧s
local_after
，再启用多线程，线程数根据工控机核数而定为n
parallel_cores
，每个线程拥有各自的非共享变量因此互相之间不会发生干扰，并不会引发线程安全问题，这里选择不上锁。此时n个线程将由操作系统自行分配调度并执行。
59.步骤6.4、针对每个线程，关键在于分别计算点云中每个点映射到图像中的精确方位。首先需要计算每个点的垂直方位角p
i_va
＝arctan(p
i_y
/p
i_x
)*(180/pi)，水平方位角p
i_ha
＝arctan(z/sqrt(x^2 z^2))*(180/pi)
60.步骤6.5、可以计算点云在图像中的垂直方位索引p
index_x
，进而将三维点云映射到二维图像,公式为
[0061][0062]
步骤6.6、可以计算点云在图像中的水平方位索引p
index_y
，进而将三维点云映射到二维图像，公式为
[0063][0064]
步骤6.7、根据已知的点云在图像中的索引p
index_x
、p
index_y
，如果激光强度pi《v
x,y
说明该像素点未被占用，则将每个点的激光强度作为单通道图像的值赋值给图像矩阵m
local_after
，否则已被占用那么就不进行任何处理。进而得到投影后的点云，且坐标系此时位于激光局部坐标系，此时得到的二维图像是原始的未处理图像，即同时拥有静态坐标点与动态坐标点。
[0065]
步骤6.8、将下采样后的全局地图点云p
global_after
，通过变换矩阵p
reverse
、雷达外参矩阵，将点云从全局坐标系变换到雷达坐标系下得到p
global_after_local
，进而使得坐标系和当前关键帧的点云坐标系一致，才能便于后续步骤中对两个图像进行比较。
[0066]
步骤6.9、再初始化一个大小为n
num_row
和n
num_col
的单通道图像矩阵m
global_after_local
，用于映射s
global_after_local
；初始化一个用于映射点的索引与图像索引的映射矩阵m
point_to_image
[0067]
步骤6.10、针对每个转换到与当前关键帧同坐标系的s
global_after_local
，再启用多线程，线程数根据工控机核数而定为n
parallel_cores
，每个线程拥有各自的非共享变量因此互相之间不会发生干扰，并不会引发线程安全问题，这里选择不上锁。此时n个线程将由操作系统自行分配调度并执行
[0068]
步骤6.11、与步骤6.5和6.6同理，将s
global_after_local
同样映射到二维图像m
global_after_local
[0069]
步骤6.12、根据已知的点云在图像中的索引p
index_x
、p
index_y
，如果激光强度pi《v
x,y
说明该像素点未被占用，则将每个点的激光强度作为单通道图像的值赋值给图像矩阵m
global_after_local
，同时最关键的是将点云索引p
index
赋值给点云与图像的映射矩阵m
point_to_image
[0070]
进一步的是，步骤7包括：
[0071]
步骤7.1、根据步骤6.7和6.12得到的m
local_after
和m
global_after_local
，计算其差值m
diff
[0072]
步骤7.2、遍历图像的每个像素点，高精度的进行判断，计算阈值t
min
＝t
adaptive
*p
intensity
,也就是将自适应参数与原m
local_after
的相应像素点的值即激光强度p
intensity
进行乘积，同时根据激光最远探测距离设定上限阈值t
max
，以防由于噪声扰动所产生的无效点的误差。
[0073]
步骤7.3、遍历m
diff
中的每个值，如果该值大于t
min
且小于t
max
,则可标记该点为动态点，将全部的动态点索引放入集合s
dynamic_index
[0074]
步骤7.4、根据上述得到的动态点索引，将每一帧点云中的动态点全部去除，保留的即为静态点s
static_index
[0075]
进一步的是，步骤8包括：
[0076]
步骤8.1、根据动态点索引集合s
dynamic_index
，通过使用滤波器extractindices，这是一种点云分割的滤波算法，通过利用索引向量来构建点云索引并提取点云子集，因此通过我们输入的索引点，即可从输入点云中提取到给定索引所对应的动态点云子集p
dynamic_i
[0077]
步骤8.2、根据静态点索引集合s
static_index
，通过使用滤波器extractindices，这是一种点云分割的滤波算法，通过利用索引向量来构建点云索引并提取点云子集，因此通过我们输入的索引点，即可从输入点云中提取到给定索引所对应的静态点云子集p
static_i
[0078]
进一步的是，步骤9包括：
[0079]
步骤9.1、回到步骤6.2，并开始下一帧点云关键帧的处理，直至全部点云关键帧运算完毕
[0080]
步骤9.2、回到步骤6.1，并开始图像金字塔上一层图像尺度的变换处理，直至全部图像尺度下的全部点云关键帧处理完毕。
[0081]
步骤9.3、将得到的全部点云子集变换到全局坐标系下进行叠加，即可得到最终的动态地图最终的静态地图
[0082]
步骤9.4、输出并保存完整的静态地图
[0083]
在本发明的实施方式中允许使用任意型号的多线激光雷达以及任意多线激光slam系统，均能够输入到该静态地图构建系统中并去除动态目标，以构建高精度静态点云地图。
[0084]
本发明提供的一种基于多线激光雷达点云的多尺度静态地图构建系统具有以下有益效果：
[0085]
本发明应用与无人系统定位建图领域，具体可以用于服务机器人、自动驾驶车辆、未知三维环境建模等多个对象和场景。
[0086]
1、具有优良的可解释性的同时便于后续进一步处理。将三维点云根据垂直角和水平角，考虑噪声等误差，精确的投影到二维图像，实现数据的降维，进而能够以第一人称视角基于数字图像处理技术和图像特征来解决三维环境的特征问题，化繁为简。
[0087]
2、本发明将错误标记率和误差最低化，提供高精度的静态地图。通过使用图像金字塔，将投影得到的单一尺度的图像化为多个尺度，底层最大尺度下得到的动态点云集一定包括了真实集与误差集，本发明将第一个动态点云集输入到图像金字塔的上一层尺度，变换尺度后进行第二次迭代再标记。从而有效的标记并且得到新的动态点云集，这将会过滤并更正第一次标记中错误的动态点云。以此类推地不断迭代，从而将标记错误率最小化。
[0088]
3、本发明充分利用内存，并加快运算速度：在最耗时的点云投影阶段，由于为了保证精度需要不可避免的对点云中的每个点进行遍历处理，我们采用omp多线程同时解决了线程并发安全的问题，充分发挥了工控机的多核处理器性能。同时我们通过在关键帧和全局地图两个级别分别进行了降采样处理，且依据不同的点云规模采用了不同的降采样算法。
附图说明
[0089]
图1是基于多线激光雷达点云的多尺度静态地图构建系统的模型结构图；
[0090]
图2是本发明具体实施方式的多尺度静态地图构建系统示意图；
[0091]
图3是本发明具体实施方式的点云地图投影二维图像的示意图
[0092]
图4是本发明具体实施方式的静态构建结果；
具体实施方式
[0093]
为了使本技术领域的人员更好地理解本发明，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清晰、完整地描述，。本发明可以通过许多不同形式的实施例来得以体现，本发明的保护范围并非仅限于文中提到的实施例。
[0094]
本实施例提出一种基于多线激光雷达点云的多尺度静态地图构建系统，其模型结构图见图1，流程图见图2包括以下步骤：
[0095]
步骤1、激光雷达采集点云帧序列，slam系统采集位姿序列
[0096]
步骤2、按时间戳提取关键帧与帧对应的位姿
[0097]
步骤3、关键帧点云三维体素降采样
[0098]
步骤4、全局坐标系下将全部关键帧点云拼接成原始地图。
[0099]
步骤5、建立图像金字塔，判断图像金字塔是否已经到达顶层，否则迭代金字塔的上一层图像。
[0100]
步骤6、针对每一层图像将关键帧三维点云与原始地图分别投影至二维图像
[0101]
步骤7、将两图像的差与阈值进行比较从而确定动态目标的索引
[0102]
步骤8、将动态目标的位置映射回三维点云中点云的索引，对动态点与静态点分别进行标记并提取。
[0103]
步骤9、判断是否到达图像金字塔顶层，否就回到步骤6；是则静态地图构建完毕，输出静态地图和动态点集
[0104]
其中，预设的参数有：
[0105]vimage_vfov
为二维图像的预设方向视场
[0106]vimage_hfov
为二维图像的水平方向视场
[0107]ddownsample_size
为局部帧与全局地图的点云下采样参数
[0108]smulti_resolution_list
为图像金字塔的各个预设尺度序列
[0109]nparallel_cores
为三维点云投影至二维图像的并发线程数
[0110]rkeyframe_range
关键帧的选取范围
[0111]mextrinsic_lidar
雷达的外参矩阵
[0112]
其中，步骤1、利用多线激光雷达获取道路行驶数据输出每一帧的带有时间戳的激
光点云，在工控机下通过三维激光slam输出每帧的姿态数据。激光雷达位于机器人或车辆的正中心上方，每一帧激光点云都包括多条激光线扫描后接收到的强度信息以及相对于激光接收器的相对笛卡尔坐标。所使用的slam系统可以是任意以多线激光雷达数据和九轴imu数据为输入，能够输出每帧激光点云对应位姿的三维多线激光slam系统。
[0113]
其中，步骤2的按时间戳提取关键帧与帧对应的位姿包括：
[0114]
步骤2.1、根据存储了激光点云扫描帧的已经持久化的文件路径读取或订阅实时扫描到的异步通信的消息队列scanlist，按照预设参数r
keyframe_range
读取关键帧存入工控机内存。
[0115]
步骤2.2、读取激光slam系统输出的各个激光点云扫描帧所对应的经过迭代优化的位姿估计，该位姿是雷达的局部坐标系下的位姿，得到雷达坐标系到当前位姿的变换矩阵集合p
direct
。
[0116]
步骤2.3、对2.2中读取的全部位姿进行求逆得到当前位姿到雷达坐标系的变换矩阵集合p
reverse
[0117]
其中，步骤3、关键帧点云三维体素降采样，利用步骤2中输入的每一帧激光点云扫描帧，根据预设的d
downsample_size
下采样参数，通过在点云数据中创建三维体素栅格，然后用每个体素的重心来近似表达体素中的其它点，进而实现对关键帧的降采样处理，便于后续处理速度，同时减少内存占用。最终得到下采样后的关键帧集合s
local_after
[0118]
其中，步骤4的全局坐标系下将全部关键帧点云拼接成原始地图包括：
[0119]
步骤4.1、利用步骤3读入的关键帧点云和位姿中，根据索引依次提取对应的关键帧s
local_after
和关键帧对应的位姿t
direct
[0120]
步骤4.2、使用外参变换矩阵，将s
local_after
从雷达坐标系变换到主体baselink坐标系得到s
local_after_then
[0121]
步骤4.3、使用4.1中读取的t
direct
将位姿从主体baselink坐标系变换到全局坐标系下得到s
global_i
[0122]
步骤4.4、根据索引，通过迭代将全部的s
global_i
进行拼接，得到原始的全局地图p
global
[0123]
步骤4.5、根据预设的下采样参数d
downsample_size
对原始全局地图p
global
进行下采样，此时地图过于大，点云数据庞大，不适合用步骤3中的传统体素栅格进行下采样，而应该使用更复杂的数据结构八叉树，通过基于八叉树的体素滤波实现降采样，最终得到下采样后的全局地图p
global_after
[0124]
其中，步骤5、建立图像金字塔，判断图像金字塔是否已经到达顶层，否则迭代金字塔的上一层图像。根据预设的s
multi_resolution_list
，该预设的参数取决于雷达的分辨率，体现在激多线光雷达的线数。自顶向下的采集多分辨率，构建图像金字塔序列，包含不同尺度的图像分辨率参数，并输出到步骤6中进行分别迭代。
[0125]
如图3所示，其中，步骤6的针对每一层图像将关键帧三维点云与原始地图分别投影至二维图像包括：
[0126]
步骤6.1、根据步骤5中的图像金字塔，自底向上遍历图像分辨率进行迭代，根据激光雷达垂直方向上所感知的角度以及水平方向的视场范围，我们预设了参数v
image_vfov
，v
image_hfov
，将当前的尺度ri*v
image_vfov
得到垂直方向的图像像素点个数n
num_row
，再将ri*vimage_hfov
得到水平方向上图像像素点的个数n
num_col
[0127]
步骤6.2、遍历s
local_after
中的每个关键帧，根据n
num_row
和n
num_col
初始化图像矩阵m
local_after
[0128]
步骤6.3、针对每个关键帧s
local_after
，再启用多线程，线程数根据工控机核数而定为n
parallel_cores
，每个线程拥有各自的非共享变量因此互相之间不会发生干扰，并不会引发线程安全问题，这里选择不上锁。此时n个线程将由操作系统自行分配调度并执行。
[0129]
步骤6.4、针对每个线程，关键在于分别计算点云中每个点映射到图像中的精确方位。首先需要计算每个点的垂直方位角p
i_va
＝arctan(p
i_y
/p
i_x
)*(180/pi)，水平方位角p
i_ha
＝arctan(z/sqrt(x^2 z^2))*(180/pi)
[0130]
步骤6.5、可以计算点云在图像中的垂直方位索引p
index_x
，进而将三维点云映射到二维图像,公式为
[0131][0132]
步骤6.6、可以计算点云在图像中的水平方位索引p
index_y
，进而将三维点云映射到二维图像，公式为
[0133][0134]
步骤6.7、根据已知的点云在图像中的索引p
index_x
、p
index_y
，如果激光强度pi《v
x,y
说明该像素点未被占用，则将每个点的激光强度作为单通道图像的值赋值给图像矩阵m
local_after
，否则已被占用那么就不进行任何处理。进而得到投影后的点云，且坐标系此时位于激光局部坐标系，此时得到的二维图像是原始的未处理图像，即同时拥有静态坐标点与动态坐标点。
[0135]
步骤6.8、将下采样后的全局地图点云p
global_after
，通过变换矩阵p
reverse
、雷达外参矩阵，将点云从全局坐标系变换到雷达坐标系下得到p
global_after_local
，进而使得坐标系和当前关键帧的点云坐标系一致，才能便于后续步骤中对两个图像进行比较。
[0136]
步骤6.9、再初始化一个大小为n
num_row
和n
num_col
的单通道图像矩阵m
global_after_local
，用于映射s
global_after_local
；初始化一个用于映射点的索引与图像索引的映射矩阵m
point_to_image
[0137]
步骤6.10、针对每个转换到与当前关键帧同坐标系的s
global_after_local
，再启用多线程，线程数根据工控机核数而定为n
parallel_cores
，每个线程拥有各自的非共享变量因此互相之间不会发生干扰，并不会引发线程安全问题，这里选择不上锁。此时n个线程将由操作系统自行分配调度并执行
[0138]
步骤6.11、与步骤6.5和6.6同理，将s
global_after_local
同样映射到二维图像m
global_after_local
[0139]
步骤6.12、根据已知的点云在图像中的索引p
index_x
、p
index_y
，如果激光强度pi《v
x,y
说明该像素点未被占用，则将每个点的激光强度作为单通道图像的值赋值给图像矩阵m
global_after_local
，同时最关键的是将点云索引p
index
赋值给点云与图像的映射矩阵m
point_to_image
[0140]
其中，步骤7、将两图像的差与阈值进行比较从而确定动态目标的索引，包括：
[0141]
步骤7.1、根据步骤6.7和6.12得到的m
local_after
和m
global_after_local
，计算其差值m
diff
[0142]
步骤7.2、遍历图像的每个像素点，高精度的进行判断，计算阈值t
min
＝t
adaptive
*p
intensity
,也就是将自适应参数与原m
local_after
的相应像素点的值即激光强度p
intensity
进行乘积，同时根据激光最远探测距离设定上限阈值t
max
，以防由于噪声扰动所产生的无效点的误差。
[0143]
步骤7.3、遍历m
diff
中的每个值，如果该值大于t
min
且小于t
max
,则可标记该点为动态点，将全部的动态点索引放入集合s
dynamic_index
[0144]
步骤7.4、根据上述得到的动态点索引，将每一帧点云中的动态点全部去除，保留的即为静态点s
static_index
[0145]
其中，步骤8包括：
[0146]
步骤8.1、根据动态点索引集合s
dynamic_index
，通过使用滤波器extractindices，这是一种点云分割的滤波算法，通过利用索引向量来构建点云索引并提取点云子集，因此通过我们输入的索引点，即可从输入点云中提取到给定索引所对应的动态点云子集p
dynamic_i
[0147]
步骤8.2、根据静态点索引集合s
static_index
，通过使用滤波器extractindices，这是一种点云分割的滤波算法，通过利用索引向量来构建点云索引并提取点云子集，因此通过我们输入的索引点，即可从输入点云中提取到给定索引所对应的静态点云子集p
static_i
[0148]
其中，步骤9、判断是否到达图像金字塔顶层，否就回到步骤6；是则静态地图构建完毕，输出静态地图和动态点集，包括：
[0149]
步骤9.1、回到步骤6.2，并开始下一帧点云关键帧的处理，直至全部点云关键帧运算完毕
[0150]
步骤9.2、回到步骤6.1，并开始图像金字塔上一层图像尺度的变换处理，直至全部图像尺度下的全部点云关键帧处理完毕。
[0151]
步骤9.3、将得到的全部点云子集变换到全局坐标系下进行叠加，即可得到最终的动态地图最终的静态地图
[0152]
步骤9.4、输出并保存完整的静态地图，结果如图4所示。