CN112669250B - 一种轨迹对齐方法及电子设备 - Google Patents

一种轨迹对齐方法及电子设备 Download PDF

Info

Publication number
CN112669250B
CN112669250B CN202110278640.9A CN202110278640A CN112669250B CN 112669250 B CN112669250 B CN 112669250B CN 202110278640 A CN202110278640 A CN 202110278640A CN 112669250 B CN112669250 B CN 112669250B
Authority
CN
China
Prior art keywords
node
dimensional point
point cloud
track
cloud map
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.)
Active
Application number
CN202110278640.9A
Other languages
English (en)
Other versions
CN112669250A (zh
Inventor
鲁荣荣
李能
王彦哲
李正伟
包星星
杨再甫
林宝尉
丁亚芬
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hubei Ecarx Technology Co Ltd
Original Assignee
Hubei Ecarx Technology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Hubei Ecarx Technology Co Ltd filed Critical Hubei Ecarx Technology Co Ltd
Priority to CN202110278640.9A priority Critical patent/CN112669250B/zh
Publication of CN112669250A publication Critical patent/CN112669250A/zh
Application granted granted Critical
Publication of CN112669250B publication Critical patent/CN112669250B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Abstract

本发明实施例提供了一种轨迹对齐方法及电子设备，该方法包括：针对第一轨迹中的每个待关联节点，确定该待关联节点的目标节点以及三维点云地图对；再根据对三维点云地图对中三维点的选择指令确定三维点对，根据三维点对对三维点云地图对进行配准，得到三维点云地图对之间的约束关系；基于约束关系，对第一轨迹中各节点的位姿以及第二轨迹中各节点的位姿进行位姿优化，得到对齐轨迹。可见，本方案中，根据选择指令确定的三维点对，对三维点云地图对配准，建立约束关系后，根据约束关系对轨迹进行优化得到对齐轨迹，也就是说本方案中，对三维点云地图对中的匹配三维点对进行筛选，提高了不同轨迹的节点之间匹配的准确率，从而优化了轨迹对齐效果。

Description

一种轨迹对齐方法及电子设备
技术领域
本发明涉及计算机视觉技术领域，特别是涉及一种轨迹对齐方法及电子设备。
背景技术
地图具有多种作用，举例来说，一些场景中，需要使用地图来进行路线规划，例如，在自动驾驶场景中，需要使用地图来规划行车路线；在导航场景中，需要使用地图来规划前往目的地的路径，等等。在使用地图规划路线的情况下，地图的精确度越高，规划出的路线就越准确。或者，一些场景中，需要使用地图来模拟真实环境，例如，在评估绿化程度的场景中，需要使用地图来模拟出真实环境中的植被覆盖率、植被形态等等；在评估居民区密集程度的场景中，需要使用地图来模拟出真实环境中的居民区密集程度，等等。在使用地图模拟真实环境的情况下，地图的精确度越高，对真实环境的模拟效果越真实。
目前，制作地图的方案一般包括：利用组合导航或者SLAM（SimultaneousLocalization And Mapping，即时定位与建图）多次采集轨迹数据以及三维点云数据；对每次采集到的轨迹数据中的各个轨迹节点进行匹配，根据匹配结果对每次采集到的轨迹数据进行对齐，之后根据对齐结果，拼接每次采集到的轨迹数据对应的三维点云数据，得到地图。
然而，上述方案中，由于一些因素的影响，如复杂环境、GPS（Global PositioningSystem，全球定位系统）信号失效以及动态物干扰等，单次采集的轨迹数据存在误差，此外，不同次采集到的轨迹数据之间也存在误差，这就导致在轨迹节点匹配时，会出现匹配不准确的情况，导致轨迹数据的对齐效果不好，使得地图的精确度较低。
发明内容
本发明实施例的目的在于提供一种轨迹对齐方法及电子设备，以优化轨迹对齐效果。具体技术方案如下：
为达到上述目的，本发明实施例提供了一种轨迹对齐方法，包括：
获取第一轨迹和第二轨迹，所述第一轨迹和所述第二轨迹中均包括多个节点及每个节点的位姿和三维点云；
在所述第一轨迹的各个节点中，选择多个待关联节点；针对每个待关联节点，基于该待关联节点与所述第二轨迹中各节点的距离，在所述第二轨迹的各节点中，确定与该待关联节点具有关联关系的目标节点；
将该待关联节点的三维点云与该待关联节点的邻近节点的三维点云进行融合，得到该待关联节点的三维点云地图；将所述目标节点的三维点云与所述目标节点的邻近节点的三维点云进行融合，得到所述目标节点的三维点云地图；将该待关联节点的三维点云地图与所述目标节点的三维点云地图确定为具有关联关系的三维点云地图对；
基于接收到的针对所述三维点云地图对中的三维点的选择指令在所述三维点云地图对中确定预设数量个三维点对；其中，所述三维点对中包括该待关联节点的三维点云地图中的一个三维点与所述目标节点的三维点云地图中的一个三维点；
基于所述预设数量个三维点对，对该待关联节点的三维点云地图和所述目标节点的三维点云地图进行配准，得到该待关联节点的三维点云地图和所述目标节点的三维点云地图之间的约束关系；
基于所述约束关系，对所述第一轨迹中的各节点的位姿以及所述第二轨迹中各节点的位姿进行位姿优化，得到对齐轨迹。
为达到上述目的，本发明实施例还提供了一种电子设备，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；
存储器，用于存放计算机程序；
处理器，用于执行存储器上所存放的程序时，实现上述任一轨迹对齐方法。
应用本发明实施例，针对第一轨迹中的每个待关联节点，确定与该待关联节点具有关联关系的目标节点以及三维点云地图对；再根据针对三维点云地图对中三维点的选择指令，确定预设数量个三维点对，根据预设数量个三维点对对三维点云地图对进行配准，得到待关联节点的三维点云地图和目标节点的三维点云地图之间的约束关系；基于约束关系，对第一轨迹中各节点的位姿以及第二轨迹中各节点的位姿进行位姿优化，得到对齐轨迹。可见，本方案中，根据选择指令确定三维点对，根据确定出的三维点对三维点云地图对进行配准，建立约束关系，之后根据约束关系对轨迹进行优化得到对齐轨迹，也就是说本方案中，对三维点云地图对中的匹配三维点对进行筛选，提高了不同轨迹的节点之间匹配的准确率，从而优化了轨迹对齐效果。
当然，实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的实施例。
图1为本发明实施例提供的轨迹对齐方法的第一种流程示意图；
图2为本发明实施例提供的一种可视化界面的示意图；
图3为本发明实施例提供的轨迹节点间的关联关系示意图；
图4为本发明实施例提供的位姿优化示意图；
图5为本发明实施例提供的轨迹对齐方法的第二种流程示意图；
图6为本发明实施例提供的一种电子设备的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员基于本申请所获得的所有其他实施例，都属于本发明保护的范围。
为了达到上述目的，本发明实施例提供了一种轨迹对齐方法及电子设备，该方法可以应用于各种电子设备，具体不做限定。下面首先对该轨迹对齐方法进行详细介绍。
图1为本发明实施例提供的轨迹对齐方法的第一种流程示意图，包括：
S101：获取第一轨迹和第二轨迹，第一轨迹和第二轨迹中均包括多个节点及每个节点的位姿和三维点云。
节点的位姿可以理解为：节点的位置和节点的姿态角的合称。
举例来说，可以利用组合导航获取轨迹以及轨迹中各节点的位姿。组合导航可以理解为：综合各种导航设备及系统，采集轨迹以及轨迹中各节点的位姿。例如，导航设备可以为：IMU（Inertial Measurement Unit，惯性传感器）以及OD（odometer，里程计），导航系统可以为GPS（Global Positioning System，全球定位系统），这样，结合IMU、OD以及GPS，采集轨迹以及轨迹中各节点对应的位姿的方案可以理解为组合导航。导航设备可以为：IMU、OD、Geomagnetic（地磁场传感器），等等，具体导航设备不做限定；导航系统可以为：GPS、北斗导航，等等，具体导航系统不做限定。
再举一例，可以利用SLAM（Simultaneous Localization And Mapping，即时定位与建图）获取轨迹中各节点的位姿，等等，具体获取轨迹中各节点的位姿的方式不做限定。
举例来说，可以利用激光雷达获取轨迹中各节点对应的三维点云。获取轨迹中各节点对应的三维点云的方式可以为：利用激光雷达获取轨迹中各节点对应的三维点云或者利用惯性导航采集轨迹中各节点对应的三维点云，等等，具体获取轨迹中各节点对应的三维点云的方式不做限定。
一种实施方式中，S101可以包括：获取两条待处理轨迹，待处理轨迹中包括多个节点及每个节点的位姿，节点中包括以下任意一种或多种：经度、纬度、高度、姿态角；分别计算每个节点的经度的平均值、纬度的平均值以及高度的平均值，作为几何重心的经度、纬度以及高度；确定几何重心对应的子午圈主曲率半径和卯酉圈主曲率半径；针对每个节点，基于子午圈主曲率半径、卯酉圈主曲率半径、该节点的经度、纬度、高度以及该节点的姿态角，对该节点的位姿进行标准化，将任意一条标准化后的待处理轨迹确定为第一轨迹，将另一条标准化后的待处理轨迹确定为第二轨迹。
举例来说，几何重心可以理解为各节点的中心点，几何重心的坐标可以通过计算各节点的经度、纬度和高度的平均值得到；确定几何重心对应的子午圈主曲率半径和卯酉圈主曲率半径；针对每个节点，基于子午圈主曲率半径、卯酉圈主曲率半径、该节点的经度、纬度、高度以及该节点的姿态角，对该节点的位姿矩阵进行标准化，得到标准化后的待处理轨迹。
举例来说，可以利用如下算式计算几何重心的经度：
Figure 896527DEST_PATH_IMAGE001
其中，long表示几何重心的经度；A表示两条待处理轨迹中的任意一条轨迹；B表示两条待处理轨迹中的另一条轨迹；m表示轨迹A中包含的节点数；n表示轨迹B中包含的节点数；i表示轨迹A中节点的序号，i可以为任意不大于m的正整数；loni表示轨迹A中第i个节点的经度；j表示轨迹B中节点的序号，j可以为任意不大于n的正整数；lonj表示轨迹B中第j个节点的经度。
举例来说，可以利用如下算式计算几何重心的纬度：
Figure 393760DEST_PATH_IMAGE002
其中，latg表示几何重心的纬度；A表示两条待处理轨迹中的任意一条轨迹；B表示两条待处理轨迹中的另一条轨迹；m表示轨迹A中包含的节点数；n表示轨迹B中包含的节点数；i表示轨迹A中节点的序号，i可以为任意不大于m的正整数；lati表示轨迹A中第i个节点的纬度；j表示轨迹B中节点的序号，j可以为任意不大于n的正整数；latj表示轨迹B中第j个节点的纬度。
举例来说，可以利用如下算式计算几何重心的高度：
Figure 970235DEST_PATH_IMAGE003
其中，heightg表示几何重心的高度；A表示两条待处理轨迹中的任意一条轨迹；B表示两条待处理轨迹中的另一条轨迹；m表示轨迹A中包含的节点数；n表示轨迹B中包含的节点数；i表示轨迹A中节点的序号，i可以为任意不大于m的正整数；heighti表示轨迹A中第i个节点的高度；j表示轨迹B中节点的序号，j可以为任意不大于n的正整数；heightj表示轨迹B中第j个节点的高度。
这样，可以得到几何重心的坐标（long，latg，heightg），以几何重心为原点，建立三维局部坐标系，对两条待处理轨迹中各节点的原始坐标进行变换。例如，可以利用如下算式计算得到经坐标变换后，两条待处理轨迹中各节点在三维局部坐标系中的坐标：
Figure 178494DEST_PATH_IMAGE004
，
Figure 992866DEST_PATH_IMAGE005
，
Figure 877645DEST_PATH_IMAGE006
，
其中，xi表示第i个节点在三维局部坐标系中的横坐标，loni表示第i个节点的经度，long表示几何重心的经度，Rnh表示几何重心的子午圈主曲率半径，latg表示几何重心的纬度，yi表示第i个节点在三维局部坐标系中的纵坐标，lati表示第i个节点的纬度，Rmh表示几何重心的卯酉圈主曲率半径，zi表示第i个节点在三维局部坐标系中的竖坐标，heighti表示第i个节点的高度，heightg表示几何重心的高度。
在确定了轨迹中各节点经坐标变换后在三维局部坐标系中的坐标之后，可以利用如下算式，计算各节点的标准化后的位姿矩阵：
Figure 144679DEST_PATH_IMAGE007
其中，Ri表示旋转矩阵，ti表示第i个节点的位置矩阵，
Figure 655163DEST_PATH_IMAGE008
，
Figure 386359DEST_PATH_IMAGE009
其中，αi表示第i个节点的横滚角，βi表示第i个节点的俯仰角，γi表示第i个节点的偏航角。
对两条待处理轨迹的位姿进行标准化后，可以将任意一条标准化后的待处理轨迹确定为第一轨迹，将另一条标准化后的待处理轨迹确定为第二轨迹。
应用本实施方式，对轨迹中各节点的位姿进行标准化，也就是说将两条待处理轨迹中各节点的位置与姿态角转换到三维局部坐标系中，在后续对轨迹进行对齐的过程中，使用标准化后的位姿进行轨迹对齐，相比于直接利用原始坐标进行对齐的方式而言，提高了对齐的精度，此外，利用标准化后的位姿也简化了计算的步骤，降低了计算量。
S102：在第一轨迹的各个节点中，选择多个待关联节点；针对每个待关联节点，基于该待关联节点与第二轨迹中各节点的距离，在第二轨迹的各节点中，确定与该待关联节点具有关联关系的目标节点。
一种实施方式中，可以将第一轨迹的全部节点作为待关联节点。
或者，另一种实施方式中，节点中包括时间戳，在第一轨迹的各个节点中，选择多个待关联节点可以包括：根据所述时间戳的顺序，在第二轨迹的各个节点中，将时间最早的节点确定为当前待确定节点；判断是否存在与当前待确定节点具有关联关系的目标节点；若存在，则将当前待确定节点确定为待关联节点；在第二轨迹的各个节点中，确定除与所确定的待关联节点之间的距离在第二预设阈值之外的节点，依次作为下一待确定节点；判断是否存在与下一待确定节点具有关联关系的目标节点；若存在，则将下一待确定节点确定为待关联节点。
举例来说，可以按照第一轨迹中的各节点的时间戳顺序，将时间最早的节点确定为当前待确定节点，判断是否存在与该当前待确定节点具有关联关系的目标节点；若存在，则将当前待确定节点确定为待关联节点；在第一轨迹的各个节点中，按照时间戳顺序，依次将除与所确定的待关联节点之间的距离在第二预设阈值之外的节点确定为下一待确定节点；判断是否存在与下一待确定节点具有关联关系的目标节点；若存在，则将下一待确定节点确定为待关联节点。判断是否存在与待确定节点具有关联关系的目标节点的具体方式可以参考下述实施方式。第二预设阈值可以为10米、15米，等等，具体第二预设阈值不做限定。
一些相关方案中，针对第一轨迹中的所有节点均确定与其具有关联关系的目标节点，后续在获取某节点对应的三维地图对时，需要获取该节点附近的其他节点的三维点云，则该相关方案中，需要针对所有节点均获取三维地图对，则三维地图对的重复率较高，使得后续优化的效率较低。
应用本实施方式，在判定某一节点存在具有关联关系的目标节点的情况下，针对与该节点的距离在第二预设阈值之外的节点确定具有关联关系的目标节点。也就是说，本实施方式中，并不针对所有节点确定目标节点，而是在第一轨迹中将具有一定距离间隔的点确定为待关联节点，这样，在后续获取三维地图对时，只需获取待关联节点对应的三维地图对，减少了需要获取的数据量，此外，减少了后续优化过程中的数据量，提高了优化效率和鲁棒性。
一种实施方式中，S102可以包括：针对第一轨迹中的每个待关联节点，将第二轨迹的各个节点中的与该待关联节点距离最近的节点确定为候选节点；判断候选节点与该待关联节点之间的距离是否大于第一预设阈值；若否，则将候选节点确定为与该待关联节点具有关联关系的目标节点。
举例来说，可以针对第一轨迹中的每个待关联节点，将第二轨迹的各个节点中与该待关联节点距离最近的节点确定为候选节点，参考图3所示，轨迹A表示第一轨迹，轨迹B表示第二轨迹，轨迹A中包括节点
Figure 379723DEST_PATH_IMAGE010
、节点
Figure 947101DEST_PATH_IMAGE011
、节点
Figure 684113DEST_PATH_IMAGE012
、节点
Figure 269815DEST_PATH_IMAGE013
以及节点
Figure 699660DEST_PATH_IMAGE014
，轨迹B中包括节点
Figure 267518DEST_PATH_IMAGE015
、节点
Figure 542642DEST_PATH_IMAGE016
、节点
Figure 248429DEST_PATH_IMAGE017
以及节点
Figure 583596DEST_PATH_IMAGE018
。针对待关联节点
Figure 125567DEST_PATH_IMAGE010
而言，在轨迹B的各个节点中与
Figure 204381DEST_PATH_IMAGE010
距离最近的节点为
Figure 764676DEST_PATH_IMAGE015
，可以将
Figure 536322DEST_PATH_IMAGE015
确定为
Figure 798545DEST_PATH_IMAGE010
的候选节点；针对待关联节点
Figure 681051DEST_PATH_IMAGE012
而言，在轨迹B的各个节点中与
Figure 95852DEST_PATH_IMAGE012
距离最近的节点为
Figure 38400DEST_PATH_IMAGE016
，可以将
Figure 289384DEST_PATH_IMAGE016
确定为
Figure 710001DEST_PATH_IMAGE012
的候选节点。
S103：将该待关联节点的三维点云与该待关联节点的邻近节点的三维点云进行融合，得到该待关联节点的三维点云地图；将目标节点的三维点云与目标节点的邻近节点的三维点云进行融合，得到目标节点的三维点云地图。
一种实施方式中，S103可以包括：在第一轨迹的各个节点中，识别与该待关联节点的距离在第三预设阈值之内的节点，作为第一邻近节点；将第一邻近节点的三维点云与该待关联节点的三维点云进行融合，得到该待关联节点的三维点云地图。在第二轨迹的各个节点中，识别与目标节点的距离在第三预设阈值之内的节点，作为第二邻近节点；将第二邻近节点的三维点云与目标节点的三维点云进行融合，得到目标节点的三维点云地图。
举例来说，若第三预设阈值为15米，那么可以在第一轨迹的各个节点中，将与该待关联节点之间的距离在15米内的节点确定为第一邻近节点。依然参考图3所示，针对
Figure 244887DEST_PATH_IMAGE012
节点而言，若
Figure 358337DEST_PATH_IMAGE012
和
Figure 598082DEST_PATH_IMAGE010
的距离为10米，
Figure 822390DEST_PATH_IMAGE012
和
Figure 946203DEST_PATH_IMAGE011
的距离为5米，
Figure 230554DEST_PATH_IMAGE012
和
Figure 456130DEST_PATH_IMAGE013
的距离为4米，
Figure 484129DEST_PATH_IMAGE012
和
Figure 665712DEST_PATH_IMAGE014
的距离为20米，那么可以确定
Figure 917702DEST_PATH_IMAGE010
、
Figure 817524DEST_PATH_IMAGE011
和
Figure 695220DEST_PATH_IMAGE013
为
Figure 996888DEST_PATH_IMAGE012
的第一邻近节点。可以将
Figure 419779DEST_PATH_IMAGE010
、
Figure 806898DEST_PATH_IMAGE011
、
Figure 989749DEST_PATH_IMAGE012
和
Figure 880344DEST_PATH_IMAGE013
的三维点云进行融合，得到
Figure 739716DEST_PATH_IMAGE012
的三维点云地图。第三预设阈值可以为15米、20米，等等，具体第三预设阈值不做限定。
举例来说，可以利用如下算式计算待关联节点的第一邻近节点与待关联节点之间的刚体变换矩阵：
Figure 348552DEST_PATH_IMAGE019
其中，Tig表示第i个待关联节点和第i个待关联节点的第g个第一邻近节点之间的刚体变换矩阵，
Figure 836558DEST_PATH_IMAGE020
表示轨迹A中的第i个待关联节点的位姿矩阵，
Figure 847240DEST_PATH_IMAGE021
表示轨迹A中第i个待关联节点的第g个第一邻近节点的位姿矩阵。
计算得到待关联节点与该待关联节点的第一邻近节点之间的刚体变换矩阵后，可以对第一邻近节点的三维点云进行该第一邻近节点对应的刚体变换，得到刚体变换后的第一邻近节点的三维点云，再将刚体变换后的第一邻近节点的三维点云与该待关联节点的三维点云进行融合，得到该待关联节点的三维点云地图。
举例来说，若第三预设阈值为15米，那么可以在第二轨迹的各个节点中，将与目标节点之间的距离在15米内的节点确定为第二邻近节点。依然参考图3所示，针对
Figure 877513DEST_PATH_IMAGE016
节点而言，若
Figure 973645DEST_PATH_IMAGE016
和
Figure 685249DEST_PATH_IMAGE015
的距离为16米，
Figure 363486DEST_PATH_IMAGE016
和
Figure 502343DEST_PATH_IMAGE017
的距离为7米，
Figure 148088DEST_PATH_IMAGE016
和
Figure 397804DEST_PATH_IMAGE018
的距离为8米，那么可以确定和
Figure 694662DEST_PATH_IMAGE017
和
Figure 4420DEST_PATH_IMAGE018
为
Figure 137461DEST_PATH_IMAGE016
的第二邻近节点。可以将
Figure 190868DEST_PATH_IMAGE016
、
Figure 843697DEST_PATH_IMAGE017
和
Figure 324357DEST_PATH_IMAGE018
的三维点云进行融合，得到
Figure 679115DEST_PATH_IMAGE016
的三维点云地图。第三预设阈值可以为15米、20米，等等，具体第三预设阈值不做限定。
举例来说，可以利用如下算式计算目标节点的第二邻近节点与目标节点之间的刚体变换矩阵：
Figure 536213DEST_PATH_IMAGE022
其中，Tig表示第i个目标节点和第i个目标节点的第g个第二邻近节点之间的刚体变换矩阵，
Figure 545014DEST_PATH_IMAGE023
表示轨迹B中的第i个目标节点的位姿矩阵，
Figure 196575DEST_PATH_IMAGE024
表示轨迹B中第i个目标节点的第g个第二邻近节点的位姿矩阵。
计算得到目标节点与目标节点的第二邻近节点之间的刚体变换矩阵后，可以对第二邻近节点的三维点云进行该第二邻近节点对应的刚体变换，得到刚体变换后的第二邻近节点的三维点云，再将刚体变换后的第二邻近节点的三维点云与目标节点的三维点云进行融合，得到目标节点的三维点云地图。
或者，另一种实施方式中，S103可以包括：在第一轨迹的各个节点中，识别与该待关联节点的时间戳的时间间隔在预设时长阈值之内的节点，作为第一邻近节点；将第一邻近节点的三维点云与该待关联节点的三维点云进行融合，得到该待关联节点的三维点云地图。在第二轨迹的各个节点中，识别与目标节点的时间戳的时间间隔在预设时长阈值之内的节点，作为第二邻近节点；将第二邻近节点的三维点云与目标节点的三维点云进行融合，得到目标节点的三维点云地图。
举例来说，若预设时长阈值为15秒，那么可以在第一轨迹的各个节点中，将与该待关联节点之间的距离在15秒内的节点确定为第一邻近节点。针对
Figure 304208DEST_PATH_IMAGE012
节点而言，若
Figure 699417DEST_PATH_IMAGE012
的时间戳和
Figure 248210DEST_PATH_IMAGE010
的时间戳的时间间隔为10秒，
Figure 149301DEST_PATH_IMAGE012
的时间戳和
Figure 416335DEST_PATH_IMAGE011
的时间戳的时间间隔为5秒，
Figure 677552DEST_PATH_IMAGE012
的时间戳和
Figure 80851DEST_PATH_IMAGE013
的时间戳的时间间隔为4秒，
Figure 651379DEST_PATH_IMAGE012
的时间戳和
Figure 405708DEST_PATH_IMAGE014
的时间戳的时间间隔为20秒，那么可以确定
Figure 205037DEST_PATH_IMAGE010
、
Figure 728422DEST_PATH_IMAGE011
和
Figure 971316DEST_PATH_IMAGE013
为
Figure 947362DEST_PATH_IMAGE012
的第一邻近节点。可以将
Figure 550382DEST_PATH_IMAGE010
、
Figure 193853DEST_PATH_IMAGE011
、
Figure 831814DEST_PATH_IMAGE012
和
Figure 560736DEST_PATH_IMAGE013
的三维点云进行融合，得到
Figure 701867DEST_PATH_IMAGE012
的三维点云地图。预设时长阈值可以为15秒、20秒，等等，具体预设时长阈值不做限定。
举例来说，若预设时长阈值为15秒，那么可以在第二轨迹的各个节点中，将与目标节点之间的距离在15秒内的节点确定为第二邻近节点。针对
Figure 199845DEST_PATH_IMAGE016
节点而言，若
Figure 518962DEST_PATH_IMAGE016
的时间戳和
Figure 735179DEST_PATH_IMAGE015
的时间戳的时间间隔为16秒，
Figure 680002DEST_PATH_IMAGE016
的时间戳和
Figure 32486DEST_PATH_IMAGE017
的时间戳的时间间隔为7秒，
Figure 286618DEST_PATH_IMAGE016
的时间戳和
Figure 724553DEST_PATH_IMAGE018
的时间戳的时间间隔为8秒，那么可以确定和
Figure 207487DEST_PATH_IMAGE017
和
Figure 680057DEST_PATH_IMAGE018
为
Figure 340976DEST_PATH_IMAGE016
的第二邻近节点。可以将
Figure 266207DEST_PATH_IMAGE016
、
Figure 490515DEST_PATH_IMAGE017
和
Figure 879908DEST_PATH_IMAGE018
的三维点云进行融合，得到
Figure 164259DEST_PATH_IMAGE016
的三维点云地图。预设时长阈值可以为15秒、20秒，等等，具体预设时长阈值不做限定。
S104：将该待关联节点的三维点云地图与目标节点的三维点云地图确定为具有关联关系的三维点云地图对。
举例来说，可以将S103中得到的该待关联节点的三维地图和目标节点的三维点云地图确定为具有关联关系的三维点云地图对。
S105：基于接收到的针对三维点云地图对中的三维点的选择指令在三维点云地图对中确定预设数量个三维点对；其中，三维点对中包括该待关联节点的三维点云地图中的一个三维点与目标节点的三维点云地图中的一个三维点。
一种实施方式中，S105可以包括：在可视化界面中展示三维点云地图对，接收用户对所展示的三维点云地图中的三维点的选择指令；根据选择指令，确定用户在三维点云地图中选择的三维点对。
可视化界面可以参考图2所示，可视化界面中包括三维点云地图对可视化区域以及五个功能标识，五个功能标识分别为：加载三维点云地图对、选三维点、下一个、保存结果以及丢弃。例如，用户选择“加载三维点云地图对”之后，三维点云地图对可以在可视化界面中的三维点云地图对可视化区域进行展示；用户选择“选三维点”之后，可以在该三维点云地图展示区中展示的三维点云地图中，选择所展示的三维点云地图中的三维点。图中“下一个”、“保存结果”与“丢弃”功能，在后续实施方式中再进行详细介绍。图2中展示的界面布局以及各功能标识仅为举例说明，并不构成限定。
举例来说，可以在触摸式显示终端中加载可视化界面，或者可以在按键式显示终端中加载可视化界面，等等，具体显示终端的类型不做限定。以在触摸式显示终端中加载可视化界面为例，用户可以点触可视化界面所展示的三维点云地图对中的三维点，在用户点触显示终端后，可以根据用户在显示终端上的点触点的位置，确定用户针对三维点云地图对中的三维点的选择指令。
或者，另一种实施方式中，S105可以包括：针对三维点云地图对中的每个三维点分配一个标识，在显示终端展示三维点云地图对以及三维点云地图对中三维点的标识，获取用户针对三维点的标识的选择指令，作为用户对所展示的三维点云地图中的三维点的选择指令。
举例来说，可以在显示终端展示三维点云地图对以及三维点云地图对中三维点的标识，用户可以将选择的三维点对应的标识输入至显示终端，可以根据用户输入的三维点的标识，确定用户针对三维点云地图对中的三维点的选择指令
举例来说，可以将三维点云地图对展示在可视化界面中，用户对三维点云地图对中的三维点进行选择，用户可以在三维点云地图对中选出预设数量个三维点对，其中，三维点对中包括待关联节点的三维点云地图中的一个三维点与目标节点的三维点云地图中的一个三维点。例如，用户可以在三维点云地图对中选择三个三维点对，选择的三维点可以为具有明显特征的点，如树干分叉点、标识牌的角点，等等，具体用户选择的三维点不做限定。预设数量可以为不少于3的正整数，如3、4，等等。
S106：基于预设数量个三维点对，对该待关联节点的三维点云地图和目标节点的三维点云地图进行配准，得到该待关联节点的三维点云地图和目标节点的三维点云地图之间的约束关系。
一种实施方式中，S106可以包括：基于预设数量个三维点对，对该待关联节点的三维点云地图和目标节点的三维点云地图进行配准，得到配准结果及其对应的刚体变换矩阵；在可视化界面中展示配准结果，接收用户对所展示的配准结果的判断指令，判断指令为通过指令或继续指令；若判断指令为通过指令，则将配准结果对应的刚体变换矩阵确定为该待关联节点的三维点云地图和目标节点的三维点云地图之间的约束关系；若判断指令为继续指令，则返回基于接收到的针对三维点云地图对中的三维点的选择指令在三维点云地图对中确定预设数量个三维点对的步骤，直至接收到通过指令。
一种实施方式中，基于预设数量个三维点对，对该待关联节点的三维点云地图和目标节点的三维点云地图进行配准，得到配准结果及其对应的刚体变换矩阵，可以包括：分别计算预设数量个三维点对中的预设数量个第一三维点的几何重心和预设数量个第二三维点的几何重心，第一三维点为：该待关联节点的三维点云地图中的三维点，第二三维点为：目标节点的三维点云地图中的三维点；基于第一三维点的几何重心和第二三维点的几何重心，计算预设数量个三维点对的协方差矩阵；根据协方差矩阵，得到刚体变换矩阵；根据刚体变换矩阵，对该待关联节点的三维点云地图进行刚体变换，将刚体变换后的该待关联节点的三维点云地图与目标节点的三维点云地图，确定为配准结果。
举例来说，若S105中用户选择了三个三维点对，假设从待关联节点的三维点云地图中选择的三维点为
Figure 891300DEST_PATH_IMAGE025
，从目标节点的三维点云地图中选择的三维点为
Figure 653719DEST_PATH_IMAGE026
，i=1，2，3。可以利用如下算式，分别计算
Figure 897619DEST_PATH_IMAGE025
的几何重心和
Figure 352871DEST_PATH_IMAGE026
的几何重心：
Figure 65743DEST_PATH_IMAGE027
，
Figure 631854DEST_PATH_IMAGE028
，其中
Figure 730260DEST_PATH_IMAGE029
其中，
Figure 356413DEST_PATH_IMAGE030
表示
Figure 55116DEST_PATH_IMAGE025
的几何重心，
Figure 159339DEST_PATH_IMAGE031
表示
Figure 377830DEST_PATH_IMAGE026
的几何重心，
Figure 174885DEST_PATH_IMAGE032
表示
Figure 596770DEST_PATH_IMAGE025
的横坐标，
Figure 770263DEST_PATH_IMAGE033
表示
Figure 843261DEST_PATH_IMAGE025
的纵坐标，
Figure 811217DEST_PATH_IMAGE034
表示
Figure 907349DEST_PATH_IMAGE025
的竖坐标。
举例来说，可以利用如下算式计算三个三维点对的协方差矩阵：
Figure 667888DEST_PATH_IMAGE035
其中，M表示协方差矩阵。
一种情况下，根据协方差矩阵，得到刚体变换矩阵，可以包括：对协方差矩阵进行奇异值分解，得到粗匹配位姿变换矩阵；基于迭代最近邻算法对粗匹配位姿变换矩阵进行迭代调整，得到刚体变换矩阵。
举例来说，可以利用如下算式对协方差矩阵进行奇异值分解，得到粗匹配位姿变换矩阵：
Figure 798655DEST_PATH_IMAGE036
，其中，
Figure 999829DEST_PATH_IMAGE037
其中，
Figure 583257DEST_PATH_IMAGE038
表示待关联节点的三维点云地图与目标节点的三维点云地图之间的刚体变换矩阵，M表示协方差矩阵，U表示M的左奇异矩阵，V表示M的右奇异矩阵，
Figure 646022DEST_PATH_IMAGE039
表示
Figure 631296DEST_PATH_IMAGE025
的几何重心，
Figure 3371DEST_PATH_IMAGE031
表示
Figure 74096DEST_PATH_IMAGE026
的几何重心。
举例来说，可以基于迭代最近邻算法对粗匹配位姿变换矩阵
Figure 439087DEST_PATH_IMAGE036
进行迭代调整，得到刚体变换矩阵。或者，也可以基于G-ICP（Generalized-Iterative Closest Point，广义迭代最近邻算法）对粗匹配位姿变换矩阵
Figure 278867DEST_PATH_IMAGE036
进行迭代调整，得到刚体变换矩阵。具体不做限定。
或者，一种情况下，可以将上述对协方差矩阵进行奇异值分解，得到的粗匹配位姿变换矩阵作为待关联节点的三维点云地图与目标节点的三维点云地图之间的刚体变换矩阵。
确定了待关联节点的三维点云地图与目标节点的三维点云地图之间的刚体变换矩阵之后，可以将待关联节点的三维点云地图与该刚体变换矩阵相乘，得到配准结果。
在得到配准结果后，可以在可视化界面中展示配准结果，接收用户对所展示的配准结果的判断指令，判断指令为通过指令或继续指令；若判断指令为通过指令，则将配准结果对应的刚体变换矩阵确定为该待关联节点的三维点云地图和目标节点的三维点云地图之间的约束关系。
一种情况下，若判断指令为继续指令，则返回基于接收到的针对三维点云地图对中的三维点的选择指令在三维点云地图对中确定预设数量个三维点对的步骤，直至接收到通过指令。
或者，另一种情况下，若判断指令为继续指令，则判断判断指令为继续指令的次数是否大于预设次数阈值；若否，则执行返回基于接收到的针对三维点云地图对中的三维点的选择指令在三维点云地图对中确定预设数量个三维点对的步骤；若是，则舍弃配准结果，并返回基于接收到的针对三维点云地图对中的三维点的选择指令在三维点云地图对中确定预设数量个三维点对的步骤。
举例来说，预设次数阈值可以为5次、7次，等等，具体预设次数阈值不做限定。例如，在每次用户的判断指令为继续指令时，对继续指令进行计次，并判断继续指令的次数是否大于预设次数阈值，若预设次数阈值为5次，那么若本次继续指令为第6次继续指令，那么可以舍弃该配准结果，并将下一待关联节点的三维点云地图对展示在可视化界面中，并接收用户对这一三维点云地图对中三维点的选择指令，根据这一选择指令确定用户在下一待关联节点的三维点云地图中选择的三维点对。
参考图2所示的可视化界面，用户点击“加载三维点云地图对”可以理解为用户开始对三维点云地图对中的三维点开始选择；用户点击“下一个”或点击“保存结果”可以理解为判断指令为通过指令；用户点击“选三维点”可以理解为判断指令为继续指令；用户点击“丢弃”可以理解为用户选择舍弃配准结果。
S107：基于约束关系，对第一轨迹中各节点的位姿以及第二轨迹中各节点的位姿进行位姿优化，得到对齐轨迹。
一种实施方式中，S107可以包括：以第一轨迹中各节点的位姿以及第二轨迹中各节点的位姿作为优化变量，构建最小优化误差函数，最小优化误差函数包括第一优化误差、第二优化误差和第三优化误差，第一优化误差包含第一轨迹中各个相邻节点间的相对位姿误差以及所述第二轨迹中各个相邻节点间的相对位姿误差，第二优化误差包含第一轨迹中预先确定的自环节点间的相对位姿误差以及第二轨迹中预先确定的自环节点间的相对位姿误差，第三优化误差包含各具有关联关系的待关联节点和目标节点之间的相对位姿误差；求解最小优化误差函数，确定第一轨迹中各节点的优化后位姿以及第二轨迹中各节点的优化后位姿，得到对齐轨迹。
参考图4所示，轨迹A表示第一轨迹，轨迹A中包括节点
Figure 821844DEST_PATH_IMAGE010
、节点
Figure 114285DEST_PATH_IMAGE011
、节点
Figure 784431DEST_PATH_IMAGE012
、节点
Figure 478718DEST_PATH_IMAGE013
、节点
Figure 192596DEST_PATH_IMAGE014
、节点
Figure 237912DEST_PATH_IMAGE040
以及节点
Figure 633122DEST_PATH_IMAGE041
，轨迹B表示第二轨迹，轨迹B中包括节点
Figure 496429DEST_PATH_IMAGE015
、节点
Figure 584471DEST_PATH_IMAGE016
、节点
Figure 913821DEST_PATH_IMAGE017
、节点
Figure 847142DEST_PATH_IMAGE018
、节点
Figure 329070DEST_PATH_IMAGE042
以及节点
Figure 588013DEST_PATH_IMAGE043
。针对轨迹A中的
Figure 139080DEST_PATH_IMAGE011
这一节点而言，节点
Figure 141671DEST_PATH_IMAGE012
为节点
Figure 976641DEST_PATH_IMAGE011
的一阶邻边，节点
Figure 140906DEST_PATH_IMAGE013
为节点
Figure 444848DEST_PATH_IMAGE011
的二阶邻边，针对节点
Figure 985551DEST_PATH_IMAGE014
这一节点而言，节点
Figure 176492DEST_PATH_IMAGE040
为节点
Figure 777238DEST_PATH_IMAGE014
的一阶邻边，针对节点
Figure 302897DEST_PATH_IMAGE015
这一节点而言，节点
Figure 647290DEST_PATH_IMAGE017
为节点
Figure 471501DEST_PATH_IMAGE015
的二阶邻边，节点与其一阶邻边及二阶邻边之间的相对位姿误差可以理解为第一优化误差；针对轨迹A以及轨迹B中的每个节点，该节点可以理解为该节点的自环节点，或者说该节点为自身的自环边，自环节点之间的相对位姿误差可以理解为第二优化误差；轨迹A中的节点
Figure 977569DEST_PATH_IMAGE040
与轨迹B中的节点
Figure 256103DEST_PATH_IMAGE042
为具有关联关系的节点对，所以节点
Figure 138608DEST_PATH_IMAGE042
为节点
Figure 304142DEST_PATH_IMAGE040
的关联边，节点与其关联边之间的相对位姿误差可以理解为第三优化误差。
举例来说，最小优化误差函数可以为：
Figure 981111DEST_PATH_IMAGE044
其中，E表示最小优化误差函数，ξ表示第一轨迹以及第二轨迹中各节点的位姿的李代数，i、j和k表示第一轨迹以及第二轨迹中节点的序号，ε表示具有关联关系的节点对、一阶邻边节点对和二阶邻边节点对的集合，eij表示第i个节点和第j个节点之间的相对位姿误差，
Figure 481362DEST_PATH_IMAGE045
表示预设的针对eij的第一信息矩阵，ek表示自环节点之间的相对位姿误差，Ω表示自环节点的集合，
Figure 167558DEST_PATH_IMAGE046
表示预设的针对ek的第二信息矩阵，(.)T表示矩阵转置，(.)-1表示矩阵求逆。
其中，第i节点和第j节点之间的相对位姿误差eij通过如下公式进行计算：
Figure 686133DEST_PATH_IMAGE047
其中，ΔTij表示根据第i个节点和第j个节点的优化前的位姿计算出的第i个节点和第j个节点之间的相对位姿，Ti表示第i个节点的待优化的位姿，Tj表示第j个节点的待优化的位姿；ξij表示ΔTij的李代数，ξi表示Ti的李代数，ξj表示Tj的李代数。
为了推导相对位姿误差对两端待优化的位姿的雅各比矩阵，对相对位姿误差eij施加左乘扰动，有
Figure 799583DEST_PATH_IMAGE048
利用伴随性质，
Figure 787130DEST_PATH_IMAGE049
，可将上式改写为
Figure 11438DEST_PATH_IMAGE050
其中，
Figure 338515DEST_PATH_IMAGE051
,
Figure 435915DEST_PATH_IMAGE052
，
Figure 582862DEST_PATH_IMAGE053
，
Figure 673178DEST_PATH_IMAGE054
。
自环节点的相对位姿误差ek通过如下公式进行计算：
Figure 854761DEST_PATH_IMAGE055
，其中
Figure 624527DEST_PATH_IMAGE056
；
Tk表示第一轨迹以及第二轨迹中的第k个节点的待优化的位姿，
Figure 524350DEST_PATH_IMAGE057
表示第一轨迹以及第二轨迹中的第k个节点的初始位姿，ξk表示Tk的李代数，(.)-1表示矩阵求逆。
上述相对位姿误差ek对待优化位姿的求导表示为：
Figure 152777DEST_PATH_IMAGE058
,
Figure 188866DEST_PATH_IMAGE059
，
Figure 628069DEST_PATH_IMAGE060
。
进而可以求解上述构建的最小优化误差函数，得到第一轨迹以及第二轨迹中各节点的优化后位姿。例如，可以采用非线性优化方法进行求解，例如采用非线性最小二乘法、一阶梯度和二阶梯度法等。
或者，另一种实施方式中，S107可以包括：利用优化数据库对第一轨迹中各节点的位姿进行位姿优化，得到对齐轨迹。
举例来说，优化数据库可以为：Ceres（谷神星）库、g2o（General GraphOptimization，通用图优化）图优化库，等等，具体优化数据库不做限定。
应用本发明实施例，针对第一轨迹中的每个待关联节点，确定与该待关联节点具有关联关系的目标节点以及三维点云地图对；再根据针对三维点云地图对中三维点的选择指令，确定预设数量个三维点对，根据预设数量个三维点对对三维点云地图对进行配准，得到待关联节点的三维点云地图和目标节点的三维点云地图之间的约束关系；基于约束关系，对第一轨迹中各节点的位姿以及第二轨迹中各节点的位姿进行位姿优化，得到对齐轨迹。可见，本方案中，根据选择指令确定三维点对，根据确定出的三维点对三维点云地图对进行配准，建立约束关系，之后根据约束关系对轨迹进行优化得到对齐轨迹，也就是说本方案中，对三维点云地图对中的匹配三维点对进行筛选，提高了不同轨迹的节点之间匹配的准确率，从而优化了轨迹对齐效果。
图5为本发明实施例提供的轨迹对齐方法的第二种流程示意图，包括：
S501：获取第一轨迹和第二轨迹，第一轨迹和第二轨迹中均包括多个节点及每个节点的位姿和三维点云。
节点的位姿可以理解为：节点的位置和节点的姿态角的合称。
举例来说，可以利用组合导航获取轨迹以及轨迹中各节点的位姿。组合导航可以理解为：综合各种导航设备及系统，采集轨迹以及轨迹中各节点的位姿。例如，导航设备可以为：IMU（Inertial Measurement Unit，惯性传感器）以及OD（odometer，里程计），导航系统可以为GPS（Global Positioning System，全球定位系统），这样，结合IMU、OD以及GPS，采集轨迹以及轨迹中各节点对应的位姿的方案可以理解为组合导航。导航设备可以为：IMU、OD、Geomagnetic（地磁场传感器），等等，具体导航设备不做限定；导航系统可以为：GPS、北斗导航，等等，具体导航系统不做限定。
再举一例，可以利用SLAM（Simultaneous Localization And Mapping，即时定位与建图）获取轨迹中各节点的位姿，等等，具体获取轨迹中各节点的位姿的方式不做限定。
举例来说，可以利用激光雷达获取轨迹中各节点对应的三维点云。获取轨迹中各节点对应的三维点云的方式可以为：利用激光雷达获取轨迹中各节点对应的三维点云、利用惯性导航采集轨迹中各节点对应的三维点云，等等，具体获取轨迹中各节点对应的三维点云的方式不做限定。
一种实施方式中，S501可以包括：获取两条待处理轨迹，待处理轨迹中包括多个节点及每个节点的位姿，节点中包括以下任意一种或多种：经度、纬度、高度、姿态角；分别计算每个节点的经度的平均值、纬度的平均值以及高度的平均值，作为几何重心的经度、纬度以及高度；确定几何重心对应的子午圈主曲率半径和卯酉圈主曲率半径；针对每个节点，基于子午圈主曲率半径、卯酉圈主曲率半径、该节点的经度、纬度、高度以及该节点的姿态角，对该节点的位姿进行标准化，将任意一条标准化后的待处理轨迹确定为第一轨迹，将另一条标准化后的待处理轨迹确定为第二轨迹。
举例来说，几何重心可以理解为各节点的中心点，几何重心的坐标可以通过计算各节点的经度、纬度和高度的平均值得到；确定几何重心对应的子午圈主曲率半径和卯酉圈主曲率半径；针对每个节点，基于子午圈主曲率半径、卯酉圈主曲率半径、该节点的经度、纬度、高度以及该节点的姿态角，对该节点的位姿矩阵进行标准化，得到标准化后的待处理轨迹。
举例来说，可以利用如下算式计算几何重心的经度：
Figure 749609DEST_PATH_IMAGE061
其中，long表示几何重心的经度；A表示两条待处理轨迹中的任意一条轨迹；B表示两条待处理轨迹中的另一条轨迹；m表示轨迹A中包含的节点数；n表示轨迹B中包含的节点数；i表示轨迹A中节点的序号，i可以为任意不大于m的正整数；loni表示轨迹A中第i个节点的经度；j表示轨迹B中节点的序号，j可以为任意不大于n的正整数；lonj表示轨迹B中第j个节点的经度。
举例来说，可以利用如下算式计算几何重心的纬度：
Figure 181727DEST_PATH_IMAGE062
其中，latg表示几何重心的纬度；A表示两条待处理轨迹中的任意一条轨迹；B表示两条待处理轨迹中的另一条轨迹；m表示轨迹A中包含的节点数；n表示轨迹B中包含的节点数；i表示轨迹A中节点的序号，i可以为任意不大于m的正整数；lati表示轨迹A中第i个节点的纬度；j表示轨迹B中节点的序号，j可以为任意不大于n的正整数；latj表示轨迹B中第j个节点的纬度。
举例来说，可以利用如下算式计算几何重心的高度：
Figure 337902DEST_PATH_IMAGE063
其中，heightg表示几何重心的高度；A表示两条待处理轨迹中的任意一条轨迹；B表示两条待处理轨迹中的另一条轨迹；m表示轨迹A中包含的节点数；n表示轨迹B中包含的节点数；i表示轨迹A中节点的序号，i可以为任意不大于m的正整数；heighti表示轨迹A中第i个节点的高度；j表示轨迹B中节点的序号，j可以为任意不大于n的正整数；heightj表示轨迹B中第j个节点的高度。
这样，可以得到几何重心的坐标（long，latg，heightg），以几何重心为原点，建立三维局部坐标系，对两条待处理轨迹中各节点的原始坐标进行变换。例如，可以利用如下算式计算得到经坐标变换后，两条待处理轨迹中各节点在三维局部坐标系中的坐标：
Figure 446541DEST_PATH_IMAGE004
，
Figure 55377DEST_PATH_IMAGE005
，
Figure 963290DEST_PATH_IMAGE006
，
其中，xi表示第i个节点在三维局部坐标系中的横坐标，loni表示第i个节点的经度，long表示几何重心的经度，Rnh表示几何重心的子午圈主曲率半径，latg表示几何重心的纬度，yi表示第i个节点在三维局部坐标系中的纵坐标，lati表示第i个节点的纬度，Rmh表示几何重心的卯酉圈主曲率半径，zi表示第i个节点在三维局部坐标系中的竖坐标，heighti表示第i个节点的高度，heightg表示几何重心的高度。
在确定了轨迹中各节点经坐标变换后在三维局部坐标系中的坐标之后，可以利用如下算式，计算各节点的标准化后的位姿矩阵：
Figure 36288DEST_PATH_IMAGE007
其中，Ri表示旋转矩阵，ti表示第i个节点的位置矩阵，
Figure 4244DEST_PATH_IMAGE008
，
Figure 913426DEST_PATH_IMAGE064
其中，αi表示第i个节点的横滚角，βi表示第i个节点的俯仰角，γi表示第i个节点的偏航角。
对两条待处理轨迹的位姿进行标准化后，可以将任意一条标准化后的待处理轨迹确定为第一轨迹，将另一条标准化后的待处理轨迹确定为第二轨迹。
应用本实施方式，对轨迹中各节点的位姿进行标准化，也就是说将两条待处理轨迹中各节点的位置与姿态角转换到三维局部坐标系中，在后续对轨迹进行对齐的过程中，使用标准化后的位姿进行轨迹对齐，相比于直接利用原始坐标进行对齐的方式而言，提高了对齐的精度，此外，利用标准化后的位姿也简化了计算的步骤，降低了计算量。
S502：针对第一轨迹中的每个待关联节点，将第二轨迹的各个节点中的与该待关联节点距离最近的节点确定为候选节点。
一种实施方式中，节点中包括时间戳，确定第一轨迹中的待关联节点的步骤可以包括：根据所述时间戳的顺序，在第二轨迹的各个节点中，将时间最早的节点确定为当前待确定节点；判断是否存在与当前待确定节点具有关联关系的目标节点；若存在，则将当前待确定节点确定为待关联节点；在第二轨迹的各个节点中，确定除与所确定的待关联节点之间的距离在第二预设阈值之外的节点，依次作为下一待确定节点；判断是否存在与下一待确定节点具有关联关系的目标节点；若存在，则将下一待确定节点确定为待关联节点。
举例来说，可以按照第一轨迹中的各节点的时间戳顺序，将时间最早的节点确定为当前待确定节点，判断是否存在与该当前待确定节点具有关联关系的目标节点；若存在，则将当前待确定节点确定为待关联节点；在第一轨迹的各个节点中，按照时间戳顺序，依次将除与所确定的待关联节点之间的距离在第二预设阈值之外的节点确定为下一待确定节点；判断是否存在与下一待确定节点具有关联关系的目标节点；若存在，则将下一待确定节点确定为待关联节点。判断是否存在与待确定节点具有关联关系的目标节点的具体方式可以参考下述实施方式。第二预设阈值可以为10米、15米，等等，具体第二预设阈值不做限定。
一些相关方案中，针对第一轨迹中的所有节点均确定与其具有关联关系的目标节点，后续在获取某节点对应的三维地图对时，需要获取该节点附近的其他节点的三维点云，则该相关方案中，需要针对所有节点均获取三维地图对，则三维地图对的重复率较高，使得后续优化的效率较低。
应用本实施方式，在判定某一节点存在具有关联关系的目标节点的情况下，针对与该节点的距离在第二预设阈值之外的节点确定具有关联关系的目标节点。也就是说，本实施方式中，并不针对所有节点确定目标节点，而是在第一轨迹中将具有一定距离间隔的点确定为待关联节点，这样，在后续获取三维地图对时，只需获取待关联节点对应的三维地图对，减少了需要获取的数据量，此外，减少了后续优化过程中的数据量，提高了优化效率和鲁棒性。
参考图3所示，轨迹A表示第一轨迹，轨迹B表示第二轨迹，轨迹A中包括节点
Figure 625030DEST_PATH_IMAGE010
、节点
Figure 818114DEST_PATH_IMAGE011
、节点
Figure 956971DEST_PATH_IMAGE012
、节点
Figure 589334DEST_PATH_IMAGE013
以及节点
Figure 104629DEST_PATH_IMAGE014
，轨迹B中包括节点
Figure 152219DEST_PATH_IMAGE015
、节点
Figure 461978DEST_PATH_IMAGE016
、节点
Figure 80172DEST_PATH_IMAGE017
以及节点
Figure 133579DEST_PATH_IMAGE018
。针对待关联节点
Figure 973359DEST_PATH_IMAGE010
而言，在轨迹B的各个节点中与
Figure 516336DEST_PATH_IMAGE010
距离最近的节点为
Figure 74356DEST_PATH_IMAGE015
，可以将
Figure 977459DEST_PATH_IMAGE015
确定为
Figure 937325DEST_PATH_IMAGE010
的候选节点；针对待关联节点
Figure 651203DEST_PATH_IMAGE012
而言，在轨迹B的各个节点中与
Figure 430940DEST_PATH_IMAGE012
距离最近的节点为
Figure 639199DEST_PATH_IMAGE016
，可以将
Figure 453571DEST_PATH_IMAGE016
确定为
Figure 338350DEST_PATH_IMAGE012
的候选节点。
S503：判断候选节点与该待关联节点之间的距离是否大于第一预设阈值。若否，可以执行S504。
举例来说，可以判断候选节点与该待关联节点之间的距离是否大于第一预设阈值，若第一预设阈值为15米，那么，若
Figure 605383DEST_PATH_IMAGE010
与
Figure 118798DEST_PATH_IMAGE015
之间的距离为20米，
Figure 787677DEST_PATH_IMAGE012
与
Figure 843357DEST_PATH_IMAGE016
之间的距离为10米，则
Figure 597687DEST_PATH_IMAGE010
与
Figure 600278DEST_PATH_IMAGE015
之间的距离大于第一预设阈值，
Figure 936712DEST_PATH_IMAGE012
与
Figure 100977DEST_PATH_IMAGE016
之间的距离不大于第一预设阈值，可以执行S504。第一预设阈值可以为15米、20米，等等，具体第一预设阈值不做限定。
S504：将候选节点确定为与该待关联节点具有关联关系的目标节点。
举例来说，S503中确定
Figure 404920DEST_PATH_IMAGE012
与
Figure 945622DEST_PATH_IMAGE016
之间的距离不大于第一预设阈值，则
Figure 635099DEST_PATH_IMAGE012
与
Figure 235844DEST_PATH_IMAGE016
具有关联关系，可以确定
Figure 761504DEST_PATH_IMAGE016
为
Figure 105897DEST_PATH_IMAGE012
的目标节点。
S505：将该待关联节点的三维点云与该待关联节点的邻近节点的三维点云进行融合，得到该待关联节点的三维点云地图；将目标节点的三维点云与目标节点的邻近节点的三维点云进行融合，得到目标节点的三维点云地图。
一种实施方式中，S505可以包括：在第一轨迹的各个节点中，识别与该待关联节点的距离在第三预设阈值之内的节点，作为第一邻近节点；将第一邻近节点的三维点云与该待关联节点的三维点云进行融合，得到该待关联节点的三维点云地图。在第二轨迹的各个节点中，识别与目标节点的距离在第三预设阈值之内的节点，作为第二邻近节点；将第二邻近节点的三维点云与目标节点的三维点云进行融合，得到目标节点的三维点云地图。
举例来说，若第三预设阈值为15米，那么可以在第二轨迹的各个节点中，将与该待关联节点之间的距离在15米内的节点确定为第一邻近节点。依然参考图3所示，针对
Figure 151345DEST_PATH_IMAGE012
节点而言，若
Figure 922992DEST_PATH_IMAGE012
和
Figure 935947DEST_PATH_IMAGE010
的距离为10米，
Figure 818452DEST_PATH_IMAGE012
和
Figure 473732DEST_PATH_IMAGE011
的距离为5米，
Figure 416280DEST_PATH_IMAGE012
和
Figure 916531DEST_PATH_IMAGE013
的距离为4米，
Figure 602728DEST_PATH_IMAGE012
和
Figure 809718DEST_PATH_IMAGE014
的距离为20米，那么可以确定
Figure 736217DEST_PATH_IMAGE010
、
Figure 661448DEST_PATH_IMAGE011
和
Figure 948072DEST_PATH_IMAGE013
为
Figure 275149DEST_PATH_IMAGE012
的第一邻近节点。可以将
Figure 605505DEST_PATH_IMAGE010
、
Figure 18031DEST_PATH_IMAGE011
、
Figure 108347DEST_PATH_IMAGE012
和
Figure 289930DEST_PATH_IMAGE013
的三维点云进行融合，得到
Figure 558231DEST_PATH_IMAGE012
的三维点云地图。第三预设阈值可以为15米、20米，等等，具体第三预设阈值不做限定。
举例来说，可以利用如下算式计算待关联节点的第一邻近节点与待关联节点之间的刚体变换矩阵：
Figure 192475DEST_PATH_IMAGE019
其中，Tig表示第i个待关联节点和第i个待关联节点的第g个第一邻近节点之间的刚体变换矩阵，
Figure 820902DEST_PATH_IMAGE020
表示轨迹A中的第i个待关联节点的位姿矩阵，
Figure 122571DEST_PATH_IMAGE021
表示轨迹A中第i个待关联节点的第g个第一邻近节点的位姿矩阵。
计算得到待关联节点与该待关联节点的第一邻近节点之间的刚体变换矩阵后，可以对第一邻近节点的三维点云进行该第一邻近节点对应的刚体变换，得到刚体变换后的第一邻近节点的三维点云，再将刚体变换后的第一邻近节点的三维点云与该待关联节点的三维点云进行融合，得到该待关联节点的三维点云地图。
举例来说，若第三预设阈值为15米，那么可以在第二轨迹的各个节点中，将与目标节点之间的距离在15米内的节点确定为第二邻近节点。依然参考图3所示，针对
Figure 63238DEST_PATH_IMAGE016
节点而言，若
Figure 184778DEST_PATH_IMAGE016
和
Figure 616896DEST_PATH_IMAGE015
的距离为16米，
Figure 773071DEST_PATH_IMAGE016
和
Figure 383175DEST_PATH_IMAGE017
的距离为7米，
Figure 992011DEST_PATH_IMAGE016
和
Figure 899924DEST_PATH_IMAGE018
的距离为8米，那么可以确定和
Figure 972923DEST_PATH_IMAGE017
和
Figure 940879DEST_PATH_IMAGE018
为
Figure 348595DEST_PATH_IMAGE016
的第二邻近节点。可以将
Figure 122516DEST_PATH_IMAGE016
、
Figure 987704DEST_PATH_IMAGE017
和
Figure 939610DEST_PATH_IMAGE018
的三维点云进行融合，得到
Figure 523038DEST_PATH_IMAGE016
的三维点云地图。第三预设阈值可以为15米、20米，等等，具体第三预设阈值不做限定。
举例来说，可以利用如下算式计算目标节点的第二邻近节点与目标节点之间的刚体变换矩阵：
Figure 100650DEST_PATH_IMAGE065
其中，Tig表示第i个目标节点和第i个目标节点的第g个第二邻近节点之间的刚体变换矩阵，
Figure 820345DEST_PATH_IMAGE023
表示轨迹B中的第i个目标节点的位姿矩阵，
Figure 444617DEST_PATH_IMAGE024
表示轨迹B中第i个目标节点的第g个第二邻近节点的位姿矩阵。
计算得到目标节点与目标节点的第二邻近节点之间的刚体变换矩阵后，可以对第二邻近节点的三维点云进行该第二邻近节点对应的刚体变换，得到刚体变换后的第二邻近节点的三维点云，再将刚体变换后的第二邻近节点的三维点云与目标节点的三维点云进行融合，得到目标节点的三维点云地图。
或者，另一种实施方式中，S505可以包括：在第一轨迹的各个节点中，识别与该待关联节点的时间戳的时间间隔在预设时长阈值之内的节点，作为第一邻近节点；将第一邻近节点的三维点云与该待关联节点的三维点云进行融合，得到该待关联节点的三维点云地图。在第二轨迹的各个节点中，识别与目标节点的时间戳的时间间隔在预设时长阈值之内的节点，作为第二邻近节点；将第二邻近节点的三维点云与目标节点的三维点云进行融合，得到目标节点的三维点云地图。
举例来说，若预设时长阈值为15秒，那么可以在第一轨迹的各个节点中，将与该待关联节点之间的距离在15秒内的节点确定为第一邻近节点。针对
Figure 515342DEST_PATH_IMAGE012
节点而言，若
Figure 631065DEST_PATH_IMAGE012
的时间戳和
Figure 470845DEST_PATH_IMAGE010
的时间戳的时间间隔为10秒，
Figure 764554DEST_PATH_IMAGE012
的时间戳和
Figure 322575DEST_PATH_IMAGE011
的时间戳的时间间隔为5秒，
Figure 976410DEST_PATH_IMAGE012
的时间戳和
Figure 670696DEST_PATH_IMAGE013
的时间戳的时间间隔为4秒，
Figure 587837DEST_PATH_IMAGE012
的时间戳和
Figure 679159DEST_PATH_IMAGE014
的时间戳的时间间隔为20秒，那么可以确定
Figure 74368DEST_PATH_IMAGE010
、
Figure 951057DEST_PATH_IMAGE011
和
Figure 773519DEST_PATH_IMAGE013
为
Figure 853602DEST_PATH_IMAGE012
的第一邻近节点。可以将
Figure 52502DEST_PATH_IMAGE010
、
Figure 783698DEST_PATH_IMAGE011
、
Figure 777062DEST_PATH_IMAGE012
和
Figure 328129DEST_PATH_IMAGE013
的三维点云进行融合，得到
Figure 65140DEST_PATH_IMAGE012
的三维点云地图。预设时长阈值可以为15秒、20秒，等等，具体预设时长阈值不做限定。
举例来说，若预设时长阈值为15秒，那么可以在第二轨迹的各个节点中，将与目标节点之间的距离在15秒内的节点确定为第二邻近节点。针对
Figure 588526DEST_PATH_IMAGE016
节点而言，若
Figure 598463DEST_PATH_IMAGE016
的时间戳和
Figure 840089DEST_PATH_IMAGE015
的时间戳的时间间隔为16秒，
Figure 928262DEST_PATH_IMAGE016
的时间戳和
Figure 571733DEST_PATH_IMAGE017
的时间戳的时间间隔为7秒，
Figure 969216DEST_PATH_IMAGE016
的时间戳和
Figure 698138DEST_PATH_IMAGE018
的时间戳的时间间隔为8秒，那么可以确定和
Figure 88537DEST_PATH_IMAGE017
和
Figure 586514DEST_PATH_IMAGE018
为
Figure 154899DEST_PATH_IMAGE016
的第二邻近节点。可以将
Figure 105537DEST_PATH_IMAGE016
、
Figure 988042DEST_PATH_IMAGE017
和
Figure 419155DEST_PATH_IMAGE018
的三维点云进行融合，得到
Figure 361703DEST_PATH_IMAGE016
的三维点云地图。预设时长阈值可以为15秒、20秒，等等，具体预设时长阈值不做限定。
S506：将该待关联节点的三维点云地图与目标节点的三维点云地图确定为具有关联关系的三维点云地图对。
举例来说，可以将S505中得到的该待关联节点的三维地图和目标节点的三维点云地图确定为具有关联关系的三维点云地图对。
S507：在可视化界面中展示三维点云地图对，接收用户对所展示的三维点云地图中的三维点的选择指令；根据选择指令，确定用户在三维点云地图中选择的三维点对；其中，三维点对中包括该待关联节点的三维点云地图中的一个三维点与目标节点的三维点云地图中的一个三维点。
可视化界面可以参考图2所示，可视化界面中包括三维点云地图对可视化区域以及五个功能标识，五个功能标识分别为：加载三维点云地图对、选三维点、下一个、保存结果以及丢弃。例如，用户选择“加载三维点云地图对”之后，三维点云地图对可以在可视化界面中的三维点云地图对可视化区域进行展示；用户选择“选三维点”之后，可以在该三维点云地图展示区中展示的三维点云地图中，选择所展示的三维点云地图中的三维点。图中“下一个”、“保存结果”与“丢弃”功能，在后续实施方式中再进行详细介绍。图2中展示的界面布局以及各功能标识仅为举例说明，并不构成限定。
举例来说，可以在触摸式显示终端中加载可视化界面，或者可以在按键式显示终端中加载可视化界面，等等，具体显示终端的类型不做限定。以在触摸式显示终端中加载可视化界面为例，用户可以点触可视化界面所展示的三维点云地图对中的三维点，在用户点触显示终端后，可以根据用户在显示终端上的点触点的位置，确定用户针对三维点云地图对中的三维点的选择指令。
举例来说，可以将三维点云地图对展示在可视化界面中，用户对三维点云地图对中的三维点进行选择，用户可以在三维点云地图对中选出预设数量个三维点对，其中，三维点对中包括待关联节点的三维点云地图中的一个三维点与目标节点的三维点云地图中的一个三维点。例如，用户可以在三维点云地图对中选择三个三维点对，选择的三维点可以为具有明显特征的点，如树干分叉点、标识牌的角点，等等，具体用户选择的三维点不做限定。预设数量可以为不少于3的正整数，如3、4，等等。
S508：基于预设数量个三维点对，对该待关联节点的三维点云地图和目标节点的三维点云地图进行配准，得到配准结果及其对应的刚体变换矩阵；在可视化界面中展示配准结果，接收用户对所展示的配准结果的判断指令。
举例来说，若S507中用户选择了三个三维点对，假设从待关联节点的三维点云地图中选择的三维点为
Figure 861955DEST_PATH_IMAGE025
，从目标节点的三维点云地图中选择的三维点为
Figure 282572DEST_PATH_IMAGE026
，i=1，2，3。可以利用如下算式，分别计算
Figure 551879DEST_PATH_IMAGE025
的几何重心和
Figure 665328DEST_PATH_IMAGE026
的几何重心：
Figure 182371DEST_PATH_IMAGE066
，
Figure 406679DEST_PATH_IMAGE067
，其中
Figure 264914DEST_PATH_IMAGE068
其中，
Figure 549265DEST_PATH_IMAGE039
表示
Figure 961791DEST_PATH_IMAGE025
的几何重心，
Figure 68419DEST_PATH_IMAGE031
表示
Figure 561586DEST_PATH_IMAGE026
的几何重心，
Figure 547996DEST_PATH_IMAGE032
表示
Figure 447819DEST_PATH_IMAGE025
的横坐标，
Figure 92558DEST_PATH_IMAGE069
表示
Figure 394227DEST_PATH_IMAGE025
的纵坐标，
Figure 551539DEST_PATH_IMAGE034
表示
Figure 938658DEST_PATH_IMAGE025
的竖坐标。
举例来说，可以利用如下算式计算三个三维点对的协方差矩阵：
Figure 370776DEST_PATH_IMAGE035
其中，M表示协方差矩阵。
一种情况下，根据协方差矩阵，得到刚体变换矩阵，可以包括：对协方差矩阵进行奇异值分解，得到粗匹配位姿变换矩阵；基于迭代最近邻算法对粗匹配位姿变换矩阵进行迭代调整，得到刚体变换矩阵。
举例来说，可以利用如下算式对协方差矩阵进行奇异值分解，得到粗匹配位姿变换矩阵：
Figure 261372DEST_PATH_IMAGE036
，其中，
Figure 638520DEST_PATH_IMAGE070
其中，
Figure 247356DEST_PATH_IMAGE038
表示待关联节点的三维点云地图与目标节点的三维点云地图之间的刚体变换矩阵，M表示协方差矩阵，U表示M的左奇异矩阵，V表示M的右奇异矩阵，
Figure 217586DEST_PATH_IMAGE030
表示
Figure 228267DEST_PATH_IMAGE025
的几何重心，
Figure 274851DEST_PATH_IMAGE031
表示
Figure 167721DEST_PATH_IMAGE026
的几何重心。
举例来说，可以基于迭代最近邻算法对粗匹配位姿变换矩阵
Figure 879325DEST_PATH_IMAGE036
进行迭代调整，得到刚体变换矩阵。或者，也可以基于G-ICP（Generalized-Iterative Closest Point，广义迭代最近邻算法）对粗匹配位姿变换矩阵
Figure 56097DEST_PATH_IMAGE036
进行迭代调整，得到刚体变换矩阵。具体不做限定。
或者，一种情况下，可以将上述对协方差矩阵进行奇异值分解，得到的粗匹配位姿变换矩阵作为待关联节点的三维点云地图与目标节点的三维点云地图之间的刚体变换矩阵。
确定了待关联节点的三维点云地图与目标节点的三维点云地图之间的刚体变换矩阵之后，可以将待关联节点的三维点云地图与该刚体变换矩阵相乘，得到配准结果。
在得到配准结果后，可以在可视化界面中展示配准结果，接收用户对所展示的配准结果的判断指令，判断指令为通过指令或继续指令。
S509：判断用户的判断指令是否为通过指令。若是，可以执行S510；若否，可以执行S507。
S510：将配准结果对应的刚体变换矩阵确定为该待关联节点的三维点云地图和目标节点的三维点云地图之间的约束关系。
S511：基于约束关系，对第一轨迹中各节点的位姿以及第二轨迹中各节点的位姿进行位姿优化，得到对齐轨迹。
一种实施方式中，S511可以包括：以第一轨迹中各节点的位姿以及第二轨迹中各节点的位姿作为优化变量，构建最小优化误差函数，最小优化误差函数包括第一优化误差、第二优化误差和第三优化误差，第一优化误差包含第一轨迹中各个相邻节点间的相对位姿误差以及所述第二轨迹中各个相邻节点间的相对位姿误差，第二优化误差包含第一轨迹中预先确定的自环节点间的相对位姿误差以及第二轨迹中预先确定的自环节点间的相对位姿误差，第三优化误差包含各具有关联关系的待关联节点和目标节点之间的相对位姿误差；求解最小优化误差函数，确定第一轨迹中各节点的优化后位姿以及第二轨迹中各节点的优化后位姿，得到对齐轨迹。
参考图4所示，轨迹A表示第一轨迹，轨迹A中包括节点
Figure 194955DEST_PATH_IMAGE010
、节点
Figure 857011DEST_PATH_IMAGE011
、节点
Figure 106727DEST_PATH_IMAGE012
、节点
Figure 888738DEST_PATH_IMAGE013
、节点
Figure 198497DEST_PATH_IMAGE014
、节点
Figure 331538DEST_PATH_IMAGE040
以及节点
Figure 384945DEST_PATH_IMAGE041
，轨迹B表示第二轨迹，轨迹B中包括节点
Figure 224725DEST_PATH_IMAGE015
、节点
Figure 19899DEST_PATH_IMAGE016
、节点
Figure 312340DEST_PATH_IMAGE017
、节点
Figure 231754DEST_PATH_IMAGE018
、节点
Figure 926041DEST_PATH_IMAGE042
以及节点
Figure 656231DEST_PATH_IMAGE043
。针对轨迹A中的
Figure 701547DEST_PATH_IMAGE011
这一节点而言，节点
Figure 893494DEST_PATH_IMAGE012
为节点
Figure 442287DEST_PATH_IMAGE011
的一阶邻边，节点
Figure 841913DEST_PATH_IMAGE013
为节点
Figure 108946DEST_PATH_IMAGE011
的二阶邻边，针对节点
Figure 370163DEST_PATH_IMAGE014
这一节点而言，节点
Figure 773463DEST_PATH_IMAGE040
为节点
Figure 845455DEST_PATH_IMAGE014
的一阶邻边，针对节点
Figure 599785DEST_PATH_IMAGE015
这一节点而言，节点
Figure 336797DEST_PATH_IMAGE017
为节点
Figure 922499DEST_PATH_IMAGE015
的二阶邻边，节点与其一阶邻边及二阶邻边之间的相对位姿误差可以理解为第一优化误差；针对轨迹A以及轨迹B中的每个节点，该节点可以理解为该节点的自环节点，或者说该节点为自身的自环边，自环节点之间的相对位姿误差可以理解为第二优化误差；轨迹A中的节点
Figure 352343DEST_PATH_IMAGE040
与轨迹B中的节点
Figure 642903DEST_PATH_IMAGE042
为具有关联关系的节点对，所以节点
Figure 183606DEST_PATH_IMAGE042
为节点
Figure 889394DEST_PATH_IMAGE040
的关联边，节点与其关联边之间的相对位姿误差可以理解为第三优化误差。
举例来说，最小优化误差函数可以为：
Figure 224560DEST_PATH_IMAGE071
其中，E表示最小优化误差函数，ξ表示第一轨迹以及第二轨迹中各节点的位姿的李代数，i、j和k表示第一轨迹以及第二轨迹中节点的序号，ε表示具有关联关系的节点对、一阶邻边节点对和二阶邻边节点对的集合，eij表示第i个节点和第j个节点之间的相对位姿误差，
Figure 766531DEST_PATH_IMAGE045
表示预设的针对eij的第一信息矩阵，ek表示自环节点之间的相对位姿误差，Ω表示自环节点的集合，
Figure 845346DEST_PATH_IMAGE046
表示预设的针对ek的第二信息矩阵，(.)T表示矩阵转置，(.)-1表示矩阵求逆。
其中，第i节点和第j节点之间的相对位姿误差eij通过如下公式进行计算：
Figure 405640DEST_PATH_IMAGE047
其中，ΔTij表示根据第i个节点和第j个节点的优化前的位姿计算出的第i个节点和第j个节点之间的相对位姿，Ti表示第i个节点的待优化的位姿，Tj表示第j个节点的待优化的位姿；ξij表示ΔTij的李代数，ξi表示Ti的李代数，ξj表示Tj的李代数。
为了推导相对位姿误差对两端待优化的位姿的雅各比矩阵，对相对位姿误差eij施加左乘扰动，有
Figure 911708DEST_PATH_IMAGE072
利用伴随性质，
Figure 127925DEST_PATH_IMAGE073
，可将上式改写为
Figure 322015DEST_PATH_IMAGE050
其中，
Figure 674499DEST_PATH_IMAGE074
，
Figure 679364DEST_PATH_IMAGE075
，
Figure 117299DEST_PATH_IMAGE076
，
Figure 350965DEST_PATH_IMAGE077
。
自环节点的相对位姿误差ek通过如下公式进行计算：
Figure 823535DEST_PATH_IMAGE078
，其中
Figure 733722DEST_PATH_IMAGE079
；
Tk表示第一轨迹以及第二轨迹中的第k个节点的待优化的位姿，
Figure 658953DEST_PATH_IMAGE057
表示第一轨迹以及第二轨迹中的第k个节点的初始位姿，ξk表示Tk的李代数，(.)-1表示矩阵求逆。
上述相对位姿误差ek对待优化位姿的求导表示为：
Figure 883261DEST_PATH_IMAGE080
,
Figure 513132DEST_PATH_IMAGE081
，
Figure 797483DEST_PATH_IMAGE082
。
进而可以求解上述构建的最小优化误差函数，得到第一轨迹以及第二轨迹中各节点的优化后位姿。例如，可以采用非线性优化方法进行求解，例如采用非线性最小二乘法、一阶梯度和二阶梯度法等。
或者，另一种实施方式中，S511可以包括：利用优化数据库对第一轨迹中各节点的位姿进行位姿优化，得到对齐轨迹。
举例来说，优化数据库可以为：Ceres（谷神星）库、g2o（General GraphOptimization，通用图优化）图优化库，等等，具体优化数据库不做限定。
应用本发明实施例，针对第一轨迹中的每个待关联节点，确定与该待关联节点具有关联关系的目标节点以及三维点云地图对；再根据针对三维点云地图对中三维点的选择指令，确定预设数量个三维点对，根据预设数量个三维点对对三维点云地图对进行配准，得到待关联节点的三维点云地图和目标节点的三维点云地图之间的约束关系；基于约束关系，对第一轨迹中各节点的位姿以及第二轨迹中各节点的位姿进行位姿优化，得到对齐轨迹。可见，本方案中，根据选择指令确定三维点对，根据确定出的三维点对三维点云地图对进行配准，建立约束关系，之后根据约束关系对轨迹进行优化得到对齐轨迹，也就是说本方案中，对三维点云地图对中的匹配三维点对进行筛选，提高了不同轨迹的节点之间匹配的准确率，从而优化了轨迹对齐效果。
本发明实施例还提供了一种电子设备，如图6所示，包括处理器601、通信接口602、存储器603和通信总线604，其中，处理器601，通信接口602，存储器603通过通信总线604完成相互间的通信，
存储器603，用于存放计算机程序；
处理器601，用于执行存储器603上所存放的程序时，实现上述任意一种轨迹对齐方法。
上述电子设备提到的通信总线可以是外设部件互连标准（Peripheral ComponentInterconnect，PCI）总线或扩展工业标准结构（Extended Industry StandardArchitecture，EISA）总线等。该通信总线可以分为地址总线、数据总线、控制总线等。为便于表示，图中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。
通信接口用于上述电子设备与其他设备之间的通信。
存储器可以包括随机存取存储器（Random Access Memory，RAM），也可以包括非易失性存储器（Non-Volatile Memory，NVM），例如至少一个磁盘存储器。可选的，存储器还可以是至少一个位于远离前述处理器的存储装置。
上述的处理器可以是通用处理器，包括中央处理器（Central Processing Unit，CPU）、网络处理器（Network Processor，NP）等；还可以是数字信号处理器（Digital SignalProcessor，DSP）、专用集成电路（Application Specific Integrated Circuit，ASIC）、现场可编程门阵列（Field-Programmable Gate Array，FPGA）或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
在本发明提供的又一实施例中，还提供了一种计算机可读存储介质，该计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现上述任一轨迹对齐方法的步骤。
在本发明提供的又一实施例中，还提供了一种包含指令的计算机程序产品，当其在计算机上运行时，使得计算机执行上述实施例中任一轨迹对齐方法。
在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。所述计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行所述计算机程序指令时，全部或部分地产生按照本发明实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一个计算机可读存储介质传输，例如，所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线（例如同轴电缆、光纤、数字用户线（DSL））或无线（例如红外、无线、微波等）方式向另一个网站站点、计算机、服务器或数据中心进行传输。所述计算机可读存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，DVD)、或者半导体介质（例如固态硬盘Solid State Disk (SSD)）等。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
本说明书中的各个实施例均采用相关的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于电子设备、计算机可读存储介质及计算机程序产品实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
以上所述仅为本发明的较佳实施例，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本发明的保护范围内。

Claims (9)

1.一种轨迹对齐方法，其特征在于，包括：
获取第一轨迹和第二轨迹，所述第一轨迹和所述第二轨迹中均包括多个节点及每个节点的位姿和三维点云；
在所述第一轨迹的各个节点中，选择多个具有一定距离间隔的点确定为待关联节点；针对每个待关联节点，基于该待关联节点与所述第二轨迹中各节点的距离，在所述第二轨迹的各节点中，确定与该待关联节点具有关联关系的目标节点，其中，所述节点中包括时间戳，所述在所述第一轨迹的各个节点中，选择多个具有一定距离间隔的点确定为待关联节点，包括：根据所述时间戳的顺序，在所述第一轨迹的各个节点中，将时间最早的节点确定为当前待确定节点；判断是否存在与所述当前待确定节点具有关联关系的目标节点；若存在，则将所述当前待确定节点确定为待关联节点；在所述第一轨迹的各个节点中，确定与所确定的待关联节点之间的距离在第二预设阈值之外的节点，依次作为下一待确定节点；判断是否存在与所述下一待确定节点具有关联关系的目标节点；若存在，则将所述下一待确定节点确定为待关联节点；
将该待关联节点的三维点云与该待关联节点的邻近节点的三维点云进行融合，得到该待关联节点的三维点云地图；将所述目标节点的三维点云与所述目标节点的邻近节点的三维点云进行融合，得到所述目标节点的三维点云地图；将该待关联节点的三维点云地图与所述目标节点的三维点云地图确定为具有关联关系的三维点云地图对，其中，所述将该待关联节点的三维点云与该待关联节点的邻近节点的三维点云进行融合，得到该待关联节点的三维点云地图，包括：计算得到所述待关联节点与所述待关联节点的邻近节点之间的刚体变换矩阵，对所述待关联节点的邻近节点的三维点云进行该邻近节点对应的刚体变换，将刚体变换后的该邻近节点的三维点云与所述待关联节点的三维点云进行融合，得到所述待关联节点的三维点云地图；所述将所述目标节点的三维点云与所述目标节点的邻近节点的三维点云进行融合，得到所述目标节点的三维点云地图，包括：计算得到所述目标节点与所述目标节点的邻近节点之间的刚体变换矩阵，对所述目标节点的邻近节点的三维点云进行该邻近节点对应的刚体变换，将刚体变换后的该邻近节点的三维点云与所述目标节点的三维点云进行融合，得到所述目标节点的三维点云地图；
在可视化界面中展示所述三维点云地图对，接收用户对所展示的三维点云地图中的三维点的选择指令；根据所述选择指令，确定所述用户在三维点云地图中选择的预设数量个三维点对；其中，所述三维点对中包括该待关联节点的三维点云地图中的一个三维点与所述目标节点的三维点云地图中的一个三维点；
基于所述预设数量个三维点对，对该待关联节点的三维点云地图和所述目标节点的三维点云地图进行配准，得到该待关联节点的三维点云地图和所述目标节点的三维点云地图之间的约束关系；
基于所述约束关系，对所述第一轨迹中的各节点的位姿以及所述第二轨迹中各节点的位姿进行位姿优化，得到对齐轨迹。
2.根据权利要求1所述的方法，其特征在于，所述获取第一轨迹和第二轨迹，包括：
获取两条待处理轨迹，所述待处理轨迹中包括多个节点及每个节点的位姿，所述节点中包括以下任意一种或多种：经度、纬度、高度、姿态角；
分别计算各节点的经度的平均值、纬度的平均值以及高度的平均值，作为几何重心的经度、纬度以及高度，其中，几何重心为所有节点的中心点；
确定所述几何重心对应的子午圈主曲率半径和卯酉圈主曲率半径；
针对每个节点，基于所述几何重心的经度、纬度、高度、子午圈主曲率半径、卯酉圈主曲率半径、该节点的经度、纬度、高度以及该节点的姿态角，对该节点的位姿进行标准化，将任意一条标准化后的待处理轨迹确定为第一轨迹，将另一条标准化后的待处理轨迹确定为第二轨迹。
3.根据权利要求1所述的方法，其特征在于，所述基于所述预设数量个三维点对，对该待关联节点的三维点云地图和所述目标节点的三维点云地图进行配准，得到该待关联节点的三维点云地图和所述目标节点的三维点云地图之间的约束关系，包括：
基于所述预设数量个三维点对，对该待关联节点的三维点云地图和所述目标节点的三维点云地图进行配准，得到配准结果及其对应的刚体变换矩阵；
在可视化界面中展示所述配准结果，接收用户对所展示的配准结果的判断指令，所述判断指令为通过指令或继续指令；
若所述判断指令为通过指令，则将所述配准结果对应的刚体变换矩阵确定为该待关联节点的三维点云地图和所述目标节点的三维点云地图之间的约束关系；
若所述判断指令为继续指令，则返回所述基于接收到的针对所述三维点云地图对中的三维点的选择指令在所述三维点云地图对中确定预设数量个三维点对的步骤，直至接收到通过指令。
4.根据权利要求3所述的方法，其特征在于，所述方法还包括：
若所述判断指令为继续指令，则判断所述判断指令为继续指令的次数是否大于预设次数阈值；
若否，则执行所述返回所述基于接收到的针对所述三维点云地图对中的三维点的选择指令在所述三维点云地图对中确定预设数量个三维点对的步骤；
若是，则舍弃所述配准结果，并返回执行所述基于接收到的针对所述三维点云地图对中的三维点的选择指令在所述三维点云地图对中确定预设数量个三维点对的步骤。
5.根据权利要求3所述的方法，其特征在于，所述基于所述预设数量个三维点对，对该待关联节点的三维点云地图和所述目标节点的三维点云地图进行配准，得到配准结果及其对应的刚体变换矩阵，包括：
分别计算所述预设数量个三维点对中的预设数量个第一三维点的几何重心和预设数量个第二三维点的几何重心，所述第一三维点为：该待关联节点的三维点云地图中的三维点，所述第二三维点为：所述目标节点的三维点云地图中的三维点；
基于所述第一三维点的几何重心和所述第二三维点的几何重心，计算所述预设数量个三维点对的协方差矩阵；
根据所述协方差矩阵，得到刚体变换矩阵；
根据所述刚体变换矩阵，对该待关联节点的三维点云地图进行刚体变换，将刚体变换后的该待关联节点的三维点云地图与所述目标节点的三维点云地图，确定为配准结果。
6.根据权利要求5所述的方法，其特征在于，所述根据所述协方差矩阵，得到刚体变换矩阵，包括：
对所述协方差矩阵进行奇异值分解，得到粗匹配位姿变换矩阵；
基于迭代最近邻算法对所述粗匹配位姿变换矩阵进行迭代调整，得到刚体变换矩阵。
7.根据权利要求1所述的方法，其特征在于，所述基于每个待关联节点与所述第二轨迹中各节点的距离，在所述第二轨迹的各节点中，确定与该待关联节点具有关联关系的目标节点，包括：
针对所述第一轨迹中的每个待关联节点，将所述第二轨迹的各个节点中的与该待关联节点距离最近的节点确定为候选节点；
判断所述候选节点与该待关联节点之间的距离是否大于第一预设阈值；
若否，则将所述候选节点确定为与该待关联节点具有关联关系的目标节点。
8.根据权利要求1所述的方法，其特征在于，所述基于所述约束关系，对所述第一轨迹中的各节点的位姿以及所述第二轨迹中各节点的位姿进行位姿优化，得到对齐轨迹，包括：
以所述第一轨迹中各节点的位姿以及所述第二轨迹中各节点的位姿作为优化变量，构建最小优化误差函数，所述最小优化误差函数包括第一优化误差、第二优化误差和第三优化误差，所述第一优化误差包含所述第一轨迹中各个相邻节点间的相对位姿误差以及所述第二轨迹中各个相邻节点间的相对位姿误差，所述第二优化误差包含所述第一轨迹中预先确定的自环节点间的相对位姿误差以及所述第二轨迹中预先确定的自环节点间的相对位姿误差，所述第三优化误差包含各具有关联关系的所述待关联节点和所述目标节点之间的相对位姿误差；
求解所述最小优化误差函数，确定所述第一轨迹中各节点的优化后位姿以及所述第二轨迹中各节点的优化后位姿，得到对齐轨迹。
9.一种电子设备，其特征在于，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；
存储器，用于存放计算机程序；
处理器，用于执行存储器上所存放的程序时，实现权利要求1-8任一所述的方法步骤。
CN202110278640.9A 2021-03-16 2021-03-16 一种轨迹对齐方法及电子设备 Active CN112669250B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110278640.9A CN112669250B (zh) 2021-03-16 2021-03-16 一种轨迹对齐方法及电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110278640.9A CN112669250B (zh) 2021-03-16 2021-03-16 一种轨迹对齐方法及电子设备

Publications (2)

Publication Number Publication Date
CN112669250A CN112669250A (zh) 2021-04-16
CN112669250B true CN112669250B (zh) 2021-09-17

Family

ID=75399489

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110278640.9A Active CN112669250B (zh) 2021-03-16 2021-03-16 一种轨迹对齐方法及电子设备

Country Status (1)

Country Link
CN (1) CN112669250B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113256804B (zh) * 2021-06-28 2021-10-22 湖北亿咖通科技有限公司 一种三维重建尺度恢复方法、装置、电子设备及存储介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111429528A (zh) * 2020-04-07 2020-07-17 高深智图(广州)科技有限公司 大型分布式高精地图数据处理系统
CN111524168A (zh) * 2020-04-24 2020-08-11 中国科学院深圳先进技术研究院 点云数据的配准方法、系统、装置及计算机存储介质
CN111649752A (zh) * 2020-05-29 2020-09-11 北京四维图新科技股份有限公司 拥堵路段的地图数据处理方法、装置以及设备
CN112051590A (zh) * 2020-08-31 2020-12-08 广州文远知行科技有限公司 激光雷达与惯性测量单元的检测方法及相关装置
CN112084289A (zh) * 2020-09-18 2020-12-15 湖北亿咖通科技有限公司 一种轨迹融合方法及装置

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20150092048A1 (en) * 2013-09-27 2015-04-02 Qualcomm Incorporated Off-Target Tracking Using Feature Aiding in the Context of Inertial Navigation
US10573018B2 (en) * 2016-07-13 2020-02-25 Intel Corporation Three dimensional scene reconstruction based on contextual analysis
CN108230247B (zh) * 2017-12-29 2019-03-15 达闼科技（北京）有限公司 基于云端的三维地图的生成方法、装置、设备及计算机可读的存储介质
CN110335316B (zh) * 2019-06-28 2023-04-18 Oppo广东移动通信有限公司 基于深度信息的位姿确定方法、装置、介质与电子设备

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111429528A (zh) * 2020-04-07 2020-07-17 高深智图(广州)科技有限公司 大型分布式高精地图数据处理系统
CN111524168A (zh) * 2020-04-24 2020-08-11 中国科学院深圳先进技术研究院 点云数据的配准方法、系统、装置及计算机存储介质
CN111649752A (zh) * 2020-05-29 2020-09-11 北京四维图新科技股份有限公司 拥堵路段的地图数据处理方法、装置以及设备
CN112051590A (zh) * 2020-08-31 2020-12-08 广州文远知行科技有限公司 激光雷达与惯性测量单元的检测方法及相关装置
CN112084289A (zh) * 2020-09-18 2020-12-15 湖北亿咖通科技有限公司 一种轨迹融合方法及装置

Also Published As

Publication number Publication date
CN112669250A (zh) 2021-04-16

Similar Documents

Publication Publication Date Title
CN107659893B (zh) 一种误差补偿方法、装置、电子设备及可读存储介质
EP2556338B1 (en) Probe data processing
Cotera et al. Indoor robot positioning using an enhanced trilateration algorithm
CN106912105B (zh) 基于pso_bp神经网络的三维定位方法
US20150341886A1 (en) Correcting Interference of Non-Line-Of-Sight Signals
WO2017162036A1 (zh) 偏航识别方法、终端和存储介质
Cooper et al. Statistical concepts and their application in photogrammetry and surveying
CN106574975A (zh) 使用外围信号的轨迹匹配
CN109283562B (zh) 一种车联网中车辆三维定位方法及装置
CN103827632B (zh) 位置检测装置以及位置检测方法
Leung et al. A general framework for error analysis in measurement-based GIS Part 1: The basic measurement-error model and related concepts
US10488206B2 (en) Method and system for improving spatial accuracy of map data
Velaga et al. Improving the performance of a topological map-matching algorithm through error detection and correction
CN112669250B (zh) 一种轨迹对齐方法及电子设备
Fakhrmoosavi et al. An iterative learning approach for network contraction: Path finding problem in stochastic time‐varying networks
CN113159457A (zh) 一种智能路径规划的方法、系统及电子设备
CN113758492A (zh) 地图检测方法和装置
CN108132458B (zh) 室内测距方法、装置、设备及存储介质
CN108834053B (zh) 一种定位方法、装置及设备
US20190197726A1 (en) Geolocation System
WO2019162877A1 (en) System for providing location corrections
CN109752690A (zh) 无人机定位nlos的消除算法、系统、装置及存储介质
Welte et al. HD map errors detection using smoothing and multiple drives
CN113701768A (zh) 一种路径确定方法、装置及电子设备
Li et al. Geometric analysis of reality-based indoor 3D mapping

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
GR01 Patent grant
GR01 Patent grant