CN113432593B

CN113432593B - 集中式同步定位与地图构建方法、装置和系统

Info

Publication number: CN113432593B
Application number: CN202110713270.7A
Authority: CN
Inventors: 林雅云; 李骊
Original assignee: Beijing HJIMI Technology Co Ltd
Current assignee: Beijing HJIMI Technology Co Ltd
Priority date: 2021-06-25
Filing date: 2021-06-25
Publication date: 2023-05-23
Anticipated expiration: 2041-06-25
Also published as: CN113432593A

Abstract

本申请提供了一种集中式同步定位与地图构建方法、装置和系统，在本申请的方案中，集中式SLAM系统的客户端在生成关键帧，并基于该关键帧所观测到的地图点确定出待优化的局部地图之后，会综合局部地图中来源于该客户端和来源于其他客户端的所有关键帧各自对应的地图点的视觉重投影误差，以及该局部地图中来源于该客户端的关键帧对应的惯性信息的预积分误差，优化该局部地图中各关键帧的位姿以及各关键帧对应的地图点的坐标，从而实现了能够结合视觉信息和惯性信息两方面的误差来综合优化局部地图，有利于基于客户端生成的关键帧更为精准的更新局部地图，使得客户端能够更为精准的构建出地图。

Description

集中式同步定位与地图构建方法、装置和系统

技术领域

本申请涉及定位与地图构建技术领域，尤其涉及一种集中式同步定位与地图构建方法、装置和系统。

背景技术

同步定位与地图构建(Simultaneous Localization and Mapping，SLAM)最早源于机器人领域，其目标是在一个未知的环境中实时重建环境的三维结构并同时对机器人自身进行定位。随着定位技术的不断发展，人们提出了集中式协同SLAM。

集中式SLAM包括多个可以独立完成定位和建图功能的客户端以及一个计算能力强大的服务器。其中，客户端向服务端发送本地地图信息，服务端+则对多个客户端地图进行合并，以构建信息更为丰富的全局地图。然而，在目前的集中式SLAM领域中，客户端构建出的地图的准确性普遍不高，因此，如何提高客户端构建出的地图的准确性是本领域技术人员需要解决的技术问题。

发明内容

有鉴于此，本申请提供了一种集中式同步定位与地图构建方法、装置和系统，以更为准确的实现客户端的地图构建。

为实现上述目的，本申请提供了如下技术方案：

一方面，本申请提供了一种集中式同步定位与地图构建方法，应用于集中式同步定位与地图构建SLAM系统中的客户端，所述方法包括：+

获得所述客户端生成的且尚未优化处理的目标关键帧；

基于目标关键帧所能观测到的地图点，确定出待优化的局部地图，所述局部地图包括所述目标关键帧，且所述局部地图中的关键帧包括来源于所述客户端的至少一个第一关键帧和来源于所述SLAM系统中所述客户端之外的其他客户端的至少一个第二关键帧；

分别确定所述局部地图中每个关键帧对应的各地图点的视觉重投影误差，所述局部地图中关键帧对应的地图点为所述关键帧所能观测到的且属于所述局部地图的地图点；

分别确定所述局部地图中每个第一关键帧对应的惯性信息预积分误差，所述第一关键帧对应的惯性信息预积分误差为基于所述第一关键帧的位姿以及所述第一关键帧之前最近一个第一关键帧的位姿，确定出的所述第一关键帧与所述最近一个第一关键帧之间位姿的预积分误差；

基于所述局部地图中所有关键帧各自对应的各地图点的视觉重投影误差以及所有第一关键帧对应的惯性信息预积分误差，优化所述局部地图中各关键帧的位姿以及各关键帧对应的各地图点的坐标，得到优化后的局部地图。

在一种可能的实现方式中，所述基于目标关键帧所能观测到的地图点，确定出待优化的局部地图，包括：

从所述客户端已获得的关键帧中，确定与所述目标关键帧具有强共视关系的至少一个共视关键帧，具有强共视关系的共视关键帧与所述目标关键帧能够同时观测到的地图点的个数超过设定阈值，所述客户端已获得的关键帧包括来源于所述客户端的至少一个第一关键帧和来源于所述SLAM系统中所述客户端之外的其他客户端的至少一个第二关键帧；

确定由所述目标关键帧、所述至少一个共视关键帧以及所述目标关键帧和所述至少一个共视关键帧所能观测到的地图点组成的局部地图。

在又一种可能的实现方式中，所述基于所述局部地图中所有关键帧各自对应的各地图点的视觉重投影误差以及所有第一关键帧对应的惯性信息预积分误差，优化所述局部地图中各关键帧的位姿以及各关键帧对应的各地图点的坐标，包括：

以综合视觉投影误差与综合预积分误差之和最小为优化目标，优化所述局部地图中各关键帧的位姿以及各关键帧对应的各地图点的坐标；

其中，所述综合视觉重投影误差为所述局部地图中所有目标地图点的视觉重投影误差的平方和，所述目标地图点属于所述局部地图中关键帧对应的地图点，所述综合预积分误差为所述局部地图中所有第一关键帧对应的惯性信息预积分误差的平方和。

在又一种可能的实现方式中，所述以综合视觉投影误差与综合预积分误差之和最小为优化目标，优化所述局部地图中各关键帧的位姿以及各关键帧对应的各地图点的坐标，包括：

以综合视觉投影误差与综合预积分误差之和最小为优化目标，采用非线性算法迭代求解出所述局部地图中各关键帧的最优位姿以及各关键帧对应各地图点的最优坐标。

又一方面，本申请还提供了一种集中式同步定位与地图构建系统，包括：

服务器以及多个客户端；

所述客户端，用于执行上述任一所述的集中式同步定位与地图构建方法；

所述客户端，还用于将优化后的所述目标关键帧的位姿以及所述目标关键帧所能观测到的地图点的坐标发送给服务器；

所述服务器，用于基于获得的所述目标关键帧的位姿以及所述目标关键帧所能观测到的地图点，更新所述服务器中存储的客户端对应的子地图；在至少两个客户端对应的子地图存在重叠的情况下，合并所述至少两个客户端的子地图，并将所述至少两个客户端中每个客户端的至少部分子地图共享给所述至少两个客户端。

在一种可能的实现方式中，所述客户端，还用于在确认存在由于定位丢失导致复位操作时，向服务器发送复位通知；

所述服务器还用于：响应于所述复位通知，检测所述客户端对应的子地图中关键帧的个数小于设定数值，则清空服务器中存储的所述客户端对应的子地图；如果所述客户端的子地图中关键帧个数不小于所述设定数值，在服务器中为所述客户端新建一子地图，并将新建的子地图作为所述客户端对应的待更新的子地图，以通过新建的子地图保存所述客户端新增的关键帧和地图点的信息。

又一方面，本申请还提供了一种集中式同步定位与地图构建装置，应用于集中式同步定位与地图构建SLAM系统中的客户端，所述装置包括：

关键帧获得单元，用于获得所述客户端生成的且尚未优化处理的目标关键帧；

地图确定单元，用于基于目标关键帧所能观测到的地图点，确定出待优化的局部地图，所述局部地图包括所述目标关键帧，且所述局部地图中的关键帧包括来源于所述客户端的至少一个第一关键帧和来源于所述SLAM系统中所述客户端之外的其他客户端的至少一个第二关键帧；

第一误差确定单元，用于分别确定所述局部地图中每个关键帧对应的各地图点的视觉重投影误差，所述局部地图中关键帧对应的地图点为所述关键帧所能观测到的且属于所述局部地图的地图点；

第二误差确定单元，用于分别确定所述局部地图中每个第一关键帧对应的惯性信息预积分误差，所述第一关键帧对应的惯性信息预积分误差为基于所述第一关键帧的位姿以及所述第一关键帧之前最近一个第一关键帧的位姿，确定出的所述第一关键帧与所述最近一个第一关键帧之间位姿的预积分误差；

地图优化单元，用于基于所述局部地图中所有关键帧各自对应的各地图点的视觉重投影误差以及所有第一关键帧对应的惯性信息预积分误差，优化所述局部地图中各关键帧的位姿以及各关键帧对应的各地图点的坐标，得到优化后的局部地图。

在又一种可能的实现方式中，所述地图确定单元，包括：

共视帧确定子单元，用于从所述客户端已获得的关键帧中，确定与所述目标关键帧具有强共视关系的至少一个共视关键帧，具有强共视关系的共视关键帧与所述目标关键帧能够同时观测到的地图点的个数超过设定阈值，所述客户端已获得的关键帧包括来源于所述客户端的至少一个第一关键帧和来源于所述SLAM系统中所述客户端之外的其他客户端的至少一个第二关键帧；

地图确定子单元，用于确定由所述目标关键帧、所述至少一个共视关键帧以及所述目标关键帧和所述至少一个共视关键帧所能观测到的地图点组成的局部地图。

在又一种可能的实现方式中，所述地图优化单元，包括：

地图优化子单元，用于以综合视觉投影误差与综合预积分误差之和最小为优化目标，优化所述局部地图中各关键帧的位姿以及各关键帧对应的各地图点的坐标；

在又一种可能的实现方式中，所述地图优化子单元，具体为，用于以综合视觉投影误差与综合预积分误差之和最小为优化目标，采用非线性算法迭代求解出所述局部地图中各关键帧的最优位姿以及各关键帧对应各地图点的最优坐标。

由以上内容可知，本申请中，集中式SLAM系统的客户端在生成关键帧，并基于该关键帧所观测到的地图点确定出待优化的局部地图之后，会综合局部地图中来源于该客户端和来源于其他客户端的所有关键帧各自对应的地图点的视觉重投影误差，以及该局部地图中来源于该客户端的关键帧对应的惯性信息的预积分误差，优化该局部地图中各关键帧的位姿以及各关键帧对应的地图点的坐标，从而实现了能够结合视觉信息和惯性信息两方面的误差来综合优化局部地图，有利于基于客户端生成的关键帧更为精准的更新局部地图，使得客户端能够更为精准的构建出地图。

附图说明

为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1示出了本申请提供的集中式同步定位与地图构建方法的一种示意图；

图2示出了本申请提供的集中式同步定位与地图构建方法的又一种流程示意图；

图3示出了本申请提供的集中式同步定位与地图构建系统的一种组成架构示意图；

图4示出了本申请提供的集中式同步定位与地图构建装置的一种组成结构示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

同步定位与地图构建(Simultaneous Localization and Mapping，SLAM)系统最早源于机器人领域，其目标是在一个未知的环境中实时重建环境的三维结构并同时对机器人自身进行定位。其主要思想是：一方面，依靠已创建的地图信息进行自定位；另一方面，根据定位结果对地图进行更新。视觉SLAM技术根据拍摄的视频信息推断出相机在未知环境中的位姿，并同时构建环境地图，其基本原理为多视图几何原理。

本申请的方案适用于集中式SLAM系统，集中式SLAM系统包含多个可以独立完成定位和建图功能的客户端和一个计算能力强大的服务端。其中，客户端向服务端发送本地地图信息，服务端则对多个客户端地图进行合并，以构建信息更为丰富的全局地图。

与以往的集中式SLAM系统不同，本申请是基于视觉和惯性里程计(Visual-Inertial Odometry，VIO)的集中式协同SLAM系统。该系统在客户端使用VIO的方式进行定位，在视觉信息不好的情况下仍能够使用惯性信息提供较好的定位，提高了客户端定位的鲁棒性；在服务端进行多个客户端子地图的合并，并在合并地图以后将一个客户端地图信息传递给另一个客户端，丰富了客户端地图信息；客户端则利用来自其它客户端的关键帧的视觉信息，以及本地地图的视觉惯性信息来联合优化本地地图，从而进一步提高了定位和建图的精度。

下面结合流程图对本申请的集中式同步定位与地图构建方法进行说明。

如图1所示，其示出了本申请提供的集中式同步定位与地图构建方法的一种流程示意图，本实施例的方法可以应用于集中式同步定位与地图构建SLAM系统中的客户端。本实施例的方法可以包括：

S101，获得客户端生成的且尚未优化处理的目标关键帧。

其中，目标关键帧为客户端从采集到的图像帧中最新确定出的一个关键帧，该目标关键帧为尚未用于更新客户端本地的本地地图的关键帧，因此，该目标关键帧也称为尚未被优化处理的目标关键帧。

为了能够基于定位结果构建或者优化地图，客户端确定出的关键帧关联有位姿和地图点的信息，相应的，该目标关键帧也会对应有位姿的信息以及该目标关键帧所能观测到的至少一个地图点(也称为三维路标点)的信息。其中，关键帧的位姿表征客户端采集该关键帧时的位姿或者说是客户端的相机位姿，通过关键帧的位姿可以表现出相机的运行轨迹。

可以理解的是，关键帧会一直被保存，并参与客户端的地图构建与优化。

S102，基于目标关键帧所能观测到的地图点，确定出待优化的局部地图。

其中，局部地图包括目标关键帧，且局部地图中的关键帧包括来源于客户端的至少一个第一关键帧和来源于SLAM系统中客户端之外的其他客户端的至少一个第二关键帧。

可以理解的是，客户端在定位的同时，会使用定位过程中产生的定位信息构建客户端的本地地图。具体的，客户端系统在定位的同时，也会将每个关键帧的信息存放到本地地图中，并构建关键帧与地图点之间的关联(比如某关键帧能够观测到某个地图点)，以及关键帧与关键帧之间的关联(比如两个关键帧能够观测同一个地图点，则这两关键帧之间产生了关联，被称为共视关键帧)，这样就建立了客户端的本地地图。客户端的本地地图包含了真实世界中的三维路标点也就是地图点，还包括客户端设备的轨迹即关键帧的位姿。

同时，由于集中式SLAM系统中，当服务器将服务器端不同客户端的子地图合并之后，会将子地图合并所涉及到的一个客户端的地图信息发送给该子地图合并所涉及到的其他客户端，因此，每个客户端构建的地图中除了包含来源自该客户端自身产生的关键帧和地图点之外，还包括来源于其他客户端的关键帧和地图点。

本申请为了便于区分，将客户端的地图中来源自客户端自身的关键帧称为第一关键帧，而将客户端的地图来源于其他客户端的关键帧称为第二关键帧。

在本申请中，为了提高所建地图的精度，使地图点的坐标和客户端轨迹与真实世界更加接近，在建图的过程中，在客户端构建地图的过程中，客户端需要在获得关键帧之后，逐步地对地图进行优化。在本申请中是指基于惯性信息和视觉信息对局部地图中的关键帧的位姿以及地图点的坐标进行优化。基于此，本申请需要确定出需要优化的局部地图。

可以理解的是，基于目标关键帧所能观测到的地图点，确定出局部地图的方式可以有多种，该局部地图至少包括目标关键帧及目标关键帧所能观测到的各地图点。

在一种可选方式中，可以从客户端已获得的关键帧(获得该关键帧之前已获得并保存的关键帧)中，确定与该目标关键帧具有强共视关系的至少一个共视关键帧。其中，两个关键帧之间具有共视关系是指两个关键帧能够同时观测到同一个地图点。而具有强共视关系的共视关键帧与目标关键帧能够同时观测到的地图点(即相同的地图点)的个数超过设定阈值，如，该设定阈值可以根据需要设定，如该设定阈值可以10。为了便于区分，将与该目标关键帧具有强共视关系的关键帧称为目标关键帧的共视关键帧。

相应的，可以确定出由目标关键帧、该至少一个共视关键帧，以及该目标关键帧和该至少一个共视关键帧所能观测到的各地图点组成的局部地图。

由前面介绍可知，此处客户端已获得的关键帧可以包括来源于该客户端的至少一个第一关键帧和来源于SLAM系统中该客户端之外的其他客户端的至少一个第二关键帧，从而使得该局部地图包括来源于该客户端以及其他客户端的关键帧。

S103，分别确定局部地图中每个关键帧对应的各地图点的视觉重投影误差。

其中，此处局部地图中关键帧对应的地图点是指该关键帧所能观测到的且属于局部地图的地图点。

针对局部地图中每个关键帧而言，确定该关键帧对应的属于该局部地图的地图点的视觉重投影误差是指是从视觉信息方面，确定出关键帧对应地图点的测量坐标与基于重投影确定出的预测坐标之间的误差。

如，在确定视觉重投影误差时，可以结合利用相机投影模型，需要确定关键帧中的特征点对应的且属于局部地图中的地图点的坐标，并基于地图点的坐标以及相机投影模型，确定地图点在该关键帧上的投影点，然后计算关键帧中该特征点对应的地图点的投影点坐标与该特征点的坐标之间的差值。

本申请对于确定视觉重投影误差的具体过程不加限制。

S104，分别确定局部地图中每个第一关键帧对应的惯性信息预积分误差。

该第一关键帧对应的惯性信息预积分误差为基于第一关键帧的位姿以及第一关键帧之前最近一个第一关键帧的位姿，确定出的第一关键帧与该最近一个第一关键帧之间位姿的预积分误差。

可以理解的是，由于关键帧的位姿包括位置和方向两部分，因此，此处两个关键帧之间位姿的预积分误差可以包括：位置预积分误差和方向预积分误差(也称为旋转预积分)这两部分。

其中，两个关键帧之间的惯性信息预积分误差可以理解为通过对这两个关键帧的惯性信息，确定出两个关键帧之间的相对位姿约束。

本申请对于确定预积分误差的具体实现方式不加限制。

可以理解的是，对于局部地图中来自其他客户端的关键帧而言，服务器端向该客户端发送的其他客户端的关键帧中并不包含相应的惯性信息。这是因为：不同的客户端中惯性测量单元(Inertial measurement，IMU)参数也不一样，即使客户端将关键帧的惯性信息传给服务端也无法使用，反而增加了通信量；相应的，服务端向客户端发送的其他客户端的关键帧中也没有惯性信息。基于此，本申请中仅仅需要确定来源于该客户端的关键帧与该关键帧之前最近一个来源于该客户端的关键帧之间的惯性信息的预积分误差。

S105，基于局部地图中所有关键帧各自对应的各地图点的视觉重投影误差以及所有第一关键帧对应的惯性信息的预积分误差，优化局部地图中各关键帧的位姿以及各关键帧对应的各地图点的坐标，得到优化后的局部地图。

可以理解的是，局部地图中各关键帧的位姿和地图点的坐标越准确，局部地图中各关键帧对应的地图点的视觉重投影误差以及各第一关键帧对应的预积分误差也就会越小，因此，本申请可以基于视觉信息和惯性信息两方面的误差信息，来优化局部地图中各关键帧的位姿和地图点的坐标，以使得这两部分信息的误差最小，并将最终优化得到一组关键帧的位置和地图点的坐标来表示该局部地图。

可以理解的是，在优化局部地图的过程中会不断调整前面确定视觉重投影误差过程中所涉及到的地图点的坐标和相机投影模型的参数，以及确定惯性信息预积分过程中所涉及到的关键帧的位姿，从而不断优化局部地图中关键帧的位姿以及地图点的坐标。

可以理解的是，在本申请实施例中，基于所述局部地图中所有关键帧各自对应的各地图点的视觉重投影误差以及所有第一关键帧对应的惯性信息的预积分误差，优化该局部地图的具体实现方式可以有多种实现可能。

在一种可能的实现方式中，可以计算视觉信息方面的各视觉重投影误差和各关键帧对应的惯性信息的预积分误差的平方和，得到一综合误差，并以该综合误差最小为优化目标，优化该局部地图。

具体的，以综合视觉投影误差与综合预积分误差之和最小为优化目标，优化所述局部地图中各关键帧的位姿以及各关键帧对应的各地图点的坐标。其中，综合视觉重投影误差为局部地图中所有目标地图点的视觉重投影误差的平方和，目标地图点属于局部地图中关键帧对应的地图点。综合预积分误差为局部地图中所有第一关键帧对应的惯性信息的预积分误差的平方和。

为了便于理解，下面以一种具体实现为例对本申请提供的集中式同步定位与地图构建方法进行介绍。

如图2所示，其示出了本申请提供的集中式同步定位与地图构建方法的又一种流程示意图，本实施例的方法可以应用于集中式同步定位与地图构建SLAM系统中的客户端。本实施例的方法可以包括：

S201，获得客户端生成的且尚未优化处理的目标关键帧。

S202，从客户端已获得的关键帧中，确定与目标关键帧具有强共视关系的至少一个共视关键帧。

其中，具有强共视关系的共视关键帧与目标关键帧能够同时观测到的地图点的个数超过设定阈值，客户端已获得的关键帧包括来源于客户端的至少一个第一关键帧和来源于SLAM系统中客户端之外的其他客户端的至少一个第二关键帧。

S203，确定由目标关键帧、至少一个共视关键帧以及目标关键帧和至少一个共视关键帧所能观测到的地图点组成的局部地图。

如，可以结合客户端当前已有的本地地图，并结合本地地图中各关键帧及各关键帧所能观测到的地图点的信息，确定出由该目标关键帧，与该该目标关键帧具有强共视关系的至少一个共视关键帧以及这些关键帧所能观测到的所有地图点组成的局部地图。

S204，针对局部地图中每个关键帧，确定局部地图中每个关键帧对应的各地图点的视觉重投影误差。

可以理解的是，该步骤实际上是针对局部地图中来源于该客户端的第一关键帧和来源自其他客户端的第二关键帧均需要区分确定出相应地图点的视觉重投影误差。

如，对于任意一个第一关键帧c_i，针对该第一关键帧中的任意一个特征点p，该特征点p的像素坐标为p_uv。获取客户端已确定出的特征点p对应于真实世界的三维的地图点f_j。如果地图点f_j在局部地图中，基于相机投影模型将地图点f_j重新投影到该第一关键帧c_i中，得到地图点在该第一关键帧中的重投影的投影点p'，且投影点p'的像素坐标为p_uv'。相机投影模型是将三维世界上的坐标点映射到二维图像平面的过程中所采用的转换模型。

在此基础上，该第一关键帧c_i对应的地图点f_j的视觉重投影误差r_zci,fj可以表示为如下公式一：

r_zci,fj＝p_uv'-p_uv， (公式一)；

可以理解的是，在以上公式一中，是以第一关键帧对应的地图点f_j的坐标为重投影过程中，该地图点f_j的初始坐标，后续在优化局部地图的过程中，会不断优化调整该地图点f_j的坐标的具体取值。当然，在后续优化局部地图的过程中还可能会涉及到对相机投影模型中相关的参数的调整。

以上是以确定第一关键帧c_i对应的地图点的视觉重投影误差为例说明。对于任意一个第二关键帧c_k，确定该第二关键帧c_k对应的地图点f_s的视觉重投影误差

的过程也类似，在此不再赘述。

S205，针对局部地图中的每个第一关键帧，基于第一关键帧的位姿以及第一关键帧之前最近一个第一关键帧的位姿，确定出的第一关键帧与该最近一个第一关键帧之间位姿的预积分误差，得到第一关键帧对应的惯性信息预积分误差。

如，对于任意一个第一关键帧c_i，设其位置为t_i，方向为q_i。获取该第一关键帧c_i之前最近一个来源于该客户端的第一关键帧c_i-1，该第一关键帧c_i-1对应的位置为t_i-1，方向为q_i-1。

在此基础上，基于第一关键帧c_i以及其前一第一关键帧c_i-1的惯性信息，确定c_i-1到c_i之间位姿的预积分信息，该预积分信息包含位置预积分α_i-1,i和旋转预积分β_i-1,i。其中，预积分信息为通过两个关键帧的惯性信息，即加速度计和角速度的信息进行积分，得到的两个关键帧之间的相对位姿信息，确定预积分的计算方法有多种可供选择，在此不再赘述。

利用这两个关键帧的位姿信息和该预积分信息构建两个关键帧之间的相对位姿约束，也即预积分误差。

相应的，预积分误差也包含两部分，一部分为位置预积分误差：r_ti-1,i＝(t_i-t_i-1)-α_i-1,i，另一部分为旋转预积分误差：

(因为这里旋转使用的是四元数表达，/>

表示四元数乘法)。将两部分预积分误差合并得到/>

可以理解的是，对于第一关键帧c_i，以上计算预积分误差时，可以利用客户端生成该关键帧时确定出的关键帧的位姿，该位姿可以为关键帧的初始位姿，后续在优化局部地图的过程中，会不断调整优化该初始位姿，以得到第一关键帧的最优位姿。

当然，以上是以一种确定关键帧的预积分误差的方式为例说明，在实际应用中，如果存在其他确定预积分误差的方式也同样适用于本申请。

S206，以综合视觉投影误差与综合预积分误差之和最小为优化目标，采用非线性算法迭代求解出该局部地图中各关键帧的最优位姿以及各关键帧对应各地图点的最优坐标，以得到优化出的局部地图。

其中，该综合视觉重投影误差为该局部地图中所有目标地图点的视觉重投影误差的平方和，该目标地图点属于所述局部地图中关键帧对应的地图点。

该综合预积分误差为局部地图中所有第一关键帧对应的惯性信息的预积分误差的平方和。

如，综合视觉投影误差和综合预积分误差之和可以称为总体误差，该总体误差可以表示为如下公式二：

其中，m为局部地图中第一关键帧的总个数；n表示第一关键帧中对应的属于该局部地图的地图点的总个数。l表示第二关键帧的总个数，u表示第二关键帧中对应的属于该局部地图的地图点的总个数。

相应的，本申请的目标是求解出一组该局部地图中的关键帧的位姿和地图点的坐标，使得该总体误差最小。即求解出x^*：

其中，x指代局部地图中所有关键帧的位姿以及地图点的坐标所形成的状态向量，x^*表示x的最优估计。

基于此，在本实施例中可以采用非线性优化算法迭代求解，在迭代求解过程中会不断调整前面S203和S204中计算重投影误差所采用的地图点的坐标、相机投影模型以及计算预积分误差中所涉及到的关键帧的位姿，并重新计算相关的重投影误差和预积分误差，直至确定出该总体误差收敛且最小，得到表示该局部地图中一组关键帧的最优位姿以及地图点的最优坐标。

可以理解的是，在本申请的集中式SLAM系统中可以包括多个客户端和一个服务器。如图3所示，其示出了本申请一种集中式SLAM系统的组成架构示意图。

由图3可以看出，该集中式SLAM系统包括服务器和多个客户端。

其中，客户端可以用于执行如上任意一个实施例中所述的集中式同步定位与地图构建方法。

在集中式SLAM系统中，客户端还用于将优化后的所述目标关键帧的位姿以及该目标关键帧所能观测到的地图点的坐标发送给服务器。

相应的，服务器，用于基于获得的所述目标关键帧的位姿以及所述目标关键帧所能观测到的地图点，更新所述服务器中存储的客户端对应的子地图；在至少两个客户端对应的子地图存在重叠的情况下，合并该至少两个客户端的子地图，并将该至少两个客户端中每个客户端的至少部分子地图共享给所述至少两个客户端。

如图3所示，客户端和服务端都有通信模块，用于客户端与服务端之间的信息传递。为了确保用户的信息安全，本方案采用的网络通信模式为：客户端主动发送消息，服务端被动接收消息；客户端主动向服务端申请消息，服务端才发送消息给客户端，服务端不主动发送消息。

在本申请中，客户端可以在系统初始化成功以后才开始给服务端发送消息。其中，本申请中客户端使用视觉和惯性信息进行联合定位，而定位的好坏与系统参数初始值有着直接的关系。客户端系统初始化主要用来对视觉和IMU的一些参数进行初始化。初始化所涉及到的相关参数主要有视觉尺度，重力向量，惯性加速度计偏差，陀螺仪偏差，以及地图中所有关键帧的速度等。如，客户端系统初始化可以使用开源项目ORB-SLAM3中的方法，当然，对于其他初始化方式也同样适用于客户端，对此不作赘述。

在初始化成功以前，系统只使用视觉信息进行定位；在初始化成功以后，就可以使用视觉和IMU信息进行联合定位。

其中，在客户端只有视觉信息的情况下，可以使用PnP(Perspective-n-Point，n点透视图)方法估计相机位姿。在客户端初始化之后，客户端能够获得IMU信息，则可以先使用IMU信息估计出当前图像帧的初始位姿，再将该初始位姿运用到PnP方法中，这样计算出来的图像帧的位姿具有更高的精度，同时也会减少由误差导致的定位丢失情况的出现。

在客户端由于某种情况导致定位丢失之后，在丢失时长小于设定时长内(如5秒内)时，对于每一个图像帧可以如下方法恢复定位：

(1)首先使用IMU信息估计出当前图像帧的初始位姿；

(2)根据估计出的初始位姿，将局部地图中的地图点投影到当前图像帧上，并使用BA优化方法(Bundle Adjustment，捆集调整)对这一位姿进行优化，同时得到优化后的地图内点，如果内点数大于一定阈值，则位姿恢复成功，否则失败，回到如上步骤(1)，直到位姿恢复成功或丢失超过5秒。

如果客户端定位丢失的时长超过设定时长，则IMU信息的累积误差可能已经过大而无法继续使用，此时需要对系统进行复位操作，即以接下来的第一帧图像帧作为初始帧，同时客户端重新进行初始化操作，并清空所有地图信息和IMU信息。

在客户端进行地图构建的过程中，则可以本申请前面实施例中提到的方式进行地图优化，具体详见前面实施例的相关介绍，在此不再赘述。

相应的，在本申请中客户端向服务器发送的消息主要有如下几种情况：

第一种情况为：在定位正常时，客户端发送的消息主要是本地地图中新增的关键帧及其观测到的地图点。相应的，服务端接收到消息以后，会将关键帧和地图点保存在相应的子地图中，并由地图合并模块调用；如果两个子地图发生合并，则服务端会将一个客户端的地图信息返回给另一个客户端，用以扩充客户端的本地地图。如前面所述，客户端发送的关键帧信息并不包含IMU相关信息，在此不再赘述。

第二种情况为：在客户端定位丢失，且定位丢失的时长不超过设定时长，则发送一个空消息。相应的，服务器端此时若还有其它客户端的地图信息没有发送，则返回这些其他客户端的地图信息给该客户端。

在一种可选，本申请还提供了第三种情况，第三种情况为：在客户端确认存在由于定位丢失导致复位操作时，向服务器发送复位通知。

相应的，服务器响应于该复位通知，也会执行复位操作，具体的，检测所述客户端对应的子地图中关键帧的个数小于设定数值(如，设定数值可以为10，具体可以根据需要设定)，则清空服务器中存储的该客户端对应的子地图；如果该客户端的子地图中关键帧个数不小于该设定数值，在服务器中为该客户端新建一子地图，并将新建的子地图作为该客户端对应的待更新的子地图，以通过新建的子地图保存客户端新增的关键帧和地图点的信息。

如果该客户端的子地图中关键帧个数不小于该设定数值，则说明服务器端已存储的该客户端的子地图仍可能会存在一些有用的地图信息，在此基础上本申请为客户端重新创建一子地图的同时，服务器端仍保留该客户端定位丢失之前的子地图，从而使得服务器端仍可以将客户端在定位丢失前所采用的该子地图与其他客户端的子地图进行合并，提高服务器端地图数据的丰富性。

由图3可以看出，服务器端具有地图合并模块，基于该地图合并模块可以对不同客户端对应的子地图进行合并，

如，服务器端地图合并模块首先检测各个服务器端子地图之间是否有重叠区域；在检测到重叠区域以后，使用匹配信息将子地图合并成一个地图。

具体过程如下：

每个客户端在服务器端都对应一个地图匹配线程，对于达到服务器端的关键帧，会被加入到服务器端关键帧数据库中和客户端匹配队列中。地图匹配线程会依次从匹配队列中取出关键帧作为当前关键帧，并检测当前关键帧与其它客户端关键帧之间的相似性。

检测相似性的具体流程如下:

首先，从关键帧数据库中找到与当前关键帧相似程度超过一定阈值的关键帧作为候选关键帧；

其次，对候选关键帧进行连续性检测，剔除不满足连续性的候选关键帧；

再次，计算当前关键帧和候选关键帧的Sim3变换，并得到更多的特征匹配，剔除匹配数低于阈值的候选关键帧；

最后，通过投影方式获得更多的特征匹配，如果匹配数量大于阈值，则匹配成功并进行两个地图的合并。

其中，地图合并的具体流程可以包括：

首先，通过Sim3传播的方式调整当前关键帧所在子地图的所有关键帧和地图点位姿；

其次，创建一个新的地图，并将两个子地图的关键帧和地图点都加入到新地图中，由此，两个子地图合并成为一个地图。

在服务端成功将两个子地图合并以后，会将一个客户端的地图信息返回给另一个客户端，丰富了客户端本地地图信息，帮助其更好地进行定位和建图。

对应本申请的一种集中式同步定位与地图构建方法，本申请还提供了一种集中式同步定位与地图构建装置。

如图4所示，其示出了本申请一种集中式同步定位与地图构建装置的一种组成结构示意图，该装置应用于集中式同步定位与地图构建SLAM系统中的客户端，该装置包括：

关键帧获得单元401，用于获得该客户端生成的且尚未优化处理的目标关键帧；

地图确定单元402，用于基于目标关键帧所能观测到的地图点，确定出待优化的局部地图，该局部地图包括该目标关键帧，且该局部地图中的关键帧包括来源于该客户端的至少一个第一关键帧和来源于该SLAM系统中该客户端之外的其他客户端的至少一个第二关键帧；

第一误差确定单元403，用于分别确定该局部地图中每个关键帧对应的各地图点的视觉重投影误差，该局部地图中关键帧对应的地图点为该关键帧所能观测到的且属于该局部地图的地图点；

第二误差确定单元404，用于分别确定该局部地图中每个第一关键帧对应的惯性信息预积分误差，该第一关键帧对应的惯性信息预积分误差为基于该第一关键帧的位姿以及该第一关键帧之前最近一个第一关键帧的位姿，确定出的该第一关键帧与该最近一个第一关键帧之间位姿的预积分误差；

地图优化单元405，用于基于该局部地图中所有关键帧各自对应的各地图点的视觉重投影误差以及所有第一关键帧对应的惯性信息预积分误差，优化该局部地图中各关键帧的位姿以及各关键帧对应的各地图点的坐标，得到优化后的局部地图。

在一种可能的实现方式中，该地图确定单元，包括：

共视帧确定子单元，用于从该客户端已获得的关键帧中，确定与该目标关键帧具有强共视关系的至少一个共视关键帧，具有强共视关系的共视关键帧与该目标关键帧能够同时观测到的地图点的个数超过设定阈值，该客户端已获得的关键帧包括来源于该客户端的至少一个第一关键帧和来源于该SLAM系统中该客户端之外的其他客户端的至少一个第二关键帧；

地图确定子单元，用于确定由该目标关键帧、该至少一个共视关键帧以及该目标关键帧和该至少一个共视关键帧所能观测到的地图点组成的局部地图。

在又一种可能的实现方式中，该地图优化单元，包括：

地图优化子单元，用于以综合视觉投影误差与综合预积分误差之和最小为优化目标，优化该局部地图中各关键帧的位姿以及各关键帧对应的各地图点的坐标；

其中，该综合视觉重投影误差为该局部地图中所有目标地图点的视觉重投影误差的平方和，该目标地图点属于该局部地图中关键帧对应的地图点，该综合预积分误差为该局部地图中所有第一关键帧对应的惯性信息预积分误差的平方和。

在又一种可能的实现方式中，该地图优化子单元，具体为，用于以综合视觉投影误差与综合预积分误差之和最小为优化目标，采用非线性算法迭代求解出该局部地图中各关键帧的最优位姿以及各关键帧对应各地图点的最优坐标。

需要说明的是，本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。同时，本说明书中各实施例中记载的特征可以相互替换或者组合，使本领域专业技术人员能够实现或使用本申请。对于装置类实施例而言，由于其与方法实施例基本相似，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

最后，还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括要素的过程、方法、物品或者设备中还存在另外的相同要素。

对所公开的实施例的上述说明，使本领域技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims

1.一种集中式同步定位与地图构建方法，其特征在于，应用于集中式同步定位与地图构建SLAM系统中的客户端，所述方法包括：

获得所述客户端生成的且尚未优化处理的目标关键帧；

基于所述局部地图中所有关键帧各自对应的各地图点的视觉重投影误差以及所有第一关键帧对应的惯性信息预积分误差，优化所述局部地图中各关键帧的位姿以及各关键帧对应的各地图点的坐标，得到优化后的局部地图；

所述基于所述局部地图中所有关键帧各自对应的各地图点的视觉重投影误差以及所有第一关键帧对应的惯性信息预积分误差，优化所述局部地图中各关键帧的位姿以及各关键帧对应的各地图点的坐标，包括：

以综合视觉重投影误差与综合预积分误差之和最小为优化目标，优化所述局部地图中各关键帧的位姿以及各关键帧对应的各地图点的坐标；

2.根据权利要求1所述的方法，其特征在于，所述基于目标关键帧所能观测到的地图点，确定出待优化的局部地图，包括：

3.根据权利要求1所述的方法，其特征在于，所述以综合视觉重投影误差与综合预积分误差之和最小为优化目标，优化所述局部地图中各关键帧的位姿以及各关键帧对应的各地图点的坐标，包括：

以综合视觉重投影误差与综合预积分误差之和最小为优化目标，采用非线性算法迭代求解出所述局部地图中各关键帧的最优位姿以及各关键帧对应各地图点的最优坐标。

4.一种集中式同步定位与地图构建系统，其特征在于，包括：

服务器以及多个客户端；

所述客户端，用于执行如上权利要求1至3任一项所述的集中式同步定位与地图构建方法；

5.根据权利要求4所述的系统，其特征在于，所述客户端，还用于在确认存在由于定位丢失导致复位操作时，向服务器发送复位通知；

6.一种集中式同步定位与地图构建装置，其特征在于，应用于集中式同步定位与地图构建SLAM系统中的客户端，所述装置包括：

地图优化单元，用于基于所述局部地图中所有关键帧各自对应的各地图点的视觉重投影误差以及所有第一关键帧对应的惯性信息预积分误差，优化所述局部地图中各关键帧的位姿以及各关键帧对应的各地图点的坐标，得到优化后的局部地图；

所述地图优化单元，包括：地图优化子单元，用于以综合视觉重投影误差与综合预积分误差之和最小为优化目标，优化所述局部地图中各关键帧的位姿以及各关键帧对应的各地图点的坐标；其中，所述综合视觉重投影误差为所述局部地图中所有目标地图点的视觉重投影误差的平方和，所述目标地图点属于所述局部地图中关键帧对应的地图点，所述综合预积分误差为所述局部地图中所有第一关键帧对应的惯性信息预积分误差的平方和。

7.根据权利要求6所述的装置，其特征在于，所述地图确定单元，包括：

8.根据权利要求6所述的装置，其特征在于，所述地图优化子单元，具体为，用于以综合视觉重投影误差与综合预积分误差之和最小为优化目标，采用非线性算法迭代求解出所述局部地图中各关键帧的最优位姿以及各关键帧对应各地图点的最优坐标。