增量式地图构建的轨迹接边方法、电子设备和存储介质与流程

1.本发明涉及自动驾驶技术领域，特别是一种用于增量式地图构建的轨迹接边方法、电子设备和机器可读存储介质。


背景技术：

2.由于需要覆盖城市级的大规模地图数据，服务于自动驾驶的高精度地图(high definition map，简称为hd map)往往需要采取多次采集、增量式构建的方式。在增量式构建高精度地图的过程中，为了保证地图的全局一致性，需要对相邻路段的地图数据进行接边处理。接边的成功与否依赖于激光点云的重叠率、初始轨迹的相对和绝对精度等。在复杂的城市场景如高架、城市峡谷等下，由于依赖于gnss(global navigation satellite system，全球卫星导航系统)获取绝对位置的组合导航系统的精度下降，轨迹接边相较于高速场景更为困难。因此，亟需一种可适用于复杂场景，提高轨迹接边的成功率，从而提高增量式地图的精度的轨迹接边方案。


技术实现要素：

3.鉴于上述问题，提出了一种克服上述问题或者至少部分地解决上述问题的用于增量式地图构建的轨迹接边方法、电子设备和机器可读存储介质。
4.本发明的一个目的在于提供一种用于增量式地图构建的轨迹接边方法，可以有效地对局部轨迹进行位姿初值调整，提高激光点云配准的成功率，从而为增量式构建高精度地图提供高精度的参考轨迹。
5.本发明的一个进一步的目的在于进一步提高轨迹接边的处理效率和精度。
6.特别地，根据本发明实施例的一方面，提供了一种用于增量式地图构建的轨迹接边方法，包括：
7.获取地图库数据和待入库数据，所述地图库数据和所述待入库数据均包括含有位姿信息的轨迹和与所述轨迹相应的激光点云；
8.分别从所述待入库数据和所述地图库数据中选取对应的激光点云构成多个点云数据对，并从所述点云数据对中筛选出候选接边点对；
9.对各所述候选接边点对进行初次点云配准，记录匹配成功的接边点对和匹配失败的接边点对，其中，将所述匹配成功的接边点对中属于地图库数据的激光点云和属于待入库数据的激光点云分别称为第一地图库点云和第一待入库点云，将所述匹配失败的接边点对中属于地图库数据的激光点云和属于待入库数据的激光点云分别称为第二地图库点云和第二待入库点云；
10.根据所述初次点云配准的结果和各所述第一地图库点云和各所述第一待入库点云的所述位姿信息计算得到各所述第一待入库点云的位姿变化矩阵，并将各所述第一待入库点云的位姿变化矩阵应用于该第一待入库点云周围设定范围内的各所述第二待入库点云，以将所述第二待入库点云的初始位姿矩阵变化为更新位姿矩阵；
11.基于各所述第二待入库点云的更新位姿矩阵对各所述匹配失败的接边点对进行二次点云配准，并以所述初次点云配准和所述二次点云配准中匹配成功的所有接边点对构成成功匹配点云对集合；
12.基于所述成功匹配点云对集合通过位姿图优化方法对所述待入库数据进行轨迹接边优化得到轨迹文件。
13.可选地，所述根据所述初次点云配准的结果和各所述第一地图库点云和各所述第一待入库点云的所述位姿信息计算得到各所述第一待入库点云的位姿变化矩阵，并将各所述第一待入库点云的位姿变化矩阵应用于该第一待入库点云周围设定范围内的各所述第二待入库点云，以将所述第二待入库点云的初始位姿矩阵变化为更新位姿矩阵的步骤包括：
14.根据所述初次点云配准得到的各所述匹配成功的接边点对的匹配后相对位姿和各所述第一地图库点云的位姿矩阵计算得到各所述第一待入库点云的匹配后位姿矩阵，并根据各所述第一待入库点云的匹配后位姿矩阵和匹配前的初始位姿矩阵计算得到各所述第一待入库点云的位姿变化矩阵；
15.将各所述第一待入库点云的位姿变化矩阵与该第一待入库点云周围设定范围内的各所述第二待入库点云的初始位姿矩阵进行乘法运算得到所述第二待入库点云的更新位姿矩阵。
16.可选地，所述分别从所述待入库数据和所述地图库数据中选取对应的激光点云构成多个点云数据对的步骤包括：
17.从所述待入库数据中选取多个关键帧激光点云；
18.对于各所述关键帧激光点云，根据该关键帧激光点云对应的定位位置在所述地图库数据中搜索到与其距离最近的激光点云，以该关键帧激光点云和搜索到的所述激光点云构成一个所述点云数据对，从而得到多个所述点云数据对。
19.可选地，用于从所述点云数据对中筛选出候选接边点对的筛选条件包括下列至少之一：
20.所述点云数据对中两帧激光点云的初始重叠率；
21.所述点云数据对的激光点云中的激光点数量；
22.所述点云数据对中两帧激光点云是对向车道数据还是同向车道数据；
23.所述点云数据对中两帧激光点云的采集时刻轨迹的相对位置。
24.可选地，所述对各所述候选接边点对进行初次点云配准的步骤包括：
25.对于每一所述候选接边点对，通过迭代最近点算法进行匹配，并判断是否匹配成功；
26.若所述候选接边点对匹配失败，通过全局特征匹配方式对所述候选接边点对再次进行匹配，并判断是否匹配成功；
27.获取所有匹配成功的所述候选接边点对的匹配后相对位姿。
28.可选地，在对所述候选接边点对进行匹配时基于匹配后点云的重叠率判断是否匹配成功。
29.可选地，所述基于各所述第二待入库点云的更新位姿矩阵对各所述匹配失败的接边点对进行二次点云配准的步骤包括：
30.根据各所述第二待入库点云的更新位姿矩阵和各所述第二地图库点云的位姿矩阵计算得到各所述匹配失败的接边点对的更新相对位姿；
31.基于各所述匹配失败的接边点对的更新相对位姿对各所述匹配失败的接边点对进行二次点云配准。
32.可选地，所述基于所述成功匹配点云对集合通过位姿图优化方法对所述待入库数据进行轨迹接边优化得到轨迹文件的步骤包括：
33.从所述待入库数据中选取多个关键帧激光点云；
34.以所述关键帧激光点云的初始位姿矩阵作为待优化变量构成所述位姿图的顶点；
35.以所述成功匹配点云对集合中的各待入库点云的匹配后位姿矩阵构建单边约束条件；
36.以所述关键帧激光点云的初始位姿矩阵构建相邻位姿关系作为双边约束条件；
37.通过非线性优化算法对所述位姿图进行优化求解得到所述关键帧激光点云的优化位姿矩阵，并基于所述关键帧激光点云的优化位姿矩阵生成轨迹接边优化后的轨迹文件。
38.根据本发明实施例的另一方面，还提供了一种电子设备，包括存储器、处理器及存储在所述存储器上并在所述处理器上运行的机器可执行程序，并且所述处理器执行所述机器可执行程序时实现前述任一的用于增量式地图构建的轨迹接边方法。
39.根据本发明实施例的再一方面，还提供了一种机器可读存储介质，其上存储有机器可执行程序，所述机器可执行程序被处理器执行时实现前述任一的用于增量式地图构建的轨迹接边方法。
40.本发明提供的用于增量式地图构建的轨迹接边方法，利用组合导航轨迹的相对一致性，在对候选接边点对进行初次点云配准得到匹配成功的待入库激光点云的位姿变化后，采用相对位姿传递处理方式，将匹配成功的待入库激光点云处的位姿变化传递到其周围设定范围内的匹配失败的待入库激光点云处，以调整匹配失败处的位姿初值，之后再进行二次点云配准，最后利用两次点云配准得到的匹配成功的点云对集合进行轨迹接边优化。通过在局部范围内使匹配成功处的位姿变化传递到匹配失败处，能够有效地对局部轨迹进行位姿初值调整，以有效地提高激光点云配准的成功率，从而提高轨迹接边的成功率，为增量式构建高精度地图提供高精度的参考轨迹。
41.进一步地，本发明提供的用于增量式地图构建的轨迹接边方法，采用待入库数据的关键帧激光点云进行点云配准、相对位姿传递和位姿图优化，有效减少了数据处理量，提高了处理效率。
42.进一步地，本发明提供的用于增量式地图构建的轨迹接边方法，在进行点云配准时采用了迭代最近点算法和全局特征匹配的双重匹配方式，进一步提高了激光点云配准的成功率，从而提高了轨迹接边的处理精度。
43.上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。
44.根据下文结合附图对本发明具体实施例的详细描述，本领域技术人员将会更加明了本发明的上述以及其他目的、优点和特征。
附图说明
45.通过阅读下文优选实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。而且在整个附图中，用相同的参考符号表示相同的部件。在附图中：
46.图1示出了根据本发明一实施例的用于增量式地图构建的轨迹接边方法的流程示意图；
47.图2示出了根据本发明一实施例的对各候选接边点对进行初次点云配准的步骤的流程示意图；
48.图3示出了根据本发明一实施例的基于相对位姿传递的点云配准初值优化步骤的流程示意图；
49.图4示出了根据本发明一实施例的基于相对位姿传递的点云配准初值优化的位姿变化示意图；
50.图5示出了根据本发明一实施例的基于成功匹配点云对集合通过位姿图优化方法对待入库数据进行轨迹接边优化的步骤的流程示意图；
51.图6示出了根据本发明一实施例的轨迹接边优化结果的对比示意图；
52.图7示出了根据本发明一实施例的电子设备的示意性结构框图。
具体实施方式
53.下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。
54.为解决或至少部分解决上述技术问题，本发明实施例提出一种用于增量式地图构建的轨迹接边方法。图1示出了根据本发明一实施例的用于增量式地图构建的轨迹接边方法的流程示意图。参见图1，该方法至少可以包括以下步骤s102至步骤s112。
55.步骤s102，获取地图库数据和待入库数据，地图库数据和待入库数据均包括含有位姿信息的轨迹和与轨迹相应的激光点云。
56.本发明提出的轨迹接边方法的输入来源于两部分：地图库数据和当前待入库数据。每部分数据都包括轨迹和与轨迹相应的激光点云。
57.轨迹可以是利用高精度的组合导航(如惯性组合导航)系统推算得到的，存储有以下信息：时间戳和地心坐标系(如wgs84坐标系)下的位置和姿态信息(即位姿信息)，其中，位置可由纬度、经度和高度描述，姿态可由横滚角、俯仰角和航向角描述。轨迹可以描述为t＝{(si，γi，θi，φi，loni，lati，hi)|i＝1，2，...，n}，轨迹中的位置和姿态信息按照时间戳顺序依次存储，其中si，γi，θi，φi，loni，lati，hi分别表示第i个点对应的时间戳、横滚角、俯仰角、航向角、经度、纬度以及高度，n表示该轨迹包含的位姿点总数。激光点云为轨迹中某些时刻激光雷达采集得到的数据，并可以经过点云去畸变处理。作为输入的地图库数据的选择可以根据待入库数据所覆盖的定位位置信息进行初步筛选。该定位位置信息可以是通过全球定位系统(global positioning system，gps)、北斗卫星导航系统(beidou navigation satellite system，bds)等获得的。
58.步骤s104，分别从待入库数据和地图库数据中选取对应的激光点云构成多个点云数据对，并从点云数据对中筛选出候选接边点对。
59.步骤s106，对各候选接边点对进行初次点云配准，记录匹配成功的接边点对和匹配失败的接边点对。为了描述方便，以下将匹配成功的接边点对中属于地图库数据的激光点云和属于待入库数据的激光点云分别称为第一地图库点云和第一待入库点云，将匹配失败的接边点对中属于地图库数据的激光点云和属于待入库数据的激光点云分别称为第二地图库点云和第二待入库点云。
60.步骤s108，根据初次点云配准的结果和各第一地图库点云和各第一待入库点云的位姿信息计算得到各第一待入库点云的位姿变化矩阵，并将各第一待入库点云的位姿变化矩阵应用于该第一待入库点云周围设定范围内的各第二待入库点云，以将第二待入库点云的初始位姿矩阵变化为更新位姿矩阵。
61.本技术的发明人经过研究发现，由组合导航系统的位姿信息解算得到的连续时间内的局部轨迹可以认为是刚性的。特别是对于惯性组合导航系统，由于惯性组合导航系统的位姿信息解算经过了惯性系统预测和定位信息修正，因此一般可以认为在连续时间内的局部轨迹是刚性的。由此，发明人打破了现有技术的思想桎梏，利用组合导航轨迹的相对一致性进行了相对位姿传递，将匹配成功的待入库激光点云处的位姿变化传递到其周围设定范围内的匹配失败的待入库激光点云处，以调整匹配失败处的位姿初值，从而实现点云配准初值的优化。因此，本步骤也可以称为基于相对位姿传递的点云配准初值优化步骤。
62.步骤s110，基于各第二待入库点云的更新位姿矩阵对各匹配失败的接边点对进行二次点云配准，并以初次点云配准和二次点云配准中匹配成功的所有接边点对构成成功匹配点云对集合。
63.步骤s112，基于成功匹配点云对集合通过位姿图优化方法对待入库数据进行轨迹接边优化得到轨迹文件。
64.本发明实施例提供的用于增量式地图构建的轨迹接边方法，利用组合导航轨迹的相对一致性，在对候选接边点对进行初次点云配准得到匹配成功的待入库激光点云的位姿变化后，采用相对位姿传递处理方式，将匹配成功的待入库激光点云处的位姿变化传递到其周围设定范围内的匹配失败的待入库激光点云处，以调整匹配失败处的位姿初值，之后再进行二次点云配准，最后利用两次点云配准得到的匹配成功的点云对集合进行轨迹接边优化。通过在局部范围内使匹配成功处的位姿变化传递到匹配失败处，能够有效地对局部轨迹进行位姿初值调整，以有效地提高激光点云配准的成功率，从而提高轨迹接边的成功率，为增量式构建高精度地图提供高精度的参考轨迹。
65.通过步骤s102准备好输入数据后，轨迹接边的步骤主要分为以下三部分：初始点云配准、基于相对位姿传递的二次点云配准和位姿图优化。下面将对它们进行具体说明。
66.由于激光点云的数据量一般都很大，因此，在一些实施例中，可以从待入库数据中选取多个关键帧激光点云进行点云配准、相对位姿传递和位姿图优化处理，从而极大地减少数据处理量，提高处理效率。关键帧激光点云可以是反映所采集对象的关键信息的激光点云帧数据，其选取方法可以根据实际应用需求确定。在一种实施方式中，可以按照固定距离间隔选取相应的激光点云作为关键帧激光点云。
67.相应地，在一些实施例中，步骤s104可以进一步实施为：
68.首先，从待入库数据中选取多个关键帧激光点云。关键帧激光点云的选取方式可如前文所述。
69.然后，对于各关键帧激光点云，根据该关键帧激光点云对应的定位位置在地图库数据中搜索到与其距离最近的激光点云，以该关键帧激光点云和搜索到的激光点云构成一个点云数据对，从而得到多个点云数据对。
70.最后，根据预设准则从多个点云数据对中筛选出多个候选接边点对。
71.具体地，可以按照一定的距离和准则选取候选接边点对。以待入库数据中的关键帧激光点云cs为例，根据其对应的定位位置(如gps位置、bds位置等)在地图库数据中搜索到其距离最近的点云数据c
t
，构成一个点云数据对(cs，c
t
)。在得到多个点云数据对(cs，c
t
)后，根据预设准则对其进行筛选，决定哪些点云数据对可以成为候选接边点对。当然，也可以在每得到一个点云数据对(cs，c
t
)后，就根据预设准则决定该点云数据对是否可以成为候选接边点对，最后将所有候选接边点对集合。
72.用于从点云数据对中筛选出候选接边点对的预设准则(也可以称为筛选条件)可以包括下列至少之一：点云数据对中两帧激光点云的初始重叠率、点云数据对的激光点云中的激光点数量、点云数据对中两帧激光点云是对向车道数据还是同向车道数据、点云数据对中两帧激光点云的采集时刻轨迹的相对位置等。在实际应用中，可以综合考虑上述筛选条件，针对对向车道数据和同向车道数据，可采用不同的标准，优先选取初始重叠率高、点云信息丰富(即激光点数量多)、轨迹相对位置近的点云数据对为候选接边点对。例如，对于同向车道数据，选取两帧激光点云的初始重叠率大于第一重叠率阈值、激光点数量大于第一预设数量阈值、轨迹相对位置小于第一距离阈值的点云数据对作为候选接边点对；对于对向车道数据，选取两帧激光点云的初始重叠率大于第二重叠率阈值、激光点数量大于第二预设数量阈值、轨迹相对位置小于第二距离阈值的点云数据对作为候选接边点对，其中，第一重叠率阈值大于第二重叠率阈值，第一距离阈值小于第二距离阈值，第一预设数量阈值与第二预设数量阈值可相同或不同。
73.为描述方便，将候选接边点对集合记为c＝{(c
si
，c
ti
)|i＝1，2，...，n}，其中n为候选接边点对的数目。各候选接边点对对应的初始位姿为其中表示点云c
si
的初始位姿矩阵，t
ti
表示点云c
ti
的位姿矩阵。
74.在选取出候选接边点对后，在步骤s106中针对集合c中的各候选接边点对进行初次点云配准(也可称为初次点云匹配)。在一个实施例中，参见图2所示，对各候选接边点对进行初次点云配准的操作可以包括以下步骤：
75.步骤s1062，对于每一候选接边点对，通过迭代最近点算法进行匹配，并判断是否匹配成功。
76.迭代最近点算法可以采用icp(iterative closest point)算法或gicp(generalized iterative closest point，广义迭代最近点)算法。
77.步骤s1064，若某候选接边点对匹配失败，通过全局特征匹配方式对该候选接边点对再次进行匹配，并判断是否匹配成功。
78.步骤s1066，获取所有匹配成功的候选接边点对的匹配后相对位姿。
79.进一步地，在对候选接边点对进行匹配时可基于匹配后点云的重叠率判断是否匹
配成功。
80.本实施例在进行初次点云配准时采用了迭代最近点算法和全局特征匹配的双重匹配方式，进一步提高了激光点云配准的成功率，从而提高了后续轨迹接边的处理精度。
81.在完成初次点云配准后，以标记方式记录匹配成功的接边点对和匹配失败的接边点对，本领域技术人员应可认识到，此处的匹配失败的接边点对指初次点云配准中最终匹配失败(即通过全局特征匹配再次进行匹配后仍然匹配失败)的接边点对。
82.具体地，标记匹配成功的接边点云对并将其集合记为第一点云对集合s1＝{(c
sj
，c
tj
)|j＝1，2，...，m1}，匹配失败的接边点云对的集合记为第二点云对集合f＝{(c
sk
，c
tk
)|k＝1，2，...，p}，c＝s1+f，其中，m1和p分别为匹配成功的接边点对和匹配失败的接边点对的数目，c
sj
和c
tj
分别表示第一待入库点云和第一地图库点云，c
sk
和c
tk
分别表示第二待入库点云和第二地图库点云。
83.执行完成步骤s106后，得到初次点云配准的结果，其包括匹配成功的接边点对的集合和各匹配成功的接边点对的匹配后相对位姿，以及匹配失败的接边点对的集合。之后，执行步骤s108，根据初次点云配准的结果和各第一待入库点云和各第一地图库点云的位姿信息(具体可为各第一地图库点云的位姿矩阵t
ti
和各第一待入库点云的初始位姿矩阵)计算出各第一待入库点云c
sj
的位姿变化，并将该位姿变化传递给局部范围内的第二待入库点云c
sk
，以实现匹配失败处的第二待入库点云的初始位姿矩阵(不妨称为位姿初值)的调整。
84.在一些实施例中，参见图3所示，步骤s108可以具体实施为以下步骤：
85.步骤s1082，根据初次点云配准得到的各匹配成功的接边点对的匹配后相对位姿和各第一地图库点云的位姿矩阵计算得到各第一待入库点云的匹配后位姿矩阵，并根据各第一待入库点云的匹配后位姿矩阵和匹配前的初始位姿矩阵计算得到各第一待入库点云的位姿变化矩阵。
86.具体地，对于第j个匹配成功的接边点对(c
sj
,c
tj
)，在匹配前该接边点对中第一待入库点云与第一地图库点云的相对位姿为入库点云与第一地图库点云的相对位姿为入库点云与第一地图库点云的相对位姿为为迭代最近点算法的输入，匹配成功后能得到该接边点对中第一待入库点云与第一地图库点云的的匹配后相对位姿作为输出之一，根据可以得出第一待入库点云c
sj
的匹配后位姿矩阵其中t
tj
为第一地图库点云c
tj
的位姿矩阵，为第一待入库点云c
sj
的初始位姿矩阵。由于地图库中的数据可以视为真值，因此根据地图库中第一地图库点云c
tj
的位姿矩阵t
tj
与匹配得到的相对位姿矩阵计算第一待入库点云c
sj
的位姿，即匹配后位姿矩阵t
sj
。进而，根据第一待入库点云c
sj
的匹配后位姿矩阵t
sj
和匹配前的初始位姿矩阵可以计算出匹配前后的第一待入库点云c
sj
的位姿变化矩阵的位姿变化矩阵
87.图4示出了根据本发明一实施例的基于相对位姿传递的点云配准初值优化的位姿变化示意图。在图4中，实线表示地图库数据的轨迹，虚线表示待入库数据的轨迹，线上的点表示相应帧的位姿矩阵。
①
中，白色的点表示匹配失败的接边点对，黑色的点表示匹配成功
的接边点对，且匹配成功后第一待入库点云的位姿从白色点位置更新到黑色点位置(即匹配后位姿矩阵)，如箭头指向所示。
88.步骤s1084，将各第一待入库点云的位姿变化矩阵与该第一待入库点云周围设定范围内的各第二待入库点云的初始位姿矩阵进行乘法运算得到第二待入库点云的更新位姿矩阵。
89.前文提到，本技术的发明人经过研究发现，由组合导航系统的位姿信息解算得到的连续时间内的局部轨迹可以认为是刚性的，特别是对于惯性组合导航系统，由于惯性组合导航系统的位姿信息解算经过了惯性系统预测和定位信息修正，因此一般可以认为在连续时间内的局部轨迹是刚性的。由此，发明人做出以下假设：对于第j个匹配成功的接边点对(c
sj
,c
tj
)，匹配前后的第一待入库点云c
sj
存在位姿变化(即位姿变化矩阵δt
sj
)时，则在点云c
sj
轨迹点位置的周围一定范围内的匹配失败的点云对(具体为接边点对)也应遵循该位姿变化。
90.假设点云c
sj
周围设定范围内的匹配失败的接边点对的集合为n＝{(c
sk
,c
tk
)|k＝j-m,
…
,j-1,j+1,
…
,j+n}，可以将第二待入库点云c
sk
的初始位姿矩阵更新为更新位姿矩阵图4的
②
中黑色点周围的白色点的箭头示意了第二待入库点云的初始位姿矩阵的更新。对于所有匹配成功的第一待入库点云周围设定范围内的匹配失败的第二待入库点云，都更新初始位姿矩阵，这样就可以得到用于后续二次点云配准的优化初值，因此，步骤s108也可以称作基于相对位姿传递的点云配准初值优化步骤。
91.当然，在进行相对位姿传递时，应保证一个第二待入库点云仅应用一次位姿变化。例如，若某一第二待入库点云处于两个或两个以上的第一待入库点云周围的设定范围内，则仅将距离该第二待入库点云最近的第一待入库点云的位姿变化应用于该第二待入库点云上或求取几个第一待入库点云位姿变化的加权平均值应用于该第二待入库点云上。
92.在点云配准初值优化后，执行步骤s110以进行二次点云配准。在一个实施例中，步骤s110中基于各第二待入库点云的更新位姿矩阵对各匹配失败的接边点对进行二次点云配准的操作可以包括以下步骤：
93.首先，根据各第二待入库点云的更新位姿矩阵和各第二地图库点云的位姿矩阵计算得到各匹配失败的接边点对的更新相对位姿。具体地，各匹配失败的接边点对的更新相对位姿可根据下式计算：
94.然后，基于各匹配失败的接边点对的更新相对位姿对各匹配失败的接边点对进行二次点云配准。
95.以各匹配失败的接边点对的更新相对位姿作为输入进行二次点云配准。具体地，二次点云配准可以采用与初次点云配准相同的算法，则更新相对位姿作为迭代最近点算法的输入，输出指示接边点对是否匹配成功的结果以及匹配成功的接边点对的匹配后相对位姿。
96.在二次点云配准中若存在新的匹配成功的接边点对，则标记新的匹配成功的接边点对，并将其加入到匹配成功的接边点对的集合s1中，从而得到包括了初次点云配准和二次点云配准中所有匹配成功的接边点对的成功匹配点云对集合(不妨称为第三点云对集
合)，记为s2＝{(c
sj
，c
tj
)|j＝1，2，...，m2}，其中m2大于或等于m1，对应的匹配成功的待入库点的匹配后位姿矩阵(包括初次点云配准中匹配成功的第一待入库点云在初次点云配准后的匹配后位姿矩阵和二次点云配准中匹配成功的第二待入库点云在二次点云配准后的匹配后位姿矩阵)的集合为w可以作为后续步骤的输入项。图4的
③
中有一些接边点对从白变黑，示意了在点云配准初值优化后，原先无法成功匹配的点云在二次点云配准中能够成功匹配。
97.在得到了成功匹配点云对集合s2后，执行步骤s112，使用图优化方法进行轨迹接边优化。图优化用图模型来表达一个非线性最小二乘优化问题，其中顶点表示待优化变量，边表示误差项。
98.在一些实施例中，参见图5所示，步骤s112可以具体包括以下步骤：
99.步骤s1121，从待入库数据中选取多个关键帧激光点云。
100.步骤s1123，以关键帧激光点云的初始位姿矩阵作为待优化变量构成位姿图的顶点。也即是说，待入库数据的轨迹中的每一个关键帧激光点云的位姿矩阵都是待优化变量，构成位姿图的所有顶点，记顶点集合为v＝{ti|i＝1，2，...，m}，其中ti表示第i个点对应的待优化位姿矩阵，m表示该待优化的位姿点总数。
101.步骤s1125，以成功匹配点云对集合中各待入库点云的匹配后位姿矩阵构建单边约束条件。此处提及的成功匹配点云对集合中各待入库点云包括在初次点云配准中匹配成功的各第一待入库点云和在二次点云配准中匹配成功的各第二待入库点云。记单边约束条件集合为其中，表示轨迹中的第i个点的匹配后位姿矩阵的逆矩阵，
102.步骤s1127，以关键帧激光点云的初始位姿矩阵构建相邻位姿关系作为双边约束条件。
103.具体地，双边约束使用待入库数据的轨迹的关键帧激光点云的初始位姿矩阵为输入构建相邻位姿关系，记双边约束条件集合为其中表示待入库数据的轨迹中的第i个点的初始位姿矩阵的逆矩阵，表示待入库数据的轨迹中的第i+1个点的初始位姿矩阵，表示第i个点对应的待优化位姿矩阵的逆矩阵，t
i+1
表示第i+1个点对应的待优化位姿矩阵。
104.步骤s1129，通过非线性优化算法对位姿图进行优化求解得到关键帧激光点云的优化位姿矩阵，并基于关键帧激光点云的优化位姿矩阵生成轨迹接边优化后的轨迹文件。
105.经过步骤s1123到s1127后，即完成了优化问题的图构建。需要说明的是，步骤s1123到s2227的顺序是可以互换的，或者也可以同时进行。
106.此外，如果在前面的步骤，例如步骤s04中已从待入库数据中选取了关键帧激光点云，也可以省略步骤s1121。
107.构建完位姿图以后，可以选择高斯牛顿法、dogleg法、levenberg-marquardt法等非线性优化算法对位姿图进行优化求解。将优化后的位姿点替换原待入库的轨迹点输出，
生成最终的经过轨迹接边优化后的轨迹文件。
108.本实施例利用基于相对位姿传递的点云配准初值优化后的成功匹配点云对集合进行图优化的单边约束，并结合待入库轨迹的关键帧激光点云的初始位姿矩阵进行图优化的双边约束，可进一步有效提高轨迹接边的成功率。
109.图6示出了根据本发明一实施例的轨迹接边优化结果的对比示意图。图6中左边图片显示了改进前的无相对位姿传递的轨迹接边结果，由于待入库数据的初始轨迹精度较差，拼接后出现明显的错位。右边图片显示了改进后的基于相对位姿传递的轨迹接边结果，在采用基于相对位姿传递的轨迹接边优化方法后，由于局部轨迹得到了有效的位姿初值调整，激光点云配准的成功率提高，拼接后的点云一致性得到明显提升。
110.基于同一发明构思，本发明实施例还提供了一种电子设备200。参见图7所示，该电子设备200包括存储器201、处理器202及存储在存储器201上并在处理器202上运行的机器可执行程序203，并且处理器202执行机器可执行程序203时实现前述任意实施例或实施例组合的用于增量式地图构建的轨迹接边方法。
111.基于同一发明构思，本发明实施例还提供了一种机器可读存储介质，其上存储有机器可执行程序。该机器可执行程序被处理器执行时实现前述任意实施例或实施例组合的用于增量式地图构建的轨迹接边方法。
112.所属领域的技术人员可以清楚地了解到，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，为简洁起见，在此不另赘述。
113.另外，在本发明各个实施例中的各功能单元可以物理上相互独立，也可以两个或两个以上功能单元集成在一起，还可以全部功能单元都集成在一个处理单元中。上述集成的功能单元既可以采用硬件的形式实现，也可以采用软件或者固件的形式实现。
114.本领域普通技术人员可以理解：所述集成的功能单元如果以软件的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，其包括若干指令，用以使得一台计算设备(例如个人计算机，服务器，或者网络设备等)在运行所述指令时执行本发明各实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom)、随机存取存储器(ram)，磁碟或者光盘等各种可以存储程序代码的介质。
115.或者，实现前述方法实施例的全部或部分步骤可以通过程序指令相关的硬件(诸如个人计算机，服务器，或者网络设备等的计算设备)来完成，所述程序指令可以存储于一计算机可读取存储介质中，当所述程序指令被计算设备的处理器执行时，所述计算设备执行本发明各实施例所述方法的全部或部分步骤。
116.最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：在本发明的精神和原则之内，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案脱离本发明的保护范围。