CN115775242A - 一种基于匹配的点云地图质量评估方法 - Google Patents
一种基于匹配的点云地图质量评估方法 Download PDFInfo
- Publication number
- CN115775242A CN115775242A CN202211548632.2A CN202211548632A CN115775242A CN 115775242 A CN115775242 A CN 115775242A CN 202211548632 A CN202211548632 A CN 202211548632A CN 115775242 A CN115775242 A CN 115775242A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- coordinate system
- point
- map
- matching
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 66
- 238000013441 quality evaluation Methods 0.000 title claims abstract description 53
- 230000009466 transformation Effects 0.000 claims abstract description 94
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 76
- 230000008569 process Effects 0.000 claims abstract description 14
- 238000000605 extraction Methods 0.000 claims description 19
- 238000005259 measurement Methods 0.000 claims description 14
- 238000001303 quality assessment method Methods 0.000 claims description 14
- 238000004088 simulation Methods 0.000 claims description 14
- 238000013507 mapping Methods 0.000 claims description 10
- 230000005484 gravity Effects 0.000 claims description 7
- 230000001131 transforming effect Effects 0.000 claims description 7
- 238000012216 screening Methods 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 abstract description 10
- 230000000694 effects Effects 0.000 abstract description 4
- 238000012795 verification Methods 0.000 abstract description 4
- 238000011156 evaluation Methods 0.000 abstract description 2
- 238000013519 translation Methods 0.000 description 11
- 238000000844 transformation Methods 0.000 description 10
- 239000011159 matrix material Substances 0.000 description 9
- 238000000691 measurement method Methods 0.000 description 7
- 230000004927 fusion Effects 0.000 description 6
- 238000005070 sampling Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 230000033001 locomotion Effects 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 3
- 238000001514 detection method Methods 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 238000010606 normalization Methods 0.000 description 3
- 238000005457 optimization Methods 0.000 description 3
- 230000003068 static effect Effects 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000012935 Averaging Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000013480 data collection Methods 0.000 description 1
- 238000009616 inductively coupled plasma Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
- 238000012163 sequencing technique Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
- G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
- G06V10/757—Matching configurations of points or features
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/98—Detection or correction of errors, e.g. by rescanning the pattern or by human intervention; Evaluation of the quality of the acquired patterns
- G06V10/993—Evaluation of the quality of the acquired pattern
Landscapes
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- Quality & Reliability (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Multimedia (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Artificial Intelligence (AREA)
- Health & Medical Sciences (AREA)
- Computing Systems (AREA)
- Databases & Information Systems (AREA)
- Evolutionary Computation (AREA)
- General Health & Medical Sciences (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Processing Or Creating Images (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于匹配的点云地图质量评估方法,包括如下步骤:S1、获取待评估点云地图作为点云匹配算法输入;S2、获取用于匹配的点云样本集合;S3、使用点云匹配算法进行匹配,迭代得到估计三维坐标系变换;S4、计算点云匹配误差;S5、将得到的评估点云地图的质量评估分数作为点云匹配算法输出。本发明提供的一种基于匹配的点云地图质量评估方法,无需依赖真值、无需人工校验,可以进行全自动点云地图质量评价,从而提高点云地图质量评估效率;计算复杂度较低,可以快速有效地评估点云地图的质量;能够模拟使用点云地图进行定位的过程,从地图最终的定位效果出发,反映地图的建图质量。
Description
技术领域
本发明涉及点云地图质量评估领域,尤其是涉及一种基于匹配的点云地图质量评估方法。
背景技术
作为自动驾驶核心技术之一,点云地图能够为自动驾驶提供厘米级精度的三维地图信息,为无人驾驶技术中感知、定位、导航、规划与决策等模块提供重要的先验地图信息。点云地图的创建通常需要融合多种传感器数据,包括三维激光雷达(Light Detection AndRanging,Lidar)、全球定位系统(Global Positioning System,GPS)接收机、惯性传感器(Inertial Measurement Unit,IMU)和一个或多个摄像头。在根据多传感器数据完成点云地图创建后,为确保构建点云地图的高精度,必须对点云地图进行质量评估。
评估激光雷达绘制地图的精度,最简单的方法是计算估计位置姿态和地面真值之间的误差。但是在高精度点云地图的绘制过程中,总是会遇到真值无法获取的问题。为此,研究者们探究了其他质量评估方法,有如通过评估点云地图中局部地面的平整度,或者评估点云地图中柱状物体的柱面厚度等等方法进行点云地图质量评估。这些方法通过提取地图中的几何特征来评价点云地图质量,但是几何特征提取算法的准确度较低,因此使用这类方法评价地图质量通常还需要人工校验,效率较低。
基于此,有必要针对上述技术问题,提供一种计算复杂度较低、无需依赖真值、无需人工校验的全自动的评价点云地图质量的方法,以快速有效地评估点云地图的质量。
发明内容
本发明的目的是提供一种基于匹配的点云地图质量评估方法,以解决目前质量评估方法中存在的真值获取困难和难以准确提取几何特征的问题。
为实现上述目的,本发明提供了一种基于匹配的点云地图质量评估方法包括如下步骤:
S1、获取待评估点云地图作为点云匹配算法输入;
S2、获取用于匹配的点云样本集合;
S3、对于所述点云样本集合中的每一个所述点云样本,使用点云匹配算法将所述点云样本和所述待评估点云地图进行匹配,以所述原始三维坐标系变换为初值,迭代得到估计三维坐标系变换;
S4、计算得到点云匹配误差;
S5、计算所述待评估点云地图的质量评估分数,将所述质量评估分数作为所述点云匹配算法输出。
优选的,所述获取待评估点云地图是通过根据装配有GPS、IMU、机械旋转式Lidar等多种传感器的地图测绘车沿道路移动,采集得到点云数据,然后通过移动测量或同步定位与建图(Simultaneous localization and mapping,SLAM)算法确定每个点云坐标系到所述导航坐标系的坐标系变换,最后将一系列点云通过坐标系变换转换到所述导航坐标系,拼接为一个点云地图,从而建立待评估点云地图;
所述采集得到的点云数据,包括但不限于GPS定位结果、IMU测量结果、机械旋转式Lidar扫描结果,还可以根据实际情况增加如电子罗盘、轮速计、气压计等其他传感器采集得到的数据。
优选的,所述导航坐标系,指求解导航参数时使用的参考坐标系,在实际应用中,经常采用东北天地理坐标系作为导航坐标系;
所述东北天地理坐标系指以运载体重心为原点,三个坐标轴依次指向东北天三个方向的直角坐标系。
优选的,获取用于匹配的点云样本集合的方法,包括但不限于实地测量或从待评估点云地图抽样的方法。
优选的,所述点云样本集合中的每个元素均由一个点云样本和一个原始三维坐标系变换组成;
所述点云样本的坐标系为激光雷达坐标系,所述点云样本的点数量明显小于所述待评估点云地图的点数量;
所述激光雷达坐标系指激光雷达采集得到的点云数据所在的传感器坐标系。
优选的,将所述点云样本和所述待评估点云地图进行匹配的所述匹配算法,目的是计算两个点云之间坐标系变换,使得两个点云通过坐标系变换后能够完全重合;所述匹配算法包括但不限于迭代最近点(Iterative Closest Point,ICP)、正态分布变换(NormalDistributions Transform,NDT)。
优选的,所述原始三维坐标系变换指对应所述点云样本从所述激光雷达坐标系到所述导航坐标系的已知的准确变换。
优选的,所述原始三维坐标系变换,简要地描述了所述点云样本所在的所述激光雷达坐标系到所述导航坐标系之间的变换关系。
优选的,所述估计三维坐标系变换指通过点云匹配算法估计得到的所述点云样本从所述激光雷达坐标系到所述导航坐标系的变换。
优选的,所述点云匹配算法输入为两个点云,所述点云匹配算法输出是估计得到的两个点云坐标系之间的变换关系;
更具体地,所述点云匹配算法输入是所述点云样本和所述待评估点云地图;所述点云匹配算法输出是估计得到的所述点云样本所在的所述激光雷达坐标系到所述导航坐标系之间的变换关系。
优选的,所述点云匹配误差是通过定量地评估经过所述估计三维坐标系变换后的所述点云样本,与所述待评估点云地图之间的重合度。
优选的,计算所述点云匹配误差的方法包括但不限于使用均方点匹配误差、概率之和等指标,衡量经过所述估计三维坐标系变换后的所述点云样本和所述待评估点云地图之间的误差。
优选的,所述质量评估分数为计算全部所述点云样本对应的所述点云匹配误差的平均值,然后对所述平均值进行归一化,得到的;
用于计算所述质量评估分数的方法,目的是对不同指标得到的点云匹配误差,给出不同的所述待评估点云地图的质量评估分数;
所述质量评估分数越大,说明待评估的所述点云地图质量越高,局部地图相对精度更好,局部地图的重影程度更小。
一种从点云地图中仿真抽取点云样本的方法是根据输入的所述激光雷达坐标系到所述导航坐标系之间的变换关系,以及输入的点云地图,依据所述机械旋转式激光雷达实际获得的所述点云样本结构,以仿真抽取的形式获取所述点云样本;
具体步骤包括:
1)模拟实地测量过程,在所述待评估点云地图中沿地图测绘车数据采集的路线,进行等距离间隔仿真抽取,从而形成一系列所述原始三维坐标系变换;
2)在每个所述原始三维坐标系变换处,建立激光雷达坐标系;
3)根据待仿真激光雷达的型号,查阅产品说明书,获得对应的产品参数;
4)根据所述产品参数,求得所述点云样本中每一个点的方位角和俯仰角;
5)根据所述点云样本中每个所述点的所述方位角和俯仰角,计算每个所述点到所述激光雷达坐标系原点的距离,从而形成所述点云样本。
优选的,计算每个所述点到所述激光雷达坐标系原点的距离,获得的距离可以通过仿真反映实际地图中对应激光测距得到的距离,具体包括下述步骤:
a、以待仿真激光雷达的水平分辨角和垂直分辨角为视场角,以待求点所在的所述方位角和所述俯仰角为中心,获得一个锥体;
b、将待评估地图中所述锥体内的点全部截取出来;
c、对于截取出来的全部点形成的集合,使用基于密度的有噪空间聚类应用(Density-Based Spatial Clustering of Applications with Noise,DBSCAN)算法进行空间聚类;
d、将聚类得到的每个点集投影到垂直于所述待求点所在的所述方位角和俯仰角方位射线的平面;
e、对于聚类得到的每个所述点集的所述投影,求解其二维凸包;
f、从各个所述点云投影的所述二维凸包中,筛选出不含所述射线的投影点的凸包;
g、从剩余凸包中,计算得到对应三维点集重心距离所述激光雷达坐标系原点最近的凸包;
h、根据选出的唯一一个所述凸包对应的三维点集,计算该所述点集重心到所述激光雷达坐标系原点的沿所述射线方向的距离,作为最终的仿真抽取点距离。
一种从点云地图中仿真抽取点云样本的方法在获取所述点云样本集合中的应用。
因此,本发明提供的一种基于匹配的点云地图质量评估方法具有如下技术效果:
1.本发明提供了一种点云数据质量评估方法,包括:获取待评估点云地图;获取用于检测的准确点云样本;对于用于检测的准确点云样本中的每一个元素,使用匹配算法将准确点云和待评估点云地图进行匹配,得到估计三维点云变换;计算得到点云匹配误差;根据用于检测的准确点云样本中每一个元素得到的点云匹配误差,得到整个点云地图的质量评估分数;
2.本发明提出的基于匹配的点云质量评价方法,能够模拟使用点云地图进行定位的过程,从地图最终的定位效果出发,反映地图的建图质量;
3.本发明提出的基于匹配的点云数据质量评估方法避免了基于轨迹真值评估地图质量的传统方法无法获取真值的问题,同时相比基于几何特征评估地图质量的方法有着更低的计算复杂度。
下面通过附图和实施例,对本发明的技术方案做进一步的详细描述。
附图说明
为了更清楚地说明本发明实施例的技术方案,下面将对本发明实施例的描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1是点云地图质量评估方法的流程示意图;
图2是点云匹配算法的初始三维坐标系变换生成示意图;
图3是点云地图质量评估方法的流程示意图;
图4是仿真抽取方法所选锥体的示意图;
图5是仿真抽取方法三维点集向特定平面投影的示意图;
图6是仿真抽取方法投影点二维凸包示意图。
具体实施方式
以下通过附图和实施例对本发明的技术方案作进一步说明。
为了使得本申请的目的、技术方案及优点更加明确、透彻和完整,下面通过附图和实施例,对本发明的两个实施例中的技术方案进行清楚、完整地描述。
为了更加清晰的阐述实施例的技术方案,首先给出常用的坐标系定义,包括导航坐标系、地理坐标系、激光雷达坐标系等,以及部分三维坐标系变换名称的含义,包括原始三维坐标系变换、估计三维坐标系变换、初始三维坐标系变换等。
导航坐标系指求解导航参数时使用的参考坐标系,在实际应用中,经常采用东北天地理坐标系作为导航坐标系。东北天地理坐标系指以运载体重心为原点,三个坐标轴依次指向东北天三个方向的直角坐标系。激光雷达坐标系指激光雷达采集得到的点云数据所在的传感器坐标系。
三维坐标系变换,就是描述了不同坐标系之间如何进行转换的,对于一个三维坐标系变换来说,需要同时描述平移量和旋转量。其中,原始三维坐标系变换指对应点云样本从激光雷达坐标系到导航坐标系的已知的准确变换;估计三维坐标系变换指通过点云匹配算法估计得到的点云样本从激光雷达坐标系到导航坐标系的变换。初始三维坐标系变换指点云匹配算法需要的坐标系变换初值。
此外还需要明确三维空间内点云的变换。变换包含平移和旋转,其中平移使用三维向量而旋转使用旋转单位矢量和旋转角度φ共同描述。需要注意的是三维旋转仅有3个自由度,但是为了方便描述,这里选择使用旋转单位矢量加旋转角度共4个参数描述。此外,通常还使用四元数描述三维空间中的旋转,且四元数和旋转矢量φ的关系为给出根据三维坐标系变换参数对一个点进行变换T的公式:
其中e=1-cosφ,c=cosφ,s=sinφ。
而对点云地图的质量评估,主要是通过分析计算得出点云地图中点云对应的位姿是否准确,或者通过分析计算得到点云地图的清晰度,使得确保能够为自动驾驶提供准确、可靠的三维地图。
本发明内容中具体步骤的实现不局限于某种方法。下文的两个实施例,只是给出了两种可能的完整实施方法,具体每个步骤中使用的算法均可以在一定程度上进行替换。
对于步骤S1建立点云地图来说,既可以使用传统的移动测量的方法,也可以使用多传感器融合的SLAM方法,两者最终都可以根据采集的数据获取点云地图。移动测量方法对使用的传感器性能要求相对更高,而多传感器融合的SLAM对于传感器性能要求不高,但是要求有多种类型的传感器。
对于步骤S2获取用于匹配的点云样本集合,既可以使用实地测量的方法,也可以使用仿真抽取的方法,两者也都可以获取点云样本集合。实地测量方法要求能够使用地图采集装置在实地进行采集,而仿真抽取方法没有这个要求,但是准确度相对较低。
对于步骤S3点云匹配算法,既可以使用ICP算法,也可以使用NDT算法,两者均可以实现点云匹配。ICP算法复杂度更高,对于平移鲁棒性更好,而NDT算法复杂度相对更低,对于旋转鲁棒性更好。
对于步骤S4计算点云匹配误差,既可以使用ICP算法中以均方点匹配误差作为衡量,也可以使用NDT算法中以概率之和作为衡量,两者均可以衡量点云匹配误差。即使使用了ICP算法也可以使用概率之和作为点云匹配误差的衡量,即使使用了NDT算法也可以使用均方点匹配误差作为点云匹配误差的衡量。可以不受限于步骤3中的匹配算法。
对于步骤S5计算质量评估分数,则依赖于步骤4中点云匹配误差的选取。不同类型的点云匹配误差计算出来的质量评估分数之间不具有可比性。例如,使用均方点匹配误差计算得到的质量评估分数无法和使用概率之和计算得到的质量评估分数进行比较。但是可以在步骤3中使用不同的匹配算法,在步骤4中使用相同类型的点云匹配误差,在步骤5中计算相同类型的质量评估分数。
实施例一使用移动测量方法,获取待评估点云地图作为算法输入进行点云地图质量评估
一种基于匹配的点云地图质量评估方法,见图1,包括如下步骤:
S1、使用移动测量方法,获取待评估点云地图作为算法输入,待评估点云地图就是一个坐标系为导航坐标系的点云;
S2、使用实地测量方法,获取用于匹配的点云样本集合,点云样本集合中的每个元素由一个点云样本和一个原始三维坐标系变换组成;
S3、对于点云样本集合中的每一个点云样本,使用ICP算法将点云样本和待评估点云地图进行匹配,以原始三维坐标系变换附近为初值,迭代得到估计三维坐标系变换;
S4、根据估计三维坐标系变换,以欧式距离为指标计算得到点云匹配误差;
S5、根据点云样本集合中的每一个点云样本的点云匹配误差,得到待评估点云地图的质量评估分数,将质量评估分数作为算法输出。
更具体地,步骤S1中移动测量方法,通常来说是使用装有GPS和IMU以及机械旋转式Lidar的地图测绘车,沿道路移动采集周围环境信息并时刻记录位置信息。在采集完全部数据后,通常使用GPS和IMU形成的高精度的组合导航系统输出高频(一般和IMU输出频率相同)的载体姿态位置速度信息,经过处理可以得到不同时刻点云坐标系到导航坐标系的坐标系变换。而周围的环境信息由Lidar采集得到,但是Lidar在采集周围环境时是旋转成像的,在Lidar运动不同时刻下采集得到的数据参考坐标系不同,为了建立最终的点云地图,必须进行运动补偿,为此,需要对组合导航系统输出的高频信息进行线性插值,将Lidar采集旋转一周得到的激光点云变换到同一时刻的点云坐标系下,从而得到一系列点云,最后将一系列点云通过组合导航得到的坐标系变换转换到导航坐标系,拼接为一个点云地图。
更具体地,步骤S2中的实地测量获取准确点云样本,具体步骤是使用装有GPS和IMU以及机械旋转式Lidar的地图测绘车,在同一测量场景下,在静止的情况下,采集载体的位置姿态和周围环境。根据GPS和IMU得到载体姿态位置,经过处理后可以得到当前静止处的点云坐标系到导航坐标系的坐标系变换,也就是原始三维坐标系变换。由于Lidar静止,也无需进行运动补偿,直接将Lidar采集旋转一周得到的激光点作为点云样本即可。最后为了更加全面的评估整个地图的质量,必须在整个地图的范围进行合适的采样,因此,选择沿步骤S1中地图测绘车数据采集的路线,进行等距离间隔实地数据采集。其中间隔距离为5m。
更具体地,步骤S3中的ICP算法,是三维点云配准中较为常用的匹配算法,可以以原始三维坐标系变换附近为初值,通过迭代计算得到估计三维坐标系变换,最终实现经过变换后两个点云之间的距离最小。在每次迭代中对于目标点云中的每个点,算法将在参考点云中寻找距离最近的点,并根据这些配对的点计算三维坐标系变换参数并对目标点云进行三维坐标系变换,以获得新的目标点云并进入下一个迭代过程,直到符合预期要求。最后就可以得到优良的变换矩阵,实现两个点云之间的精确配准。
ICP的具体算法如下:
5)根据协方差矩阵ΣXZ,生成反对称矩阵并进一步得到一个列向量Δ=[A23 A31 A12]T,并最终生成对称矩阵 而由该对称矩阵Q(ΣXZ)的最大特征值对应的特征向量[q0 q1 q2 q3]T,可以得到旋转四元数
10)判断是否满足迭代终止条件,如果dk+1-dk<τ,则满足迭代终止条件,退出循环,否则跳转执行第2步。
以上就是ICP的算法流程描述。由于ICP是一个较为简单的迭代收敛过程,比较容易收敛到局部最优值,导致最终求得的变换不正确,因此一般需要向ICP提供良好的三维坐标系变换初值,使得ICP能够最终收敛到正确的位姿变换附近。通常位姿变换的初值是根据IMU航位推算确定或者选择对原始三维坐标系变换施加微小偏移得到。考虑到在进行点云质量评估时,输入不包括IMU输出的加速度和角速度信息,因此,这里ICP初值的获取只能选择对原始三维坐标系变换施加微小偏移得到。
此外,由于ICP需要计算目标点云X中每个点到参考点云Y的最近点云,计算复杂度在O(NXNY),计算量极大。因此,有必要减小目标点云X和参考点云Y的点数量,为此,通常选择降采样或者局部点云截取的方法来减小点云数量。由于评估的待评估点云的质量,因此,需要在点云质量评估方法中尽量分离出其他因素影响导致的误差。为了不引入额外误差,这里只能选择对目标点云X和参考点云Y进行局部点云截取,来降低点数量。
更具体地,步骤S3的详细步骤如下:
首先,对待评估点云地图进行局部地图截取,由于已知点云样本相对导航坐标系的原始三维坐标系变换,因此,选择以原始三维坐标系变换的平移量对应位置为球心,半径25m做球,将球内点全部截取出来,得到局部待评估点云地图。接着需要对点云样本也进行局部截取,类似地选择以原点为球心,半径25m做球,将球内全部点截取出来,得到局部点云样本。
此外,还需要为ICP算法提供一个良好初始三维坐标系变换。为了尽量减小原始三维坐标系变换对最终匹配结果的影响,将使用不同的初始三维坐标系变换对同一个点云样本进行多次匹配对准,最终使用多个估计三维坐标系变换的平均值作为估计三维坐标系变换。考虑到点云大多由车辆采集,选择对原始三维坐标系变换施加水平方向的平移偏移以及沿天向的旋转偏移得到共计8个初始三维坐标系变换,具体见图2。使图中8个初始三维坐标系变换的位置均分布在原始三维坐标系变换的东南西北四个方向偏差5m的位置上,而每个位置上有两个初始三维坐标系变换,分别是沿天向正向旋转15以及负向旋转15。
然后,将点云样本使用8个初始三维坐标系变换后,再和局部待评估点云地图使用ICP进行匹配。其中点云样本是ICP算法中的目标点云X,局部待评估点云地图是ICP算法中的参考点云Y。
更具体地,步骤S4以欧式距离为指标计算得到点云匹配误差,即ICP算法中的匹配的均方误差dk+1,因此,最后将8个初始三维坐标系变换转换得到的ICP匹配的均方误差求平均,得到点云匹配误差。更具体地,步骤S4中得到的点云匹配误差描述了点云样本每个点到其附近待评估点云地图的各点的距离最小值。而该最小值越大,说明待评估点云地图局部和真实地图局部的偏移就越大。因此,可以说,步骤S4中得到的点云匹配误差本质上描述了待评估点云地图局部质量评估分数。
更具体地,步骤S5计算待评估点云地图质量评估分数,只需对整体地图的平均点云匹配误差进行归一化后就可以作为最终的质量评估分数。考虑到点云地图的最终应用是进行精确的定位,因此,这里选择使用自动驾驶领域常用的定位评价指标0.1m作为归一化因子,得到最终的质量评估分数,也就是其中而di是每一个点云样本和待评估点云地图的点云匹配误差。用计算式可以得出点云匹配误差越小,质量评估分数越大,地图质量越高。
实施例二使用多传感器融合SLAM的方法,获取待评估点云地图作为算法输入进行点云地图质量评估
一种基于匹配的点云地图质量评估方法,见图3,包括:
1.使用多传感器融合SLAM的方法,获取待评估点云地图作为算法输入,待评估点云地图就是一个坐标系为导航坐标系的点云;
2.使用仿真抽取方法,获取用于匹配的点云样本集合,点云样本集合中的每个元素由一个点云样本和一个原始三维坐标系变换组成;
3.对于点云样本集合中的每一个点云样本,使用NDT算法将点云样本和待评估点云地图进行匹配,以原始三维坐标系变换附近为初值,迭代得到估计三维坐标系变换;
4.根据估计三维坐标系变换,以概率之和为指标计算得到点云匹配误差;
5.根据点云样本集合中的每一个点云样本的点云匹配误差,得到待评估点云地图的质量评估分数,将质量评估分数作为算法输出。
具体地,步骤1中多传感融合SLAM方法,通常来说是使用装有GPS和IMU、机械旋转式Lidar以及其他各种传感器(包括但不限于电子罗盘、轮速计、气压计等)的地图测绘车,沿道路移动采集周围环境信息并时刻记录位姿信息。在采集完全部数据后,需要通过如下处理:首先对Lidar采集得到的点云数据使用IMU输出的高频加速度和角速度数据进行运动补偿,接着对运动补偿后的点云进行帧间匹配,获取相邻时间之间两帧点云的位姿变换,并通过融合IMU实现SLAM前端融合,接着在SLAM后端通过加入GPS以及其他传感器实现多传感器融合,得到最终的点云地图。
更具体地,步骤2中的仿真抽取获取点云样本指不进行实地测量,或者不具备再次进行实地测量的条件。从待评估点云地图中仿真抽取部分点云作为点云样本,而所谓仿真抽取就是指仿真模拟实地数据采集,在待评估点云地图中选取部分采样点,以采样点为Lidar坐标系的原点,仿真扫描周围环境,并生成一帧点云样本,而原始三维坐标系变换就是当前Lidar坐标系到导航坐标系的变换。为了通过仿真抽取的方式获取点云样本,必须明确实地采集中获取的激光点云的结构。
对于机械旋转式激光雷达,其工作原理为,将多个激光测距器垂直排列(激光测距器的数量一般为16、32、64甚至128个),并呈现一定夹角,然后安装在绕轴旋转的支架上。支架旋转一圈,带动多个激光测距器旋转一圈,同时测量得到周围环境的距离,形成点云样本,而形成的点云样本中的每个点都是由某个激光测距器在某个方位上测量距离得到的。因此,我们在仿真模拟实地数据时,必须已知点云中的水平角度范围、水平分辨角和垂直角度范围、垂直分辨角。如对于Velodyne公司的VLP-16激光雷达来说,共有16个激光测距器,其参数如表1。
表1
名称 | 数值/° |
水平角度范围 | 0~360 |
水平分辨角 | 0.2(600rpm) |
垂直角度范围 | -15~+15 |
垂直分辨角 | 2 |
根据激光雷达的参数,就可以求得点云样本中全部点的角度分布。接下来就可以模拟实地测量过程,在待评估点云地图中抽取点云形成点云样本。和实施例一类似,沿步骤1中地图测绘车数据采集的路线,进行等距离间隔仿真抽取(其中间隔距离为5m),从而形成一系列原始三维坐标系变换。对于每个原始三维坐标系变换处,建立激光雷达坐标系,最后根据点云样本中每个点的角度分布,计算每个点的距离,从而形成点云样本。
其中,根据点云样本中每个点的角度分布计算每个点的距离,为仿真抽取的关键。为了描述清楚作如下符合描述,待求点为A,水平分辨率为rH,垂直分辨率为rV,待求点A在的方位角为α,待求点A所在俯仰角为ω,激光雷达坐标系原点为O,从激光雷达坐标系原点O到待求点A的射线为lOA。
计算每个待求点到激光雷达坐标系原点的距离,具体包括下述步骤:
a、以待仿真激光雷达的水平分辨率角rH和垂直分辨角rV为水平垂直视场角,以从激光雷达坐标系原点O到待求点A的射线lOA为中心轴,获得一个锥体,具体见图4;
b、将待评估地图中锥体内的点全部截取出来,记从待评估地图中截取出来的点集为P;
c、对于截取出来的全部点形成的集合P,使用DBSCAN算法进行空间聚类;
d、将聚类得到的每个点集Pi投影到垂直于lOA的平面,具体见图5;
e、对于聚类得到的每个点集Pi的投影Qi,求解其二维凸包,具体见图6;
f、从各个点云投影Qi的二维凸包中,筛选出不含射线的投影点的凸包;
g、从剩余凸包中,计算每个凸包对应三维点集Pi重心到激光雷达坐标系原点O的沿射线lOA方向的距离di;
h、选出最小距离min di,作为最终的仿真抽取点距离。
在计算每个待求点到激光雷达坐标系原点的距离中,使用到了DBSCAN算法。DBSCAN是使用密度连通性区分不同簇,同一个簇内密度相互连通,但是不同簇之间密度无法联通。为了进一步描述DBSCAN,需要给定有关密度连通的相关定义:
定义2.直接密度可达:如果p∈NEps(q)且|NEps(q)|≥MinPts(即NEps(q)点数大于MinPts)时,称q到p直接密度可达;
定义3.密度可达:如果p和q之间存在一条链条p1,…,pn,p1=q,pn=q且pi到pi+1直接密度可达,称q到p密度可达;
定义4.密度连通:如果存在o,使得o到p密度可达且o到q密度可达,称p和q密度连通;
定义5.簇:对于数据集合D,簇C是一个非空集合,且满足如下条件:
DBSCAN的部分伪代码如下:
输入:D是含n个样本点数据集;Eps是半径;MinPts是最小样本点数;
输出:簇集合。
1)对D内全部点初始化,标记D内全部点为未访问;
2)遍历D内全部点,对于一个没有被访问的点p;
3)标记p为已访问;
4)记N是p的Eps邻域;
5)如果N的点数≥MinPts;
6)创建一个新的簇C,把p放入C中;
7)对遍历N内全部点q;
8)如果q未被访问;
9)标记q为已访问;
10)记M是q的Eps邻域;
11)如果M的点数≥MinPts,则将M内点放入N中;
12)如果q不属于任何一个簇,则将q放入C;
13)否则标记p为噪声;
14)如果全部点均被访问,则终止。
在计算每个待求点到激光雷达坐标系原点的距离中,使用了二维凸包Graham算法计算三维点集Pi投影Qi的二维凸包。Graham算法是一个简单有效的二维凸包计算方法,其关键是节点的极角排序和方向判断过程。在描述Graham算法之前,首先给出凸包的定义。
输入:点集S={p1,…,pn};
输出:点集S的凸包conv(S)的边界和极点个数。
1)找到点集S中最下方的点(如果有多个最下,则选择最右端的),记为P0;
2)对S-{P0}集合内点,按点到P0的连线与x轴正向夹角从小到大排序(夹角相同,则按距离从近到远排序),排序结果记为P1,…,Pn-1;
3)初始化P0,P1入栈,i=2,进入循环;
5)Pt出栈;
6)回到第4)步;
7)否则;
8)Pi入栈;
9)如果Pi≠Pn-1;
10)i=i+1;
11)回到第4)步;
12)否则;
13)终止循环,依次输出栈内元素;
更具体地,步骤3中的NDT算法,是三维点云配准中较为常用的匹配算法,可以通过将参考点云用若干以正态分布描述的栅格表示,将变换后的目标点云各点的概率之和作为优化问题中的目标函数,将目标点云对准所需的变换作为待优化变量,从而构建优化问题,求解优化问题得到两个点云之间的平移和旋转,最终实现经过变换后两个点云之间重叠的概率最大。不同于ICP对于每个点进行处理,NDT算法使用栅格网络中每个栅格内的正态分布进行计算求解,从而大大降低了计算复杂度,同时由于NDT使用基于概率的方法描述两个点云之间的重叠度,明显增加了算法对于局部噪声的鲁棒程度。
NDT的具体算法如下:
2)将参考点云Y划分为栅格网络,每个栅格是一个边长dgrid的正方体,可能包含若干参考点云Y中的点,去除包含点数少于Nless的栅格;
7)接着根据下列公式计算分数的梯度g和海塞矩阵H
其中a=[2ex1 0 0]T,
b=[ex2 ex1 0]T,
c=[ex3 0 ex1]T,
d=[s(2rxx1+ryx2+rzx3)sryx1-cx3srzx1+cx2]T,
e=[0 2ex2 0]T,
f=[0 ex3 ex2]T,
g=[srxx2+cx3 s(rxx1+2ryx2+rzx3)srzx2-cx1]T,
h=[0 0 2ex3]T,
i=[srxx3-cx2 sryx3+cx1 s(rxx1+ryx2+2rzx3)]T,
j=[cA+sB cC+sD cE+sF]T;
9)判断是否符合收敛条件,如果满足收敛,则退出循环,完成
NDT。
以上就是NDT的算法流程描述。由于NDT是一个较为简单的迭代收敛过程,比较容易收敛到局部最优值,导致最终求得的变换不正确,因此,一般需要向NDT提供良好的三维坐标系变换初值,使得NDT能够最终收敛到正确的位姿变换附近。通常位姿变换的初值是根据IMU航位推算确定或者选择对原始三维坐标系变换施加微小偏移得到。考虑到在进行点云质量评估时,输入不包括IMU输出的加速度和角速度信息,因此,这里NDT初值的获取只能选择对原始三维坐标系变换施加微小偏移得到。
虽然NDT的计算量相较于ICP明显减小,但是为了和ICP匹配算法保持输入点云的一致,依旧需要减小目标点云X和参考点云Y的点数量,也就是对目标点云X和参考点云Y进行局部点云截取。
更具体地,步骤3的详细步骤如下:
首先,对待评估点云地图进行局部地图截取,由于已知点云样本相对导航坐标系的原始三维坐标系变换,因此,选择以原始三维坐标系变换的平移量对应位置为球心,半径25m做球,将球内点全部截取出来,得到局部待评估点云地图。接着需要对点云样本也进行局部截取,类似地选择以原点为球心,半径25m做球,将球内全部点截取出来,得到局部点云样本。
接着,还需要为NDT算法提供一个良好初始三维坐标系变换。为了尽量减小原始三维坐标系变换对最终匹配结果的影响,将使用不同的初始三维坐标系变换对同一个点云样本进行多次匹配对准,最终使用多个估计三维坐标系变换的平均值作为估计三维坐标系变换。考虑到点云大多由车辆采集,选择对原始三维坐标系变换施加水平方向的平移偏移以及沿天向的旋转偏移得到共计8个初始三维坐标系变换,具体见图2。图中使用图中8个初始三维坐标系变换的位置均分布在原始三维坐标系变换的东南西北四个方向偏差5m的位置上,而每个位置上有两个初始三维坐标系变换,分别是沿天向正向旋转15°以及负向旋转15°。
然后将点云样本使用8个初始三维坐标系变换后,再和局部待评估点云地图使用NDT进行匹配。其中点云样本是NDT算法中的目标点云X,局部待评估点云地图是NDT算法中的参考点云Y。
更具体地,步骤4以概率之和为指标计算得到点云匹配误差,就是NDT算法中的匹配分数因此最后将8个初始三维坐标系变换变换得到的NDT匹配的匹配分数求平均,得到点云匹配误差。更具体地,步骤4中得到的点云匹配误差描述了点云样本每个点在其附近待评估点云地图形成栅格内正态分布的概率之和。而该概率和越小,说明待评估点云地图局部和真实地图局部的偏移就越大。因此可以说,步骤4中得到的点云匹配误差本质上描述了待评估点云地图局部质量评估分数。
更具体地,步骤5计算待评估点云地图质量评估分数,考虑到点云匹配误差是基于概率之和计算得到的,因此无需进行进一步的归一化步骤,可以直接以概率之和作为最终的质量评估分数,也就是其中 而di是每一个点云样本和待评估点云地图的点云匹配误差。用计算式可以得出点云匹配误差越小,质量评估分数越大,地图质量越高。
因此,本发明提供的一种基于匹配的点云地图质量评估方法,无需依赖真值、无需人工校验,可以进行全自动点云地图质量评价,从而提高点云地图质量评估效率;计算复杂度较低,可以快速有效地评估点云地图的质量;能够模拟使用点云地图进行定位的过程,从地图最终的定位效果出发,反映地图的建图质量。
最后应说明的是:以上实施例仅用以说明本发明的技术方案而非对其进行限制,尽管参照较佳实施例对本发明进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对本发明的技术方案进行修改或者等同替换,而这些修改或者等同替换亦不能使修改后的技术方案脱离本发明技术方案的精神和范围。
Claims (10)
1.一种基于匹配的点云地图质量评估方法,其特征在于,包括如下步骤:
S1、获取待评估点云地图作为点云匹配算法输入;
S2、获取用于匹配的点云样本集合;
S3、对于所述点云样本集合中的每一个所述点云样本,使用点云匹配算法将所述点云样本和所述待评估点云地图进行匹配,以所述原始三维坐标系变换为初值,迭代得到估计三维坐标系变换;
S4、计算得到点云匹配误差;
S5、计算所述待评估点云地图的质量评估分数,将所述质量评估分数作为所述点云匹配算法输出。
2.根据权利要求1所述的一种基于匹配的点云地图质量评估方法,其特征在于:所述待评估点云地图是一个坐标系为导航坐标系的点云;
获取所述待评估点云地图是通过装配有GPS、IMU、机械旋转式Lidar等多种传感器的地图测绘车沿道路移动,采集得到的点云数据,进而建立/获取待评估点云地图;
所述采集得到的点云数据,包括但不限于GPS定位结果、IMU测量结果、机械旋转式Lidar扫描结果,还可以根据实际情况增加如电子罗盘、轮速计、气压计等其他传感器采集得到的数据。
3.根据权利要求1所述的一种基于匹配的点云地图质量评估方法,其特征在于:所述点云样本集合中的每个元素均由一个点云样本和一个原始三维坐标系变换组成;
所述点云样本的坐标系为激光雷达坐标系,所述点云样本的点数量明显小于所述待评估点云地图的点数量;
所述激光雷达坐标系指激光雷达采集得到的点云数据所在的传感器坐标系。
4.根据权利要求1所述的一种基于匹配的点云地图质量评估方法,其特征在于:所述原始三维坐标系变换,简要地描述了所述点云样本所在的所述激光雷达坐标系到所述导航坐标系之间的变换关系。
5.根据权利要求1所述的一种基于匹配的点云地图质量评估方法,其特征在于:所述点云匹配算法输入为两个点云,所述点云匹配算法输出是估计得到的两个点云坐标系之间的变换关系;
更具体地,所述点云匹配算法输入是所述点云样本和所述待评估点云地图;所述点云匹配算法输出是估计得到的所述点云样本所在的所述激光雷达坐标系到所述导航坐标系之间的变换关系。
6.根据权利要求1所述的一种基于匹配的点云地图质量评估方法,其特征在于:所述点云匹配误差是通过定量地评估经过所述估计三维坐标系变换后的所述点云样本,与所述待评估点云地图之间的重合度。
7.根据权利要求1所述的一种基于匹配的点云地图质量评估方法,其特征在于:所述质量评估分数为计算全部所述点云样本对应的所述点云匹配误差的平均值,然后对所述平均值进行归一化,得到的。
8.一种从点云地图中仿真抽取点云样本的方法,其特征在于,具体包括下述步骤:
1)模拟实地测量过程,在所述待评估点云地图中沿地图测绘车数据采集的路线,进行等距离间隔仿真抽取,从而形成一系列所述原始三维坐标系变换;
2)在每个所述原始三维坐标系变换处,建立激光雷达坐标系;
3)根据待仿真激光雷达的型号,查阅产品说明书,获得对应的产品参数;
4)根据所述产品参数,求得所述点云样本中每一个点的方位角和俯仰角;
5)根据所述点云样本中每个所述点的所述方位角和俯仰角,计算每个所述点到所述激光雷达坐标系原点的距离,从而形成所述点云样本。
9.根据权利要求8所述的一种从点云地图中仿真抽取点云样本的方法,其特征在于,计算每个待求点到所述激光雷达坐标系原点的距离,获得的距离可以通过仿真反映实际地图中对应激光测距得到的距离,具体包括下述步骤:
a、以待仿真激光雷达的水平分辨角和垂直分辨角为视场角,以待求点所在的所述方位角和所述俯仰角为中心,获得一个锥体;
b、将待评估地图中所述锥体内的点全部截取出来;
c、对于截取出来的全部点形成的集合,使用DBSCAN算法进行空间聚类;
d、将聚类得到的每个点集投影到垂直于所述待求点所在的所述方位角和俯仰角方位射线的平面;
e、对于聚类得到的每个所述点集的所述投影,求解其二维凸包;
f、从各个所述点云投影的所述二维凸包中,筛选出不含所述射线的投影点的凸包;
g、从剩余凸包中,计算得到对应三维点集重心距离所述激光雷达坐标系原点最近的凸包;
h、根据选出的唯一一个所述凸包对应的三维点集,计算该所述点集重心到所述激光雷达坐标系原点的沿所述射线方向的距离,作为最终的仿真抽取点距离。
10.权利要求8所述的一种从点云地图中仿真抽取点云样本的方法在获取所述点云样本集合中的应用。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211548632.2A CN115775242A (zh) | 2022-12-05 | 2022-12-05 | 一种基于匹配的点云地图质量评估方法 |
US18/138,139 US20240185595A1 (en) | 2022-12-05 | 2023-04-24 | Method for evaluating quality of point cloud map based on matching |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211548632.2A CN115775242A (zh) | 2022-12-05 | 2022-12-05 | 一种基于匹配的点云地图质量评估方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115775242A true CN115775242A (zh) | 2023-03-10 |
Family
ID=85391368
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211548632.2A Pending CN115775242A (zh) | 2022-12-05 | 2022-12-05 | 一种基于匹配的点云地图质量评估方法 |
Country Status (2)
Country | Link |
---|---|
US (1) | US20240185595A1 (zh) |
CN (1) | CN115775242A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117470280A (zh) * | 2023-12-21 | 2024-01-30 | 绘见科技(深圳)有限公司 | 激光slam实时精度评估方法、装置、介质和设备 |
-
2022
- 2022-12-05 CN CN202211548632.2A patent/CN115775242A/zh active Pending
-
2023
- 2023-04-24 US US18/138,139 patent/US20240185595A1/en active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117470280A (zh) * | 2023-12-21 | 2024-01-30 | 绘见科技(深圳)有限公司 | 激光slam实时精度评估方法、装置、介质和设备 |
CN117470280B (zh) * | 2023-12-21 | 2024-04-05 | 绘见科技(深圳)有限公司 | 激光slam实时精度评估方法、装置、介质和设备 |
Also Published As
Publication number | Publication date |
---|---|
US20240185595A1 (en) | 2024-06-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112347840B (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
CN111486845B (zh) | 基于海底地形匹配的auv多策略导航方法 | |
CN101598556B (zh) | 一种未知环境下无人机视觉/惯性组合导航方法 | |
CN110930495A (zh) | 基于多无人机协作的icp点云地图融合方法、系统、装置及存储介质 | |
CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
EP3447729B1 (en) | 2d vehicle localizing using geoarcs | |
CN113220818B (zh) | 一种停车场自动建图与高精度定位方法 | |
EP3291178B1 (en) | 3d vehicle localizing using geoarcs | |
CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
CN112346104A (zh) | 一种无人机信息融合定位方法 | |
CN116184430B (zh) | 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法 | |
CN113763549A (zh) | 融合激光雷达和imu的同时定位建图方法、装置和存储介质 | |
Zhang et al. | Online ground multitarget geolocation based on 3-D map construction using a UAV platform | |
CN115775242A (zh) | 一种基于匹配的点云地图质量评估方法 | |
CN113554705B (zh) | 一种变化场景下的激光雷达鲁棒定位方法 | |
CN110927765A (zh) | 激光雷达与卫星导航融合的目标在线定位方法 | |
CN114577196A (zh) | 使用光流的激光雷达定位 | |
Zhang et al. | Feature-based ukf-slam using imaging sonar in underwater structured environment | |
WO2020118623A1 (en) | Method and system for generating an environment model for positioning | |
Kupervasser et al. | Robust positioning of drones for land use monitoring in strong terrain relief using vision-based navigation | |
CN114897942A (zh) | 点云地图的生成方法、设备及相关存储介质 | |
Mounier et al. | High-precision positioning in GNSS-challenged environments: a LiDAR-based multi-sensor fusion approach with 3D digital maps registration | |
Furgale et al. | A comparison of global localization algorithms for planetary exploration | |
Vivet | Extracting proprioceptive information by analyzing rotating range sensors induced distortion | |
CN116664698B (zh) | 车载双目相机与gnss/imu的自动标定方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |