一种联合多类约束的车载激光点云位姿图优化方法及系统

本发明属于机器视觉、机器人建图和车载激光点云数据处理的交叉领域，具体涉及一种联合多类约束的车载激光点云位姿图优化方法及系统。

背景技术：

1、激光点云数据在城市基础设施建设、道路分析、交通分析、城市绿量评估等应用中蕴含着巨大的潜力。车载激光扫描系统凭借其高效性、灵活性、高精度性等属性，成为获取大尺度场景几何信息广泛使用的系统之一。这些任务场景都以激光点云数据有一致的空间位置作为前提。

2、车载激光扫描系统由卫星导航系统(global navigation satellite system，gnss)、惯性导航系统、激光扫描系统等硬件以及时间同步系统等软件组成，并集成在移动平台上。车载激光点云搭载的激光扫描仪通常是高精度的2d激光扫描仪，通过高频率的转速实现场景的扫描，并在移动平台的运动过程中获取场景的三维几何信息。数据采集时，首先由基准gnss接收机稳定连续地接收gnss卫星定位数据，然后移动平台通过接收基站发出的实时差分值来修正当前载体的gnss定位值，从而完成数据采集过程中的地理位置实时精准获取，因此定位精度将直接决定采集到的点云的位置精度。

3、然而在城市“峡谷”中，高耸的建筑物使得车载激光扫描系统观测到的卫星星座分布不佳，玻璃幕墙、绿植茂密等现象会导致gnss信号遮挡、多路径效应严重，使得gnss的定位精度在城市场景中大打折扣。现有城市道路又通常为双向多通道设计模式，在获取数据时通常会在同一条道路的不同车道上往返采集多次，而gnss定位精度下降导致移动测量系统直接地理定向解算的激光点云数据在遇到数据闭环时出现位置不一致现象，表现为同一区域扫描采集得到的数据出现鬼影、断层甚至畸变等现象，严重影响后续数据的处理和实际应用分析。

技术实现思路

1、针对车载激光点云数据中存在的位置偏差问题，本发明提供一种联合多类约束的车载激光点云位姿图优化方法及系统，以分段的车载激光点云数据为基本处理节点单元，通过基于特征点的点云匹配算法获取重访分段之间的同名匹配对应点，并在相邻分段之间设置采样虚拟采样点，分别依据匹配同名点和虚拟点构建具有多类型约束边的位姿图模型，通过全局优化平差，实现城市大场景的点云地图构建。

2、为了达到上述目的，本发明提供的技术方案是一种联合多类约束的车载激光点云位姿图优化方法，包括以下步骤：

3、步骤1，采用自适应分段的方式将原始激光点云数据分割为不重叠的点云分段，并将点云分段抽象为位姿图模型中的节点；

4、步骤2，根据每个分段内点云的坐标得到点云分段的2d包围框，通过计算两个点云分段包围框之间的交并比，获得粗略的分段重访关系；

5、步骤3，采用基于曲率的关键点检测方法获取点云分段中的关键点；

6、步骤4，采用基于局部几何特征的点云特征描述子匹配方法，获取重访分段之间的同名匹配关系；

7、步骤5，根据重访分段之间的同名匹配关系，构建回环约束边；

8、步骤6，在相邻分段之间设置虚拟采样点，并构建平滑约束边；

9、步骤7，通过对位姿图进行优化，实现车载激光点云的全局优化平差。

10、而且，所述步骤1中首先对移动平台的速度、加速度状态信息进行统计学分析，找到速度、加速度变化剧烈的位置作为点云分段候选点，然后把轨迹数据中瞬时方向变化剧烈的轨迹点也作为点云分段候选点，并通过滤波的方式对两部分候选分段点进行非均匀采样，得到最终的分段点。根据最终的分段点，将原始车载激光点云数据分割为长度不等的点云分段，并将各点云分段抽象为位姿图模型中的节点。节点的定义为：

11、s:＝{s1,...,si,...,sn,n＝|s}                  (1)

12、式中，s表示点云分段集合，si表示第i个点云分段，其包含本点云分段对应的位姿t和所包含的点云数据，n和|s|表示全部点云分段的数量。

13、点云分段对应的位姿t定义为：

14、

15、式中，r∈so(3)为旋转矩阵，属于三维特殊正交群空间；为平移向量，属于三维空间；位姿t∈se(3)表示由当前点云分段位姿状态到正确的点云分段位姿状态之间的变换，属于三维特殊欧式群空间。

16、而且，所述步骤2中计算两个点云分段包围框之间的交并比，获得粗略的分段重访关系，得到初始的重访候选分段判断方式为：

17、

18、式中，表示与点云分段si有重访关系的点云分段集合，s表示点云分段集合，n为分段总数，表示由两个点云分段si、sj的包围框计算得到的交并比，δ1为两个点云分段包围框之间的交并比阈值，τ(si,sj)为指示函数，如果点云分段si、sj的交并比值大于阈值δ1，则认为有重访关系，τ(si,sj)＝1，否则不构成重访，τ(si,sj)＝0。

19、而且，所述步骤3中寻找点云分段中每个点周围邻域内的点，组成邻域点集合，利用邻域点集合中的点计算该点的协方差矩阵mi，通过主成分分析法得到邻域点集合的特征值和特征向量，进而计算出该点对应的近似曲率，近似曲率计算公式下：

20、

21、式中，c表示点的曲率，λ1>λ2>λ3为降序排列的特征值。

22、通过局部最大值抑制操作得到局部曲率最大的点作为关键点，关键点定义为：

23、

24、式中，为关键点的集合，表示第t个关键点，|p|表示关键点的个数。

25、而且，所述步骤4中采用基于局部三正交平面投影的方式编码局部几何空间特征，作为关键点的特征描述子，具体计算方式为：搜索当前关键点的局部邻域点，使用主成分分析法计算主方向，以当前关键点为原点，主方向为x轴，垂直于地面向上的方向为y轴，x轴和y轴的正交方向为z轴构建局部独立坐标系，并将局部邻域点变换到该局部独立坐标系下，将邻域点分别投影到三个正交的平面xoy、xoz、yoz上，然后对每个平面的投影点进行栅格化统计，将每个栅格内的点的密度、投影距离归一化到为0-255区间内的值，通过随机特征差异性测试的方法将其进一步转换为0-1区间内的二进制值，并将三个投影平面内的二进制值串接起来作为当前关键点的二进制点云特征描述子。将每个点云分段内的关键点的特征描述子聚合得到该点云分段的特征描述子，聚合方式如下：

26、

27、式中，f(sk)表示点云分段sk的特征描述子集合，∪为取并集操作，k′为点云分段sk中的第k′个关键点，fk′为第k′个关键点的二进制特征描述子，为点云分段sk中包含的关键点的数量。

28、通过计算两个二进制特征描述子之间的汉明距离，并使用双向特征选择的方式即可获取重访分段之间的同名对应关系ωp，即：

29、

30、式中，为点云分段si、sj的关键点集合，pt、qt′分别表示属于关键点集合的关键点，ft、ft′表示关键点pt、qt′的二进制特征描述子，f(si)、f(sj)表示点云分段si、sj的特征描述子集合，minhm(a,b)表示从特征描述子集合b中选出与二进制特征描述子a汉明距离最小的特征对应的关键点。

31、使用svd分解双向特征选择得到的同名匹配关系矩阵，能够得到两个重访点云分段之间的相对位姿变换。

32、而且，所述步骤5中回环约束边代价函数计算公式如下：

33、

34、ωp＝-log(dhm/df)                  (9)

35、式中，er为回环约束边代价函数；n为分段总数；τ为指示函数，如果两个分段之间存在重访关系，则τ为1，否则τ为0；ti,tj,pk,qk分别为分段的位姿和匹配特征同名点；|ωp|为当前重访分段中包含的匹配同名点数量；(·)运算符表示将当前点pk按照位姿ti,tj进行变换，从源坐标系变换到目标坐标系的过程；表示为分段si的重访分段总数；||||2为两个点之间的欧式距离；ωp为某对匹配同名点的权重；df为特征描述子的维度；dhm为两个二进制特征描述子之间的汉明距离；log()表示取对数函数。

36、而且，所述步骤6中以分段点为中心，轨迹线的方向作为起始方向，向左前上、左前下、右前上、右前下、左后上、左后下、右后上、右后下8个方向发射射线，并在8个方向射线上进行多次采样，每次采样使用固定距离，不同批次采样使用的固定距离不同。在分属于不同分段中的采样点之间构建残差连接向量，每次采样的8个点构成一个立方体，根据立方体边长即可获得对应真实向量，从而构建虚拟采样点的平滑约束边代价函数，实现如下：

37、

38、式中，es为平滑约束边代价函数；n为分段总数；nsp为分段点附近的虚拟采样点数量；分别表示分段si-1和si的虚拟采样点；为之间预先定义好的距离向量；ti,ti-1分别为两个分段si和si-1所对应的位姿；(·)运算符表示将当前点按照位姿ti,ti-1进行变换；||||2为两个点之间的欧式距离。

39、而且，所述步骤7中位姿图的最优参数解形式表示如下：

40、t*＝argmaxp(z|t)                 (11)

41、式中，t*表示最优位姿参数解，argmaxp(z|t)表示在满足t条件下观测到z的条件概率的最大值，表示点云分段位姿集合，表示位姿图中的节点，z：＝{zij∈se(3)|(i,j)∈ε}为点云分段之间的观测值集合，ε表示位姿图中的边。

42、构建整个位姿图模型的代价函数对车载激光点云数据进行全局平差，求得最优位姿参数解。整个位姿图模型的代价函数e计算方式如下：

43、e＝ωrer+ωses                  (12)

44、式中，er、es分别为重访分段之间的匹配同名点构成的回环约束边和相邻分段之间的虚拟采样点构成的平滑约束边，ωr、ωs分别为重访代价边和平滑代价边的权重因子。

45、本发明还提供一种联合多类约束的车载激光点云位姿图优化系统，用于实现如上所述的一种联合多类约束的车载激光点云位姿图优化方法。

46、而且，包括处理器和存储器，存储器用于存储程序指令，处理器用于调用存储器中的存储指令执行如上所述的一种联合多类约束的车载激光点云位姿图优化方法。

47、或者，包括可读存储介质，所述可读存储介质上存储有计算机程序，所述计算机程序执行时，实现如上所述的一种联合多类约束的车载激光点云位姿图优化方法。

48、与现有技术相比，本发明具有如下优点：

49、1)本发明利用重访分段之间的同名匹配边以及相邻分段之间虚拟同名点约束边构建多类型约束的位姿图模型，实现了城市大场景空间几何一致的车载激光点云数据地图构建；

50、2)本发明联合分段之间的匹配同名点和相邻分段之间的虚拟采样点，通过对匹配同名点和虚拟采样点设定不同的权重，实现鲁棒的全局位姿图模型求解，所得结果可以全局和局部视角展示；

51、3)本发明对城市场景道路目标重复、对称、结构单一的场景具有较强鲁棒性，针对各种城市道路场景所采集的车载激光点云数据的几何位置精度改善均可取得很好的效果，可以应用于城市高精度点云地图构建，城市森林绿量估计，城市道路分析等重大任务。