CN114322994B - 一种基于离线全局优化的多点云地图融合方法和装置 - Google Patents

一种基于离线全局优化的多点云地图融合方法和装置 Download PDF

Info

Publication number
CN114322994B
CN114322994B CN202210228899.7A CN202210228899A CN114322994B CN 114322994 B CN114322994 B CN 114322994B CN 202210228899 A CN202210228899 A CN 202210228899A CN 114322994 B CN114322994 B CN 114322994B
Authority
CN
China
Prior art keywords
point cloud
factor
information
cloud map
gps
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
CN202210228899.7A
Other languages
English (en)
Other versions
CN114322994A (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.)
Zhejiang Lab
Original Assignee
Zhejiang Lab
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 Zhejiang Lab filed Critical Zhejiang Lab
Priority to CN202210228899.7A priority Critical patent/CN114322994B/zh
Publication of CN114322994A publication Critical patent/CN114322994A/zh
Application granted granted Critical
Publication of CN114322994B publication Critical patent/CN114322994B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Processing Or Creating Images (AREA)

Abstract

本发明公开了一种基于离线全局优化的多点云地图融合方法和装置，该方法包括：步骤1，适配基于GPS信息初始化的激光SLAM方法；步骤2，将需要用于点云融合的多个数据集，按照上述S1所述的在线激光SLAM方法依次获得多个点云地图；步骤3，离线加载各点云地图信息，并依次构建各点云地图的里程计因子，回环因子和GPS因子；步骤4，构建不同点云地图之间的互约束因子；步骤5，设定优化参数，进行全局优化，保存最终融合点云和所有关键帧信息。本发明方法相较于在线多点云地图融合的方法，提高了多点云地图融合的稳定性和可靠性，降低了算法实施的难度，无需顾及建图性能和运行实时性之间的平衡关系。

Description

一种基于离线全局优化的多点云地图融合方法和装置
技术领域
本发明涉及基于激光雷达传感器的定位与建图领域，尤其是涉及一种基于离线全局优化的多点云地图融合方法和装置。
背景技术
自1985年同时定位与建图（Simultaneous Localization and Mapping，SLAM）问题首次被提出以来，SLAM技术经历了30多年的发展，其理论体系已基本得到完善；目前已进入面向具体应用的鲁棒感知时代，尤其在移动机器人、无人驾驶等领域发挥着重要作用，大量性能卓越的SLAM方法不断涌现。
在无人车执行具体任务过程中，用于匹配定位的先验点云地图是整个算法模块不可或缺的组成部分，其中点云地图的获得可以通过现有成熟的激光SLAM方法。然而，对于大范围环境而言，很难通过一次数据采集（single-session）就能得到完整的先验点云地图；同时也存在地图需要部分调整以及场景发生局部变化等情况。因此，融合多点云地图对于完整点云地图的获得至关重要。然而目前关于多点云地图融合的相关研究较少，且主要集中于在线方法，而在线方法对于算法实施要求高，不易开展，且需要平衡建图性能和运行实时性。
发明内容
为了解决现有技术中存在的上述技术问题，本发明提出了一种基于离线全局优化的多点云地图融合方法和装置，其具体技术方案如下：
一种基于离线全局优化的多点云地图融合方法，包括以下步骤：
步骤1，以开源激光SLAM方法—LIO-SAM流程为基础，进行GPS信息初始化适配，同时在使用开源激光SLAM方法的运行过程中和结束时构建自约束，保留关键帧位姿信息、点云信息以及约束信息，其中约束信息包括：里程计因子，GPS因子和回环因子；
步骤2，将用于点云融合的多个数据集，按照步骤1所述的激光SLAM方法，依次获得多个点云地图；
步骤3，离线加载各个点云地图的位姿信息，构建各点云地图内各位姿之间的里程计因子、GPS因子和回环因子；
步骤4，构建不同点云地图之间的互约束因子；
步骤5，根据加载的自约束和构建的互约束因子，构建点云地图融合代价函数，进行全局优化，得到融合点云和所有关键帧信息。
进一步的，所述步骤1具体包括以下子步骤：
步骤1.1 ，利用GPS信息作为第一帧关键帧的初始位姿，并添加GPS因子以约束第一帧关键帧；将第一帧关键帧记为
Figure DEST_PATH_IMAGE002
，利用GPS信息
Figure DEST_PATH_IMAGE004
进行初始化，添加初始时刻GPS因子
Figure DEST_PATH_IMAGE006
，其中
Figure DEST_PATH_IMAGE008
表示关键帧位姿信息与GPS信息之间的映射函数，
Figure DEST_PATH_IMAGE010
表示方差矩阵；
步骤1.2，在激光SLAM运行过程中，根据距离阈值和角度阈值选取关键帧
Figure DEST_PATH_IMAGE012
，
Figure DEST_PATH_IMAGE014
，添加并记录相邻关键帧之间的里程计因子
Figure DEST_PATH_IMAGE016
，其中
Figure DEST_PATH_IMAGE018
和
Figure DEST_PATH_IMAGE020
分别表示相邻关键帧之间的位姿变换函数和里程计观测信息，
Figure DEST_PATH_IMAGE022
表示方差矩阵；在线运行回环检测，添加回环因子
Figure DEST_PATH_IMAGE024
，其中
Figure DEST_PATH_IMAGE026
和
Figure DEST_PATH_IMAGE028
分别表示回环关键帧之间的位姿变换函数和回环观测信息，
Figure DEST_PATH_IMAGE030
表示方差矩阵；同时根据距离阈值，添加并记录GPS因子
Figure DEST_PATH_IMAGE032
；
步骤1.3，在激光SLAM运行结束时，构建生成单个点云地图的代价函数，记录所有关键帧位姿信息和对应的点云信息，公式如下：
Figure DEST_PATH_IMAGE034
其中，
Figure DEST_PATH_IMAGE036
和
Figure DEST_PATH_IMAGE038
分别表示是否存在GPS因子与回环因子，通过优化代价函数得到关键帧序列
Figure DEST_PATH_IMAGE040
，以获得点云地图，
Figure DEST_PATH_IMAGE042
为单个点云地图的代价函数，其中
Figure DEST_PATH_IMAGE044
表示第
Figure DEST_PATH_IMAGE046
个关键帧序列。
进一步的，所述步骤2，具体为：利用多个需要用于点云地图融合的离线数据集记为
Figure DEST_PATH_IMAGE048
，采用步骤1的开源激光SLAM方法，得到对应的多个点云地图，记为
Figure DEST_PATH_IMAGE050
，所述对应的多个点云地图的信息包括关键帧位姿信息及对应的关键帧点云信息，同时包含里程计因子、GPS因子和回环因子。
进一步的，所述步骤3，具体为：离线加载多个点云地图
Figure 187725DEST_PATH_IMAGE050
，每个点云地图包括关键帧位姿信息、点云信息以及约束信息，根据约束信息依次构建各点云地图内各位姿之间的里程计因子、GPS因子和回环因子。
进一步的，所述步骤4，具体包括以下子步骤;
步骤4.1，遍历得到两个有重叠区域的点云地图
Figure DEST_PATH_IMAGE052
与
Figure DEST_PATH_IMAGE054
，
Figure DEST_PATH_IMAGE056
，对于点云地图
Figure 760658DEST_PATH_IMAGE052
，其包含的位姿集合记为
Figure DEST_PATH_IMAGE058
，
Figure DEST_PATH_IMAGE060
，基于位姿集合
Figure 868291DEST_PATH_IMAGE058
，构造位置点云
Figure DEST_PATH_IMAGE062
和相应的KD树
Figure DEST_PATH_IMAGE064
；
步骤4.2，根据点云地图
Figure 794659DEST_PATH_IMAGE054
，遍历得到位姿集合
Figure DEST_PATH_IMAGE066
，记当前帧为
Figure DEST_PATH_IMAGE068
，利用
Figure 671348DEST_PATH_IMAGE064
搜索最近邻关键帧，得到满足距离阈值条件的目标关键帧，记为
Figure DEST_PATH_IMAGE070
；
步骤4.3，基于目标关键帧
Figure 556127DEST_PATH_IMAGE070
，构造局部点云地图
Figure DEST_PATH_IMAGE072
，根据点云匹配结果，构建互约束因子
Figure DEST_PATH_IMAGE074
，其中
Figure DEST_PATH_IMAGE076
和
Figure DEST_PATH_IMAGE078
分别表示不同点云地图之间关键帧之间的位姿变换函数和互约束观测信息，
Figure DEST_PATH_IMAGE080
表示方差矩阵。
进一步的，所述步骤5，得到所有关键帧信息，具体的表达式如下：
Figure DEST_PATH_IMAGE082
其中，
Figure DEST_PATH_IMAGE084
为代价函数，包含各点云地图内部里程计因子，回环因子和GPS因子，第二项包含步骤4所述构建的互约束因子，之后进行全局优化，以得到所有关键帧信息
Figure DEST_PATH_IMAGE086
。
一种基于离线全局优化的多点云地图融合装置，包括一个或多个处理器，用于实现所述的基于离线全局优化的多点云地图融合方法。
一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现所述的基于离线全局优化的多点云地图融合方法。
本发明的优势和有益效果在于：
本发明提出的多点云地图融合方法，算法逻辑简单清晰，实施方案灵活多变，可以有效避免多点云地图融合过程中的重影等现象，同时，基于融合过程解耦的实施手段，相较于在线多点云地图融合的方法，提高了多点云地图融合的稳定性和可靠性，同时也降低了算法实施的难度，无需顾及建图性能和运行实时性之间的平衡关系。
附图说明
图1为本发明的基于离线全局优化的多点云地图融合方法的流程图；
图2为本发明中多点云地图融合互约束构建方法的流程图；
图3为本发明中展示离线全局优化前的点云拼接效果示意图；
图4为本发明中展示离线全局优化后的点云融合效果示意图；
图5为本发明的一种基于离线全局优化的多点云地图融合装置的结构示意图。
具体实施方式
为了使本发明的目的、技术方案和技术效果更加清楚明白，以下结合说明书附图和实施例，对本发明作进一步详细说明。
如图1所示，本发明的一种基于离线全局优化的多点云地图融合方法，包括以下步骤：
步骤1，适配基于GPS信息初始化的激光SLAM方法，并通过激光SLAM方法，构建自约束：以开源激光SLAM方法—LIO-SAM流程为基础，进行GPS信息初始化适配，同时在使用开源激光SLAM方法的运行过程中和结束时保留关键帧位姿信息、点云信息以及约束信息，其中约束信息包括：里程计因子，GPS因子和回环因子。
具体的，包括以下子步骤：
步骤1.1 ，利用GPS信息作为第一帧关键帧的初始位姿，并添加GPS因子以约束第一帧关键帧；将第一帧关键帧记为
Figure 72428DEST_PATH_IMAGE002
，利用GPS信息
Figure 864804DEST_PATH_IMAGE004
进行初始化，添加初始时刻GPS因子
Figure 2524DEST_PATH_IMAGE006
，其中
Figure 589363DEST_PATH_IMAGE008
表示关键帧位姿信息与GPS信息之间的映射函数，
Figure 546955DEST_PATH_IMAGE010
表示方差矩阵；
步骤1.2，在激光SLAM运行过程中，根据距离阈值和角度阈值选取关键帧
Figure 877442DEST_PATH_IMAGE012
，
Figure 869669DEST_PATH_IMAGE014
，添加并记录相邻关键帧之间的里程计因子
Figure 565092DEST_PATH_IMAGE016
，其中
Figure 400193DEST_PATH_IMAGE018
和
Figure 878579DEST_PATH_IMAGE020
分别表示相邻关键帧之间的位姿变换函数和里程计观测信息，
Figure 115525DEST_PATH_IMAGE022
表示方差矩阵；在线运行回环检测，添加回环因子
Figure 919533DEST_PATH_IMAGE024
，其中
Figure 241930DEST_PATH_IMAGE026
和
Figure 586324DEST_PATH_IMAGE028
分别表示回环关键帧之间的位姿变换函数和回环观测信息，
Figure 553143DEST_PATH_IMAGE030
表示方差矩阵；同时根据距离阈值，添加并记录GPS因子
Figure 918265DEST_PATH_IMAGE032
；
步骤1.3，在激光SLAM运行结束时，记录所有关键帧位姿信息和对应的点云信息，构建生成单个点云地图的代价函数，
Figure DEST_PATH_IMAGE034A
其中，
Figure 400062DEST_PATH_IMAGE036
和
Figure 876043DEST_PATH_IMAGE038
分别表示是否存在GPS因子与回环因子，通过优化代价函数得到关键帧序列
Figure 431789DEST_PATH_IMAGE040
，以获得点云地图，
Figure 967813DEST_PATH_IMAGE042
为单个点云地图的代价函数，其中
Figure 874589DEST_PATH_IMAGE044
表示第
Figure 560785DEST_PATH_IMAGE046
个关键帧序列。
步骤2，将需要用于点云融合的多个数据集，按照步骤1所述的在线激光SLAM方法依次获得多个点云地图：利用多个需要用于点云地图融合的离线数据集记为
Figure 626830DEST_PATH_IMAGE048
，采用步骤1的开源激光SLAM方法，得到对应的多个点云地图，记为
Figure 209121DEST_PATH_IMAGE050
，所述对应的多个点云地图的信息包括关键帧位姿信息及对应的关键帧点云信息，同时包含里程计因子、GPS因子和回环因子。
步骤3，离线加载各个点云地图信息，加载自约束：离线加载多个点云地图
Figure 462248DEST_PATH_IMAGE050
，每个点云地图包括关键帧位姿信息、点云信息以及约束信息，根据约束信息依次构建各点云地图内各位姿之间的里程计因子、GPS因子和回环因子。
步骤4，如图2所示，构建不同点云地图之间的互约束因子：遍历得到两个有重叠区域的点云地图
Figure 155397DEST_PATH_IMAGE052
与
Figure 810370DEST_PATH_IMAGE054
，
Figure 563562DEST_PATH_IMAGE056
，对于点云地图
Figure 241668DEST_PATH_IMAGE052
，其包含的位姿集合记为
Figure 863142DEST_PATH_IMAGE058
，
Figure 247987DEST_PATH_IMAGE060
，根据点云匹配构建互约束因子。
其中所述的构建互约束因子，各点云地图之间的约束因子，通过目标关键帧搜索以及点云匹配实现，具体包括以下子步骤：
步骤4.1, 基于位姿集合
Figure 31135DEST_PATH_IMAGE058
，构造位置点云
Figure 399800DEST_PATH_IMAGE062
和相应的KD树
Figure 559386DEST_PATH_IMAGE064
；
步骤4.2，根据点云地图
Figure 126633DEST_PATH_IMAGE054
，遍历得到位姿集合
Figure 956049DEST_PATH_IMAGE066
，记当前帧为
Figure 936643DEST_PATH_IMAGE068
，利用
Figure 509707DEST_PATH_IMAGE064
搜索最近邻关键帧，得到满足距离阈值条件的目标关键帧，记为
Figure 993778DEST_PATH_IMAGE070
；
步骤4.3，基于目标关键帧
Figure 259674DEST_PATH_IMAGE070
，构造局部点云地图
Figure 134090DEST_PATH_IMAGE072
，根据点云匹配结果，构建互约束因子
Figure 901057DEST_PATH_IMAGE074
，其中
Figure 380580DEST_PATH_IMAGE076
和
Figure 4328DEST_PATH_IMAGE078
分别表示不同点云地图之间关键帧之间的位姿变换函数和互约束观测信息，
Figure 303723DEST_PATH_IMAGE080
表示方差矩阵。
步骤5，根据加载的自约束和构建的互约束，构建点云地图融合代价函数，进行全局优化，得到融合点云和所有关键帧信息，具体表达式为：
Figure DEST_PATH_IMAGE082A
其中，
Figure 671119DEST_PATH_IMAGE084
如步骤1所述的构建单个点云的代价函数，包含各点云地图内部里程计因子，回环因子和GPS因子，第二项包含步骤4所述构建的互约束因子，之后进行全局优化，以得到所有关键帧信息
Figure 5148DEST_PATH_IMAGE086
。
本发明解耦了多地图融合过程中的内部约束因子和互约束因子构建过程，通过在线激光SLAM方法，构建单个点云地图的内部约束信息，其中包含里程计因子、回环因子以及GPS因子；进一步，通过目标关键帧搜索和点云匹配结果构建任意两个点云地图之间的互约束因子，然后利用全局优化得到融合后的点云地图和所有关键帧位姿信息。如图3和图4所示，展示了离线全局优化实施方案前后的点云融合效果，可以看到，在经过离线全局优化之后，消除了各地图之间的重影现象。该发明为无人车领域完整先验点云地图的构建提供了简单清晰和极易开展的解决方案。特别是对于大范围环境的构建和局部场景的调整，不但保证融合过程的灵活性，还可以保证多点云地图的有效融合。
与前述基于离线全局优化的多点云地图融合方法的实施例相对应，本发明还提供了基于离线全局优化的多点云地图融合装置的实施例。
参见图5，本发明实施例提供的一种基于离线全局优化的多点云地图融合装置，包括一个或多个处理器，用于实现上述实施例中的基于离线全局优化的多点云地图融合方法。
本发明基于离线全局优化的多点云地图融合装置的实施例可以应用在任意具备数据处理能力的设备上，该任意具备数据处理能力的设备可以为诸如计算机等设备或装置。装置实施例可以通过软件实现，也可以通过硬件或者软硬件结合的方式实现。以软件实现为例，作为一个逻辑意义上的装置，是通过其所在任意具备数据处理能力的设备的处理器将非易失性存储器中对应的计算机程序指令读取到内存中运行形成的。从硬件层面而言，如图5所示，为本发明基于离线全局优化的多点云地图融合装置所在任意具备数据处理能力的设备的一种硬件结构图，除了图5所示的处理器、内存、网络接口、以及非易失性存储器之外，实施例中装置所在的任意具备数据处理能力的设备通常根据该任意具备数据处理能力的设备的实际功能，还可以包括其他硬件，对此不再赘述。
上述装置中各个单元的功能和作用的实现过程具体详见上述方法中对应步骤的实现过程，在此不再赘述。
对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本发明方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
本发明实施例还提供一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现上述实施例中的基于离线全局优化的多点云地图融合方法。
所述计算机可读存储介质可以是前述任一实施例所述的任意具备数据处理能力的设备的内部存储单元，例如硬盘或内存。所述计算机可读存储介质也可以是风力发电机的外部存储设备，例如所述设备上配备的插接式硬盘、智能存储卡（Smart Media Card，SMC）、SD卡、闪存卡（Flash Card）等。进一步的，所述计算机可读存储介质还可以既包括任意具备数据处理能力的设备的内部存储单元也包括外部存储设备。所述计算机可读存储介质用于存储所述计算机程序以及所述任意具备数据处理能力的设备所需的其他程序和数据，还可以用于暂时地存储已经输出或者将要输出的数据。
以上所述，仅为本发明的优选实施案例，并非对本发明做任何形式上的限制。虽然前文对本发明的实施过程进行了详细说明，对于熟悉本领域的人员来说，其依然可以对前述各实例记载的技术方案进行修改，或者对其中部分技术特征进行同等替换。凡在本发明精神和原则之内所做修改、同等替换等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种基于离线全局优化的多点云地图融合方法，其特征在于，包括以下步骤：
步骤1，以开源激光SLAM方法—LIO-SAM流程为基础，进行GPS信息初始化适配，同时在使用开源激光SLAM方法的运行过程中和结束时构建自约束，保留关键帧位姿信息、点云信息以及约束信息，其中约束信息包括：里程计因子，GPS因子和回环因子；
步骤2，将用于点云融合的多个数据集，按照步骤1所述的激光SLAM方法，依次获得多个点云地图；
步骤3，离线加载各个点云地图的位姿信息，构建各点云地图内各位姿之间的里程计因子、GPS因子和回环因子；
步骤4，构建不同点云地图之间的互约束因子；
步骤5，根据加载的自约束和构建的互约束因子，构建点云地图融合代价函数，进行全局优化，得到融合点云和所有关键帧信息。
2.如权利要求1所述的一种基于离线全局优化的多点云地图融合方法，其特征在于，所述步骤1具体包括以下子步骤：
步骤1.1 ，利用GPS信息作为第一帧关键帧的初始位姿，并添加GPS因子以约束第一帧关键帧；将第一帧关键帧记为
Figure DEST_PATH_IMAGE001
，利用GPS信息
Figure 590069DEST_PATH_IMAGE002
进行初始化，添加初始时刻GPS因子
Figure DEST_PATH_IMAGE003
，其中
Figure 492166DEST_PATH_IMAGE004
表示关键帧位姿信息与GPS信息之间的映射函数，
Figure DEST_PATH_IMAGE005
表示方差矩阵；
步骤1.2，在激光SLAM运行过程中，根据距离阈值和角度阈值选取关键帧
Figure 300722DEST_PATH_IMAGE006
，
Figure DEST_PATH_IMAGE007
，添加并记录相邻关键帧之间的里程计因子
Figure 796425DEST_PATH_IMAGE008
，其中
Figure DEST_PATH_IMAGE009
和
Figure 715839DEST_PATH_IMAGE010
分别表示相邻关键帧之间的位姿变换函数和里程计观测信息，
Figure DEST_PATH_IMAGE011
表示方差矩阵；在线运行回环检测，添加回环因子
Figure 738022DEST_PATH_IMAGE012
，其中
Figure DEST_PATH_IMAGE013
和
Figure 717479DEST_PATH_IMAGE014
分别表示回环关键帧之间的位姿变换函数和回环观测信息，
Figure DEST_PATH_IMAGE015
表示方差矩阵；同时根据距离阈值，添加并记录GPS因子
Figure 825113DEST_PATH_IMAGE016
；
步骤1.3，在激光SLAM运行结束时，构建生成单个点云地图的代价函数，记录所有关键帧位姿信息和对应的点云信息，公式如下：
Figure 954743DEST_PATH_IMAGE018
其中，
Figure DEST_PATH_IMAGE019
和
Figure 831432DEST_PATH_IMAGE020
分别表示是否存在GPS因子与回环因子，通过优化代价函数得到关键帧序列
Figure DEST_PATH_IMAGE021
，以获得点云地图，
Figure 981790DEST_PATH_IMAGE022
为单个点云地图的代价函数，其中
Figure DEST_PATH_IMAGE023
表示第
Figure 311141DEST_PATH_IMAGE024
个关键帧序列。
3.如权利要求2所述的一种基于离线全局优化的多点云地图融合方法，其特征在于，所述步骤2，具体为：利用多个需要用于点云地图融合的离线数据集记为
Figure DEST_PATH_IMAGE025
，采用步骤1的开源激光SLAM方法，得到对应的多个点云地图，记为
Figure 837937DEST_PATH_IMAGE026
，所述对应的多个点云地图的信息包括关键帧位姿信息及对应的关键帧点云信息，同时包含里程计因子、GPS因子和回环因子。
4.如权利要求3所述的一种基于离线全局优化的多点云地图融合方法，其特征在于，所述步骤3，具体为：离线加载多个点云地图
Figure 710078DEST_PATH_IMAGE026
，每个点云地图包括关键帧位姿信息、点云信息以及约束信息，根据约束信息依次构建各点云地图内各位姿之间的里程计因子、GPS因子和回环因子。
5.如权利要求4所述的一种基于离线全局优化的多点云地图融合方法，其特征在于，所述步骤4，具体包括以下子步骤;
步骤4.1，遍历得到两个有重叠区域的点云地图
Figure DEST_PATH_IMAGE027
与
Figure 320355DEST_PATH_IMAGE028
，
Figure DEST_PATH_IMAGE029
，对于点云地图
Figure 402580DEST_PATH_IMAGE027
，其包含的位姿集合记为
Figure 608433DEST_PATH_IMAGE030
，
Figure DEST_PATH_IMAGE031
，基于位姿集合
Figure 459715DEST_PATH_IMAGE030
，构造位置点云
Figure 358401DEST_PATH_IMAGE032
和相应的KD树
Figure DEST_PATH_IMAGE033
；
步骤4.2，根据点云地图
Figure 662343DEST_PATH_IMAGE028
，遍历得到位姿集合
Figure 796521DEST_PATH_IMAGE034
，记当前帧为
Figure DEST_PATH_IMAGE035
，利用
Figure 502309DEST_PATH_IMAGE033
搜索最近邻关键帧，得到满足距离阈值条件的目标关键帧，记为
Figure 306317DEST_PATH_IMAGE036
；
步骤4.3，基于目标关键帧
Figure 769659DEST_PATH_IMAGE036
，构造局部点云地图
Figure DEST_PATH_IMAGE037
，根据点云匹配结果，构建互约束因子
Figure 176370DEST_PATH_IMAGE038
，其中
Figure DEST_PATH_IMAGE039
和
Figure 736664DEST_PATH_IMAGE040
分别表示不同点云地图之间关键帧之间的位姿变换函数和互约束观测信息，
Figure DEST_PATH_IMAGE041
表示方差矩阵。
6.如权利要求5所述的一种基于离线全局优化的多点云地图融合方法，其特征在于，所述步骤5，得到所有关键帧信息，具体的表达式如下：
Figure DEST_PATH_IMAGE043
其中，
Figure 39470DEST_PATH_IMAGE044
为代价函数，包含各点云地图内部里程计因子，回环因子和GPS因子，第二项包含步骤4所述构建的互约束因子，之后进行全局优化，以得到所有关键帧信息
Figure DEST_PATH_IMAGE045
。
7.一种基于离线全局优化的多点云地图融合装置，其特征在于，包括一个或多个处理器，用于实现权利要求1-6中任一项所述的基于离线全局优化的多点云地图融合方法。
8.一种计算机可读存储介质，其特征在于，其上存储有程序，该程序被处理器执行时，实现权利要求1-6中任一项所述的基于离线全局优化的多点云地图融合方法。
CN202210228899.7A 2022-03-10 2022-03-10 一种基于离线全局优化的多点云地图融合方法和装置 Active CN114322994B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210228899.7A CN114322994B (zh) 2022-03-10 2022-03-10 一种基于离线全局优化的多点云地图融合方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210228899.7A CN114322994B (zh) 2022-03-10 2022-03-10 一种基于离线全局优化的多点云地图融合方法和装置

Publications (2)

Publication Number Publication Date
CN114322994A CN114322994A (zh) 2022-04-12
CN114322994B true CN114322994B (zh) 2022-07-01

Family

ID=81034106

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210228899.7A Active CN114322994B (zh) 2022-03-10 2022-03-10 一种基于离线全局优化的多点云地图融合方法和装置

Country Status (1)

Country Link
CN (1) CN114322994B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116147484B (zh) * 2023-02-27 2024-04-30 东南大学 一种基于多点云融合的桥梁服役线形识别方法

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110490809A (zh) * 2019-08-28 2019-11-22 清华大学 多智能体协同定位与建图方法及装置
CN110706279A (zh) * 2019-09-27 2020-01-17 清华大学 基于全局地图与多传感器信息融合的全程位姿估计方法
CN112132745A (zh) * 2019-06-25 2020-12-25 南京航空航天大学 一种基于地理信息的多子地图拼接特征融合方法
CN112268559A (zh) * 2020-10-22 2021-01-26 中国人民解放军战略支援部队信息工程大学 复杂环境下融合slam技术的移动测量方法
CN112862894A (zh) * 2021-04-12 2021-05-28 中国科学技术大学 一种机器人三维点云地图构建与扩充方法
CN112965063A (zh) * 2021-02-11 2021-06-15 深圳市安泽智能机器人有限公司 一种机器人建图定位方法
CN113269094A (zh) * 2021-05-26 2021-08-17 中国科学院自动化研究所 基于特征提取算法和关键帧的激光slam***及方法
CN113658337A (zh) * 2021-08-24 2021-11-16 哈尔滨工业大学 一种基于车辙线的多模态里程计方法
CN113819914A (zh) * 2020-06-19 2021-12-21 北京图森未来科技有限公司 一种地图构建方法及装置
CN113870318A (zh) * 2021-12-02 2021-12-31 之江实验室 一种基于多帧点云的运动目标检测***和方法
CN114018248A (zh) * 2021-10-29 2022-02-08 同济大学 一种融合码盘和激光雷达的里程计方法与建图方法
CN114088081A (zh) * 2021-10-10 2022-02-25 北京工业大学 一种基于多段联合优化的用于精确定位的地图构建方法
CN114283250A (zh) * 2021-12-23 2022-04-05 武汉理工大学 高精度的三维点云地图自动拼接与优化方法及***

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2014080330A2 (en) * 2012-11-22 2014-05-30 Geosim Systems Ltd. Point-cloud fusion

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112132745A (zh) * 2019-06-25 2020-12-25 南京航空航天大学 一种基于地理信息的多子地图拼接特征融合方法
CN110490809A (zh) * 2019-08-28 2019-11-22 清华大学 多智能体协同定位与建图方法及装置
CN110706279A (zh) * 2019-09-27 2020-01-17 清华大学 基于全局地图与多传感器信息融合的全程位姿估计方法
CN113819914A (zh) * 2020-06-19 2021-12-21 北京图森未来科技有限公司 一种地图构建方法及装置
CN112268559A (zh) * 2020-10-22 2021-01-26 中国人民解放军战略支援部队信息工程大学 复杂环境下融合slam技术的移动测量方法
CN112965063A (zh) * 2021-02-11 2021-06-15 深圳市安泽智能机器人有限公司 一种机器人建图定位方法
CN112862894A (zh) * 2021-04-12 2021-05-28 中国科学技术大学 一种机器人三维点云地图构建与扩充方法
CN113269094A (zh) * 2021-05-26 2021-08-17 中国科学院自动化研究所 基于特征提取算法和关键帧的激光slam***及方法
CN113658337A (zh) * 2021-08-24 2021-11-16 哈尔滨工业大学 一种基于车辙线的多模态里程计方法
CN114088081A (zh) * 2021-10-10 2022-02-25 北京工业大学 一种基于多段联合优化的用于精确定位的地图构建方法
CN114018248A (zh) * 2021-10-29 2022-02-08 同济大学 一种融合码盘和激光雷达的里程计方法与建图方法
CN113870318A (zh) * 2021-12-02 2021-12-31 之江实验室 一种基于多帧点云的运动目标检测***和方法
CN114283250A (zh) * 2021-12-23 2022-04-05 武汉理工大学 高精度的三维点云地图自动拼接与优化方法及***

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping;Tixiao Shan et al.;《2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)》;20201231;第5135-5142页 *

Also Published As

Publication number Publication date
CN114322994A (zh) 2022-04-12

Similar Documents

Publication Publication Date Title
US11313684B2 (en) Collaborative navigation and mapping
Zhang et al. Reference pose generation for long-term visual localization via learned features and view synthesis
Cai et al. Practical optimal registration of terrestrial LiDAR scan pairs
Ball et al. OpenRatSLAM: an open source brain-based SLAM system
JP2020067439A (ja) 移動***置推定システムおよび移動***置推定方法
Cui et al. Efficient large-scale structure from motion by fusing auxiliary imaging information
CN111415417B (zh) 一种集成稀疏点云的移动机器人拓扑经验地图构建方法
Li et al. Review of vision-based Simultaneous Localization and Mapping
CN111540011A (zh) 基于混合度量-拓扑相机的定位
CN108520543B (zh) 一种对相对精度地图进行优化的方法、设备及存储介质
Churchill et al. Experience based navigation: Theory, practice and implementation
CN116592897B (zh) 基于位姿不确定性的改进orb-slam2定位方法
US20230117498A1 (en) Visual-inertial localisation in an existing map
CN114322994B (zh) 一种基于离线全局优化的多点云地图融合方法和装置
CN114001733A (zh) 一种基于地图的一致性高效视觉惯性定位算法
Beghdadi et al. A comprehensive overview of dynamic visual SLAM and deep learning: concepts, methods and challenges
Li et al. FC-SLAM: Federated learning enhanced distributed visual-LiDAR SLAM in cloud robotic system
Tseng et al. A new architecture for simultaneous localization and mapping: an application of a planetary rover
Alam et al. A review of recurrent neural network based camera localization for indoor environments
Liu et al. A light-weight lidar-inertial slam system with loop closing
Park et al. All-in-one mobile outdoor augmented reality framework for cultural heritage sites
Wei et al. Overview of visual slam for mobile robots
Hinzmann et al. Deep uav localization with reference view rendering
Chen et al. Robust relocalization based on active loop closure for real-time monocular SLAM
Gadipudi et al. A review on monocular tracking and mapping: from model-based to data-driven methods

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