CN114120301A

CN114120301A - 一种位姿确定方法、装置及设备

Info

Publication number: CN114120301A
Application number: CN202111350622.3A
Authority: CN
Inventors: 李佳宁; 李�杰; 毛慧; 浦世亮
Original assignee: Hangzhou Hikvision Digital Technology Co Ltd
Current assignee: Hangzhou Hikvision Digital Technology Co Ltd
Priority date: 2021-11-15
Filing date: 2021-11-15
Publication date: 2022-03-01

Abstract

本申请提供一种位姿确定方法、装置及设备，该方法包括：获取目标场景的目标图像和终端设备的运动数据；基于目标图像和运动数据确定终端设备的自定位轨迹；从三维视觉地图中确定出与所述目标图像对应的目标地图点，基于所述目标地图点确定终端设备在三维视觉地图中的全局定位轨迹；基于所述自定位轨迹和所述全局定位轨迹生成所述终端设备在三维视觉地图中的融合定位轨迹，并输出所述融合定位轨迹；其中，所述融合定位轨迹包括的融合定位位姿的帧率大于全局定位轨迹包括的全局定位位姿的帧率。通过本申请的技术方案，实现高帧率和高精度的定位功能，实现室内全局一致的高帧率定位功能。

Description

一种位姿确定方法、装置及设备

技术领域

本申请涉及计算机视觉领域，尤其涉及一种位姿确定方法、装置及设备。

背景技术

GPS(Global Positioning System，全球定位系统)是一种以人造地球卫星为基础的高精度无线电导航定位系统，GPS在全球任何地方以及近地空间均能够提供准确的地理位置、车行速度及精确的时间信息。北斗卫星导航系统由空间段、地面段和用户段三部分组成，可在全球范围内全天候、全天时为用户提供高精度、高可靠定位、导航、授时服务，具备区域导航、定位和授时能力。

由于终端设备具有GPS或北斗卫星导航系统，因此，在需要对终端设备进行定位时，可以采用GPS或北斗卫星导航系统对终端设备进行定位。在室外环境下，由于GPS信号或北斗信号比较好，可以采用GPS或北斗卫星导航系统对终端设备进行准确定位。但是，在室内环境下，由于GPS信号或者北斗信号比较差，导致GPS或北斗卫星导航系统无法对终端设备进行准确定位。比如说，在煤炭、电力、石化等能源行业，对于定位的需求越来越多，这些定位需求一般是在室内环境，由于信号遮挡等问题，导致无法对终端设备进行准确定位。

发明内容

本申请提供一种位姿确定方法，应用于终端设备，所述终端设备包括目标场景的三维视觉地图，所述终端设备在目标场景的移动过程中，所述方法包括：

获取所述目标场景的目标图像和所述终端设备的运动数据；

基于所述目标图像和所述运动数据确定所述终端设备的自定位轨迹；

从所述三维视觉地图中确定出与所述目标图像对应的目标地图点，基于所述目标地图点确定所述终端设备在所述三维视觉地图中的全局定位轨迹；

基于所述自定位轨迹和所述全局定位轨迹生成所述终端设备在所述三维视觉地图中的融合定位轨迹，输出所述融合定位轨迹；其中，所述融合定位轨迹包括的融合定位位姿的帧率大于所述全局定位轨迹包括的全局定位位姿的帧率。

本申请提供一种位姿确定装置，应用于终端设备，所述终端设备包括目标场景的三维视觉地图，在终端设备在目标场景的移动过程中，所述装置包括：

获取模块，用于获取所述目标场景的目标图像和所述终端设备的运动数据；

确定模块，用于基于所述目标图像和所述运动数据确定所述终端设备的自定位轨迹；从所述三维视觉地图中确定出与所述目标图像对应的目标地图点，基于所述目标地图点确定所述终端设备在所述三维视觉地图中的全局定位轨迹；

生成模块，用于基于所述自定位轨迹和所述全局定位轨迹生成所述终端设备在三维视觉地图中的融合定位轨迹，并输出融合定位轨迹；所述融合定位轨迹包括的融合定位位姿的帧率大于全局定位轨迹包括的全局定位位姿的帧率。

本申请提供一种终端设备，包括：处理器和机器可读存储介质，所述机器可读存储介质存储有能够被所述处理器执行的机器可执行指令；所述处理器用于执行机器可执行指令，以实现本申请上述示例公开的位姿确定方法。

本申请提供一种终端设备，包括：

视觉传感器，用于在所述终端设备在目标场景的移动过程中，获取所述目标场景的目标图像，并将所述目标图像输入给处理器；

运动传感器，用于在所述终端设备在目标场景的移动过程中，获取所述终端设备的运动数据，并将所述运动数据输入给处理器；

处理器，用于基于所述目标图像和所述运动数据确定所述终端设备的自定位轨迹；从所述目标场景的三维视觉地图中确定出与所述目标图像对应的目标地图点，基于所述目标地图点确定所述终端设备在所述三维视觉地图中的全局定位轨迹；基于所述自定位轨迹和所述全局定位轨迹生成所述终端设备在所述三维视觉地图中的融合定位轨迹，输出所述融合定位轨迹；所述融合定位轨迹包括的融合定位位姿的帧率大于所述全局定位轨迹包括的全局定位位姿的帧率。

由以上技术方案可见，本申请实施例中，终端设备在目标场景的移动过程中，可以基于目标场景的目标图像和终端设备的运动数据确定终端设备的自定位轨迹，并基于目标场景的目标图像确定终端设备在三维视觉地图中的全局定位轨迹，并基于自定位轨迹和全局定位轨迹生成终端设备在三维视觉地图中的融合定位轨迹，并输出融合定位轨迹。在上述方式中，可以依据目标图像和运动数据进行高帧率的自定位，得到高帧率的自定位轨迹，可以依据目标图像和三维视觉地图进行低帧率的全局定位，得到低帧率的全局定位轨迹，然后，将高帧率的自定位轨迹和低帧率的全局定位轨迹进行融合，消除自定位累积误差，得到高帧率的融合定位轨迹，即三维视觉地图中的高帧率的融合定位轨迹，实现高帧率和高精度的定位功能，实现室内全局一致的高帧率定位功能。在上述方式中，目标场景可以是室内环境，可以基于目标图像和运动数据，实现高精度、低成本、易部署的室内定位功能，是一种基于视觉的室内定位方式，能够应用在煤炭、电力、石化等能源行业，实现人员(如工人、巡检人员等)的室内定位，快速获知人员的位置信息，保障人员安全，实现人员的高效管理。

附图说明

图1是本申请一种实施方式中的位姿确定方法的流程示意图；

图2是本申请一种实施方式中的终端设备的结构示意图；

图3是本申请一种实施方式中的确定自定位轨迹的流程示意图；

图4是本申请一种实施方式中的确定全局定位轨迹的流程示意图；

图5是自定位轨迹、全局定位轨迹和融合定位轨迹的示意图；

图6是本申请一种实施方式中的确定融合定位轨迹的流程示意图；

图7是本申请一种实施方式中的位姿确定装置的结构示意图。

具体实施方式

在本申请实施例使用的术语仅仅是出于描述特定实施例的目的，而非限制本申请。本申请和权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其它含义。还应当理解，本文中使用的术语“和/或”是指包含一个或多个相关联的列出项目的任何或所有可能组合。

应当理解，尽管在本申请实施例可能采用术语第一、第二、第三等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，在不脱离本申请范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以被称为第一信息。取决于语境，此外，所使用的词语“如果”可以被解释成为“在……时”或“当……时”或“响应于确定”。

本申请实施例中提出一种位姿确定方法，可以应用于终端设备，终端设备可以包括目标场景的三维视觉地图，比如说，终端设备从服务器下载目标场景的三维视觉地图，并存储目标场景的三维视觉地图。终端设备在目标场景的移动过程中，该位姿确定方法用于确定终端设备的位姿，并输出终端设备的位姿。

参见图1所示，为该位姿确定方法的流程示意图，该方法可以包括：

步骤101、获取目标场景的目标图像和终端设备的运动数据。

步骤102、基于该目标图像和该运动数据确定终端设备的自定位轨迹。

示例性的，若目标图像包括多帧图像，则可以从多帧图像中遍历出当前帧图像；基于当前帧图像前面的K帧图像对应的自定位位姿、终端设备在自定位坐标系中的地图位置和该运动数据，确定当前帧图像对应的自定位位姿；基于多帧图像对应的自定位位姿生成终端设备在自定位坐标系中的自定位轨迹。

示例性的，若当前帧图像是关键图像，则可以基于终端设备的当前位置(即与当前帧图像对应位置)生成自定位坐标系中的地图位置。若当前帧图像是非关键图像，则不需要基于终端设备的当前位置生成自定位坐标系中的地图位置。

若当前帧图像与当前帧图像的前一帧图像之间的匹配特征点数量达到未预设阈值，则确定当前帧图像是关键图像。若当前帧图像与当前帧图像的前一帧图像之间的匹配特征点数量达到预设阈值，则确定当前帧图像是非关键图像。

步骤103、从目标场景的三维视觉地图中确定出与目标图像对应的目标地图点，并基于目标地图点确定终端设备在该三维视觉地图中的全局定位轨迹。

示例性的，若目标图像包括多帧图像，则从多帧图像中选取M帧图像作为待测图像，即将多帧图像中的部分图像作为待测图像，M可以为正整数，如1、2、3等。针对每帧待测图像，基于该待测图像与该三维视觉地图对应的多帧样本图像之间的相似度，从多帧样本图像中选取出候选样本图像。从该待测图像中获取多个特征点；针对每个特征点，从该候选样本图像对应的多个地图点中确定出与该特征点对应的目标地图点。基于多个特征点和多个特征点对应的目标地图点确定该待测图像对应的该三维视觉地图中的全局定位位姿。基于M帧待测图像对应的全局定位位姿生成终端设备在三维视觉地图中的全局定位轨迹。

示例性的，基于该待测图像与该三维视觉地图对应的多帧样本图像之间的相似度，从多帧样本图像中选取出候选样本图像，包括但不限于：确定该待测图像对应的待测全局描述子，确定该待测全局描述子与三维视觉地图对应的每帧样本图像对应的样本全局描述子之间的距离；其中，三维视觉地图包括每帧样本图像对应的样本全局描述子。基于该待测全局描述子与每个样本全局描述子之间的距离，从多帧样本图像中选取出候选样本图像；其中，该待测全局描述子与候选样本图像对应的样本全局描述子之间的距离为最小距离；或，该待测全局描述子与候选样本图像对应的样本全局描述子之间的距离小于距离阈值。

在一种可能的实施方式中，确定该待测图像对应的待测全局描述子，可以包括但不限于：基于已训练的字典模型确定该待测图像对应的词袋向量，并将该词袋向量确定为该待测图像对应的待测全局描述子；或者，将该待测图像输入给已训练的深度学习模型，得到该待测图像对应的目标向量，并将该目标向量确定为该待测图像对应的待测全局描述子。当然，上述只是确定待测全局描述子的两个示例，对此待测全局描述子的确定方式不做限制。

示例性的，从候选样本图像对应的多个地图点中确定出与该特征点对应的目标地图点子，可以包括但不限于：确定该特征点对应的待测局部描述子，该待测局部描述子用于表示该特征点所处图像块的特征向量，且该图像块可以位于该待测图像中。确定该待测局部描述子与该候选样本图像对应的每个地图点对应的样本局部描述子之间的距离；其中，该三维视觉地图至少包括该候选样本图像对应的每个地图点对应的样本局部描述子。然后，可以基于该待测局部描述子与每个样本局部描述子之间的距离，从该候选样本图像对应的多个地图点中选取目标地图点；其中，该待测局部描述子与目标地图点对应的样本局部描述子之间的距离可以为最小距离，且该最小距离小于距离阈值。

步骤104、基于自定位轨迹和全局定位轨迹生成终端设备在三维视觉地图中的融合定位轨迹，并输出该融合定位轨迹，如显示该融合定位轨迹。

示例性的，融合定位轨迹包括的融合定位位姿的帧率可以大于全局定位轨迹包括的全局定位位姿的帧率，也就是说，融合定位轨迹的帧率可以高于全局定位轨迹的帧率，融合定位轨迹可以是三维视觉地图中的高帧率位姿，全局定位轨迹可以是三维视觉地图中的低帧率位姿。融合定位轨迹的帧率高于全局定位轨迹的帧率，表示融合定位位姿的数量大于全局定位位姿的数量。

示例性的，融合定位轨迹包括的融合定位位姿的帧率可以等于自定位轨迹包括的自定位位姿的帧率，也就是说，融合定位轨迹的帧率可以等于自定位轨迹的帧率，即自定位轨迹可以是高帧率位姿。融合定位轨迹的帧率等于自定位轨迹的帧率，表示融合定位位姿的数量等于自定位位姿的数量。

示例性的，可以从自定位轨迹包括的所有自定位位姿中选取出与目标时间段对应的N个自定位位姿，并从全局定位轨迹包括的所有全局定位位姿中选取出与目标时间段对应的P个全局定位位姿；N大于P。基于N个自定位位姿和P个全局定位位姿确定N个自定位位姿对应的N个融合定位位姿，N个自定位位姿与N个融合定位位姿一一对应。基于N个融合定位位姿生成终端设备在三维视觉地图中的融合定位轨迹，该融合定位轨迹是三维视觉地图中的高帧率位姿。

示例性的，在基于自定位轨迹和全局定位轨迹生成终端设备在三维视觉地图中的融合定位轨迹之后，还可以从该融合定位轨迹中选取出初始融合定位位姿，并从该自定位轨迹中选取出与该初始融合定位位姿对应的初始自定位位姿。从该自定位轨迹中选取出目标自定位位姿，并基于该初始融合定位位姿、该初始自定位位姿和该目标自定位位姿确定目标融合定位位姿。然后，基于目标融合定位位姿和该融合定位轨迹生成新的融合定位轨迹，以替换原融合定位轨迹。

由以上技术方案可见，本申请实施例中，可以依据目标图像和运动数据进行高帧率的自定位，得到高帧率的自定位轨迹，可以依据目标图像和三维视觉地图进行低帧率的全局定位，得到低帧率的全局定位轨迹，然后，将高帧率的自定位轨迹和低帧率的全局定位轨迹进行融合，消除自定位累积误差，得到高帧率的融合定位轨迹，即三维视觉地图中的高帧率的融合定位轨迹，实现高帧率和高精度的定位功能，实现室内全局一致的高帧率定位功能。在上述方式中，目标场景可以是室内环境，可以基于目标图像和运动数据，实现高精度、低成本、易部署的室内定位功能，是一种基于视觉的室内定位方式，能够应用在煤炭、电力、石化等能源行业，实现人员(如工人、巡检人员等)的室内定位，快速获知人员的位置信息，保障人员安全，实现人员的高效管理。

以下结合具体实施例，对本申请实施例的位姿确定方法进行说明。

本申请实施例中提出一种位姿确定方法，在终端设备在目标场景的移动过程中，可以确定终端设备在三维视觉地图中的融合定位轨迹，并输出该融合定位轨迹。目标场景可以是室内环境，即终端设备在室内环境移动时，确定终端设备在三维视觉地图中的融合定位轨迹，即提出一种基于视觉的室内定位方式，当然，目标场景并不局限于室内环境，如室外环境等，对此不做限制。

参见图2所示，为终端设备的结构示意图，该终端设备可以包括自定位模块、全局定位模块和融合定位模块。终端设备还可以包括视觉传感器和运动传感器等，视觉传感器可以如摄像机等，视觉传感器用于在终端设备的移动过程中，采集目标场景的图像，为了区分方便，将该图像记为目标图像，且目标图像可以包括多帧图像(即终端设备移动过程中的多帧实时图像)。运动传感器可以如IMU(Inertial Measurement Unit，惯性测量单元)等，IMU一般指包含陀螺仪和加速度计的测量装置，运动传感器用于在终端设备的移动过程中，采集终端设备的运动数据，比如说，加速度和角速度等运动数据。

示例性的，终端设备可以为可穿戴设备(如视频安全帽、智能手表、智能眼镜等等)，且视觉传感器和运动传感器部署在可穿戴设备上；或者，终端设备为记录仪(如由工作人员在执行工作时随身携带，具有集实时视音频采集、照相、录音、对讲、定位等功能于一体的设备)，且视觉传感器和运动传感器部署在记录仪上；或者，终端设备为摄像机(如分体式摄像机等)，且视觉传感器和运动传感器部署在摄像机上。当然，上述只是示例，对此终端设备的类型不做限制，如还可以为智能手机等，只要部署视觉传感器和运动传感器即可。

参见图2所示，自定位模块可以获取目标图像和运动数据，依据目标图像和运动数据进行高帧率的自定位，得到高帧率的自定位轨迹(如6DOF(六自由度)的自定位轨迹)，并将高帧率的自定位轨迹发送给融合定位模块。示例性的，自定位轨迹可以包括多个自定位位姿，由于自定位轨迹是高帧率的自定位轨迹，因此，自定位轨迹中的自定位位姿的数量比较多。

全局定位模块可以获取目标图像，依据目标图像和目标场景的三维视觉地图进行低帧率的全局定位，得到低帧率的全局定位轨迹(即目标图像在三维视觉地图中的全局定位轨迹)，并将低帧率的全局定位轨迹发送给融合定位模块。示例性的，全局定位轨迹可以包括多个全局定位位姿，由于全局定位轨迹是低帧率的全局定位轨迹，因此，全局定位轨迹中的全局定位位姿的数量比较少。

融合定位模块可以获取高帧率的自定位轨迹和低帧率的全局定位轨迹，将高帧率的自定位轨迹和低帧率的全局定位轨迹进行融合，得到高帧率的融合定位轨迹，即三维视觉地图中的高帧率的融合定位轨迹，即得到高帧率全局定位结果。融合定位轨迹可以包括多个融合定位位姿，由于融合定位轨迹是高帧率的融合定位轨迹，因此，融合定位轨迹中的融合定位位姿的数量比较多。

在上述实施例中，位姿(如自定位位姿、全局定位位姿、融合定位位姿等)可以是位置与姿态，一般用旋转矩阵和平移向量表示，对此不做限制。

综上所述，本实施例中，基于目标图像和运动数据，就可以实现全局统一的高帧率视觉定位功能，得到三维视觉地图中的高帧率的融合定位轨迹(如6DOF位姿)，是高帧率全局一致的定位方法，实现了终端设备的高帧率、高精度、低成本、易部署的室内定位功能，实现室内全局一致的高帧率定位功能。

以下对自定位模块、全局定位模块和融合定位模块的功能进行说明。

一、自定位模块。自定位模块用于获取目标场景的目标图像和终端设备的运动数据，并基于该目标图像和该运动数据确定终端设备的自定位轨迹。

目标图像可以包括多帧图像，针对每帧图像，自定位模块确定与该图像对应的自定位位姿，即多帧图像对应多个自定位位姿，终端设备的自定位轨迹可以包括多个自定位位姿，可以理解为，自定位轨迹是多个自定位位姿的集合。

针对多帧图像中的第一帧图像，自定位模块确定第一帧图像对应的自定位位姿，针对多帧图像中的第二帧图像，自定位模块确定第二帧图像对应的自定位位姿，以此类推。第一帧图像对应的自定位位姿可以是参考坐标系(即自定位坐标系)的坐标原点，第二帧图像对应的自定位位姿是在参考坐标系中的位姿点，即相对于坐标原点(即第一帧图像对应的自定位位姿)的位姿点，第三帧图像对应的自定位位姿是在参考坐标系中的位姿点，即相对于坐标原点的位姿点，以此类推，各帧图像对应的自定位位姿均是在参考坐标系中的位姿点。

综上所述，在得到每帧图像对应的自定位位姿之后，就可以将这些自定位位姿组成参考坐标系中的自定位轨迹，该自定位轨迹包括这些自定位位姿。

在一种可能的实施方式中，参见图3所示，采用如下步骤确定自定位轨迹：

步骤301、获取目标场景的目标图像和终端设备的运动数据。

步骤302、若目标图像包括多帧图像，则从多帧图像中遍历出当前帧图像。

在从多帧图像中遍历出第一帧图像作为当前帧图像时，第一帧图像对应的自定位位姿可以是参考坐标系(即自定位坐标系)的坐标原点，即自定位位姿与该坐标原点重合。在从多帧图像中遍历出第二帧图像作为当前帧图像时，可以采用后续步骤确定第二帧图像对应的自定位位姿。在从多帧图像中遍历出第三帧图像作为当前帧图像时，可以采用后续步骤确定第三帧图像对应的自定位位姿，以此类推，可以遍历出每帧图像作为当前帧图像。

步骤303、利用光流算法计算当前帧图像与当前帧图像的前一帧图像之间的特征点关联关系。其中，光流算法是利用当前帧图像中像素在时间域上的变化以及前一帧图像之间的相关性，来找到当前帧图像与前一帧图像之间存在的对应关系，从而计算出当前帧图像与前一帧图像之间物体的运动信息的方式。

步骤304、基于当前帧图像与前一帧图像之间的匹配特征点数量确定当前帧图像是否为关键图像。比如说，若当前帧图像与前一帧图像之间的匹配特征点数量未达到预设阈值，则用于表示当前帧图像与前一帧图像的变化较大，导致两帧图像之间的匹配特征点数量比较少，则确定当前帧图像是关键图像，执行步骤305。若当前帧图像与前一帧图像之间的匹配特征点数量达到预设阈值，则用于表示当前帧图像与前一帧图像的变化较小，导致两帧图像之间的匹配特征点数量比较多，则确定当前帧图像是非关键图像，执行步骤306。

示例性的，还可以基于当前帧图像与前一帧图像之间的匹配特征点数量计算当前帧图像与前一帧图像之间的匹配比例，比如说，匹配特征点数量与总特征点数量的比例。若该匹配比例未达到预设比例，则确定当前帧图像是关键图像，若该匹配比例达到预设比例，则确定当前帧图像是非关键图像。

步骤305、若当前帧图像是关键图像，则基于终端设备的当前位置(即终端设备采集当前帧图像时所处的位置)生成自定位坐标系(即参考坐标系)中的地图位置，即生成一个新的3D地图位置。若当前帧图像是非关键图像，则不需要基于终端设备的当前位置生成自定位坐标系中的地图位置。

步骤306、基于当前帧图像前面的K帧图像对应的自定位位姿、终端设备在自定位坐标系中的地图位置和终端设备的运动数据，确定当前帧图像对应的自定位位姿，K可以为正整数，可以是根据经验配置的数值，对此不做限制。

比如说，可以对当前帧图像的前一帧图像与当前帧图像之间的所有运动数据进行预积分，得到这两帧图像之间的惯性测量约束。基于当前帧图像前面的K帧图像(如滑窗)对应的自定位位姿和运动数据(例如速度、加速度、角速度等)、自定位坐标系中的地图位置、惯性测量约束(前一帧图像与当前帧图像之间的速度、加速度、角速度等)，可以采用捆集优化进行联合优化更新，得到当前帧图像对应的自定位位姿，对此捆集优化过程不做限制。

示例性的，为了维护待优化变量的规模，还可以将滑窗内的某个帧及部分地图位置进行边缘化，并将这些约束信息以先验的形式保留下来。

示例性的，自定位模块可以采用VIO(Visual Inertial Odometry，视觉惯性里程计)算法确定自定位位姿，也就是说，VIO算法的输入数据是目标图像和运动数据，VIO算法的输出数据是自定位位姿，比如说，基于目标图像和运动数据，VIO算法可以得到自定位位姿，例如，采用VIO算法执行步骤301-步骤306，得到自定位位姿。该VIO算法可以包括但不限于VINS(Visual Inertial Navigation Systems，视觉惯性导航系统)、SVO(Semi-directVisual Odometry，半直接视觉里程计)、MSCKF(Multi State Constraint Kalman Filter，多状态约束下的Kalman滤波器)等，这里不做限定，只要能够得到自定位位姿即可。

步骤307、基于多帧图像对应的自定位位姿生成终端设备在自定位坐标系中的自定位轨迹，该自定位轨迹包括自定位坐标系中的多个自定位位姿。

至此，自定位模块可以得到自定位坐标系中的自定位轨迹，该自定位轨迹可以包括多帧图像对应的自定位位姿，由于视觉传感器可以采集大量图像，因此，自定位模块可以得到这些图像对应的自定位位姿，即自定位轨迹可以包括大量自定位位姿，也就是说，自定位模块能够得到高帧率的自定位轨迹。

二、全局定位模块。基于已获取的目标场景的三维视觉地图，全局定位模块在得到目标图像后，从目标场景的三维视觉地图中确定出与目标图像对应的目标地图点，基于目标地图点确定终端设备在三维视觉地图中的全局定位轨迹。

目标图像可以包括多帧图像，可以从多帧图像中选取出部分图像作为待测图像，后续以选取M帧图像作为待测图像为例，M为正整数。针对每帧待测图像，全局定位模块可以确定与该待测图像对应的全局定位位姿，即M帧待测图像对应M个全局定位位姿，终端设备在三维视觉地图中的全局定位轨迹可以包括M个全局定位位姿，可以理解为，全局定位轨迹是M个全局定位位姿的集合。

针对M帧待测图像中的第一帧待测图像，全局定位模块确定第一帧待测图像对应的全局定位位姿，针对第二帧待测图像，全局定位模块确定第二帧待测图像对应的全局定位位姿，以此类推。针对每个全局定位位姿，该全局定位位姿是三维视觉地图中的位姿点，即在三维视觉地图坐标系中的位姿点。

综上所述，在得到M帧待测图像对应的全局定位位姿后，将这些全局定位位姿组成三维视觉地图中的全局定位轨迹，该全局定位轨迹包括全局定位位姿。

在一种可能的实施方式中，需要预先构建目标场景的三维视觉地图，并将三维视觉地图存储到服务器，当终端设备需要在目标场景移动时，终端设备可以从服务器下载目标场景的三维视觉地图，并存储该三维视觉地图，这样，在终端设备的移动过程中，可以基于该三维视觉地图确定终端设备在三维视觉地图中的全局定位轨迹。三维视觉地图是对目标场景的图像信息的存储方式，可以采集目标场景的多帧样本图像，基于这些样本图像构建三维视觉地图，比如说，基于目标场景的多帧样本图像，采用SFM(Structure FromMotion，运动恢复结构)或者SLAM(Simultaneous Localization And Mapping，同时定位与建图)等视觉建图算法，构建目标场景的三维视觉地图，对此构建方式不做限制。

在得到目标场景的三维视觉地图之后，该三维视觉地图可以包括如下信息：

样本图像位姿：样本图像是构建三维视觉地图时具有代表性的图像，即可以基于样本图像构建三维视觉地图，样本图像的位姿矩阵(简称样本图像位姿)可以被存储到三维视觉地图中，也就是说，三维视觉地图包括样本图像位姿。

样本全局描述子：针对每帧样本图像，该样本图像可以对应图像全局描述子，将该图像全局描述子记为样本全局描述子，样本全局描述子是用高维向量来表示该样本图像，样本全局描述子用于区分不同样本图像的图像特征。

其中，针对每帧样本图像，可以基于已训练的字典模型确定该样本图像对应的词袋向量，并将该词袋向量确定为该样本图像对应的样本全局描述子。比如说，视觉词袋(Bagof Words)方法是一种用于确定全局描述子的方式，在视觉词袋方法中，可以构建词袋向量，该词袋向量是一种用于图像相似检测的向量表示方法，可以将该词袋向量作为样本图像对应的样本全局描述子。

在视觉词袋方法中，需要预先训练一个“字典”，也称为字典模型，一般是利用大量图像中的特征点描述子进行聚类，训练得到一个分类树，每一类分类树可以代表一种视觉“单词”，这些视觉“单词”就组成字典模型。

针对样本图像来说，可以将该样本图像中的所有特征点描述子进行“单词”分类，并统计所有单词的出现频率，这样，字典中每个单词的频率可构成一个向量，该向量即为该样本图像对应的词袋向量，该词袋向量可以用于衡量两幅图像的相似程度，将该词袋向量作为该样本图像对应的样本全局描述子。

其中，针对每帧样本图像，可以将该样本图像输入给已训练的深度学习模型，得到该样本图像对应的目标向量，并将该目标向量确定为该样本图像对应的样本全局描述子。比如说，深度学习方法是一种用于确定全局描述子的方式，在深度学习方法中，可以通过深度学习模型对样本图像进行多层卷积，并最终得到一个高维的目标向量，将该目标向量作为样本图像对应的样本全局描述子。

在深度学习方法中，需要预先训练深度学习模型，如CNN(Convolutional NeuralNetworks，卷积神经网络)模型等，一般是利用大量图像训练得到深度学习模型，对此深度学习模型的训练方式不做限制。针对样本图像来说，可以将该样本图像输入给深度学习模型，由深度学习模型对该样本图像进行处理，得到一个高维的目标向量，将该目标向量作为该样本图像对应的样本全局描述子。

样本图像的特征点对应的样本局部描述子：针对每帧样本图像，该样本图像可以包括多个特征点，特征点可以是该样本图像中具有特异性的像素点位置，该特征点可以对应一个图像局部描述子，将该图像局部描述子记为样本局部描述子，样本局部描述子是用一个向量来描述特征点(即像素点位置)附近范围内的图像块的特征，该向量也可以称为特征点的描述子。综上所述，样本局部描述子是用于表示特征点所处图像块的特征向量，且该图像块可以位于该样本图像中。需要注意的是，样本图像的特征点(即二维特征点)与三维视觉地图中的地图点(即三维地图点)对应，因此，样本图像的特征点对应的样本局部描述子，也就是该特征点对应的地图点对应的样本局部描述子。

其中，可以采用ORB(Oriented FAST and Rotated BRIEF，定向快速旋转)、SIFT(Scale-Invariant Feature Transform，尺度不变特征变换)、SURF(Speeded Up RobustFeatures，加速稳健特征)等算法，从样本图像中提取特征点，并确定特征点对应的样本局部描述子。也可以采用深度学习算法(如SuperPoint，DELF，D2-Net等)，从样本图像中提取特征点，并确定特征点对应的样本局部描述子，对此不做限制，只要能够得到特征点，并确定样本局部描述子即可。

地图点信息(即特征点信息)：地图点信息可以包括但不限于：该地图点的3D空间位置、所有被观测的样本图像以及对应的2D特征点编号。

基于目标场景的三维视觉地图，在一种可能的实施方式中，参见图4所示，全局定位模块采用如下步骤确定终端设备在该三维视觉地图中的全局定位轨迹：

步骤401、获取目标场景的目标图像，若目标图像包括多帧图像，则从多帧图像中选取M帧图像作为待测图像，即待测图像为M帧，M可以为正整数。

比如说，参见步骤304，多帧图像包括关键图像和非关键图像，在此基础上，将多帧图像中的关键图像作为待测图像，而非关键图像不作为待测图像。

又例如，可以按照固定间隔从多帧图像中选取出待测图像，假设固定间隔是5(当然，固定间隔可以根据经验任意配置，对此不做限制)，则可以将第1帧图像作为待测图像，将第6(1+5)帧图像作为待测图像，将第11(6+5)帧图像作为待测图像，以此类推，每隔5帧图像选取出一帧待测图像。

当然，上述待测图像的选取方式只是两个示例，只要能够从多帧图像中选取出部分图像作为待测图像即可，对此待测图像的选取方式不做限制。

步骤402、针对每帧待测图像，确定该待测图像对应的待测全局描述子。

示例性的，针对每帧待测图像，该待测图像可以对应图像全局描述子，可以将该图像全局描述子记为待测全局描述子，待测全局描述子是用高维向量来表示该待测图像，待测全局描述子用于区分不同待测图像的图像特征。

其中，针对每帧待测图像，基于已训练的字典模型确定该待测图像对应的词袋向量，将该词袋向量确定为该待测图像对应的待测全局描述子。或者，针对每帧待测图像，将该待测图像输入给已训练的深度学习模型，得到该待测图像对应的目标向量，将该目标向量确定为该待测图像对应的待测全局描述子。

综上所述，可以基于视觉词袋方法或者深度学习方法确定待测图像对应的待测全局描述子，确定方式参见样本全局描述子的确定方式，在此不再赘述。

步骤403、针对每帧待测图像，确定该待测图像对应的待测全局描述子与三维视觉地图对应的每帧样本图像对应的样本全局描述子之间的相似度。

参见上述实施例，三维视觉地图可以包括每帧样本图像对应的样本全局描述子，因此，可以确定该待测全局描述子与每个样本全局描述子之间的相似度，以相似度是“距离相似度”为例，则可以确定待测全局描述子与每个样本全局描述子之间距离，如欧式距离，即计算两个特征向量之间的欧式距离。

步骤404、基于该待测全局描述子与每个样本全局描述子之间的距离，从三维视觉地图对应的多帧样本图像中选取出候选样本图像；其中，该待测全局描述子与候选样本图像对应的样本全局描述子之间的距离为最小距离；或，该待测全局描述子与候选样本图像对应的样本全局描述子之间的距离小于距离阈值。

比如说，假设三维视觉地图对应样本图像1、样本图像2和样本图像3，则可以计算该待测全局描述子与样本图像1对应的样本全局描述子之间的距离1，并计算该待测全局描述子与样本图像2对应的样本全局描述子之间的距离2，并计算该待测全局描述子与样本图像3对应的样本全局描述子之间的距离3。

在一种可能的实施方式中，若距离1是最小距离，则将样本图像1选取为候选样本图像。或者，若距离1小于距离阈值(可以根据经验进行配置)，且距离2小于距离阈值，但是距离3不小于距离阈值，则将样本图像1和样本图像2均选取为候选样本图像。或者，若距离1是最小距离，且距离1小于距离阈值，则将样本图像1选取为候选样本图像，但是，若距离1是最小距离，且距离1不小于距离阈值，则无法选取出候选样本图像，即重定位失败。

综上所述，针对每帧待测图像，从三维视觉地图对应的多帧样本图像中选取出与该待测图像对应的候选样本图像，候选样本图像的数量可以为至少一个。

步骤405、针对每帧待测图像，从该待测图像中获取多个特征点，针对每个特征点，确定该特征点对应的待测局部描述子，该待测局部描述子用于表示该特征点所处图像块的特征向量，且该图像块可以位于该待测图像中。

比如说，该待测图像可以包括多个特征点，特征点可以是该待测图像中具有特异性的像素点位置，该特征点可以对应一个图像局部描述子，将该图像局部描述子记为待测局部描述子，待测局部描述子是用一个向量来描述特征点(即像素点位置)附近范围内的图像块的特征，该向量也可以称为特征点的描述子。综上所述，待测局部描述子是用于表示特征点所处图像块的特征向量。

其中，可以采用ORB、SIFT、SURF等算法，从待测图像中提取特征点，并确定特征点对应的待测局部描述子。也可以采用深度学习算法(如SuperPoint，DELF，D2-Net等)，从待测图像中提取特征点，并确定特征点对应的待测局部描述子，对此不做限制，只要能够得到特征点，并确定待测局部描述子即可。

步骤406、针对待测图像对应的每个特征点，确定该特征点对应的待测局部描述子与该待测图像对应的候选样本图像对应的每个地图点对应的样本局部描述子之间的距离，如欧式距离，即计算两个特征向量之间的欧式距离。

参见上述实施例，针对每帧样本图像，三维视觉地图包括该样本图像对应的每个地图点对应的样本局部描述子(即该样本图像中的每个特征点对应的地图点对应的样本局部描述子)，因此，在得到待测图像对应的候选样本图像之后，可以从三维视觉地图中获取候选样本图像对应的每个地图点对应的样本局部描述子。在得到待测图像对应的每个特征点后，确定该特征点对应的待测局部描述子与候选样本图像对应的每个地图点对应的样本局部描述子之间的距离。

步骤407、针对每个特征点，基于该特征点对应的待测局部描述子与候选样本图像对应的每个地图点对应的样本局部描述子之间的距离，从候选样本图像对应的多个地图点中选取目标地图点；其中，该待测局部描述子与目标地图点对应的样本局部描述子之间的距离为最小距离，且最小距离小于距离阈值。

比如说，假设候选样本图像对应地图点1、地图点2和地图点3，则可以计算该特征点对应的待测局部描述子与地图点1对应的样本局部描述子之间的距离1，并计算该待测局部描述子与地图点2对应的样本局部描述子之间的距离2，并计算该待测局部描述子与地图点3对应的样本局部描述子之间的距离3。

在一种可能的实施方式中，若距离1是最小距离，则可以将地图点1选取为目标地图点。或者，若距离1小于距离阈值(可以根据经验进行配置)，且距离2小于距离阈值，但是距离3不小于距离阈值，则可以将地图点1和地图点2均选取为目标地图点。或者，若距离1是最小距离，且距离1小于距离阈值，则可以将地图点1选取为目标地图点，但是，若距离1是最小距离，且距离1不小于距离阈值，则无法选取出目标地图点，即重定位失败。

综上所述，针对待测图像的每个特征点，从待测图像对应的候选样本图像中选取与该特征点对应的目标地图点，得到特征点与目标地图点的匹配关系。

步骤408、基于待测图像对应的多个特征点以及多个特征点对应的目标地图点，确定该待测图像对应的三维视觉地图中的全局定位位姿。

针对一帧待测图像来说，该待测图像可以对应多个特征点，且每个特征点对应一个目标地图点，比如说，特征点1对应的目标地图点是地图点1，特征点2对应的目标地图点是地图点2，以此类推，从而得到多个匹配关系对，每个匹配关系对包括一个特征点(即二维特征点)和一个地图点(即三维视觉地图中的三维地图点)，该特征点表示待测图像中的二维位置，该地图点表示三维视觉地图中的三维位置，即匹配关系对包括二维位置到三维位置的映射关系，即待测图像中的二维位置到三维视觉地图中的三维位置的映射关系。

若多个匹配关系对的总数量未达到数量要求，则表示无法基于多个匹配关系对确定出该待测图像对应的三维视觉地图中的全局定位位姿。若多个匹配关系对的总数量达到数量要求(即总数量达到预设数量值)，则表示能够基于多个匹配关系对确定出该待测图像对应的三维视觉地图中的全局定位位姿，也就是说，可以基于多个匹配关系对确定出该待测图像对应的三维视觉地图中的全局定位位姿。比如说，采用PnP(Perspective N Point，n点透视)算法计算出待测图像在三维视觉地图中的全局定位位姿，对此计算方式不做限制。例如，PnP算法的输入数据是多个匹配关系对，针对每个匹配关系对，该匹配关系对包括待测图像中的二维位置和三维视觉地图中的三维位置，基于多个匹配关系对就可以采用PnP算法计算出待测图像在三维视觉地图中的位姿，即全局定位位姿。

综上所述，针对每帧待测图像，可以得到待测图像对应的三维视觉地图中的全局定位位姿，即得到该待测图像在三维视觉地图坐标系中的全局定位位姿。

在一种可能的实施方式中，在得到多个匹配关系对之后，还可以从多个匹配关系对中查找到有效的匹配关系对。基于这些有效的匹配关系对，则可以采用PnP算法计算出该待测图像在三维视觉地图中的全局定位位姿。比如说，可以采用RANSAC(RANdom SAmpleConsensus，随机样本一致性)检测算法，从所有匹配关系对中找到有效的匹配关系对，对此过程不做限制。

步骤409、基于M帧待测图像对应的全局定位位姿生成终端设备在三维视觉地图中的全局定位轨迹，该全局定位轨迹包括三维视觉地图中的多个全局定位位姿。至此，全局定位模块可以得到三维视觉地图中的全局定位轨迹，也就是三维视觉地图坐标系中的全局定位轨迹，该全局定位轨迹可以包括M帧待测图像对应的全局定位位姿，即该全局定位轨迹可以包括M个全局定位位姿。由于M帧待测图像是从所有图像中选取的部分图像，因此，全局定位轨迹可以包括少量全局定位位姿，也就是说，全局定位模块得到低帧率的全局定位轨迹。

三、融合定位模块。可以从自定位模块得到高帧率的自定位轨迹，并从全局定位模块得到低帧率的全局定位轨迹，并将高帧率的自定位轨迹与低帧率的全局定位轨迹进行融合，得到三维视觉地图坐标系下的高帧率的融合定位轨迹，即终端设备在三维视觉地图中的融合定位轨迹，并输出该融合定位轨迹。其中，融合定位轨迹是三维视觉地图中的高帧率位姿，全局定位轨迹是三维视觉地图中的低帧率位姿，即融合定位轨迹的帧率高于全局定位轨迹的帧率，融合定位轨迹包括的融合定位位姿数量大于全局定位轨迹包括的全局定位位姿数量。

参见图5所示，白色实线圆表示自定位位姿，将多个自定位位姿组成的轨迹称为自定位轨迹，即自定位轨迹包括多个自定位位姿。第一帧图像对应的自定位位姿可以是参考坐标系S_L(即自定位坐标系)的坐标原点，将第一帧图像对应的自定位位姿记为

自定位位姿

与参考坐标系S_L的坐标原点重合。针对自定位轨迹中的每个自定位位姿，是在参考坐标系S_L下的自定位位姿。

灰色实线圆表示全局定位位姿，将多个全局定位位姿组成的轨迹称为全局定位轨迹，即全局定位轨迹包括多个全局定位位姿，全局定位位姿可以是三维视觉地图坐标系S_G下的位姿，即全局定位轨迹中的每个全局定位位姿均是三维视觉地图坐标系S_G下的全局定位位姿，也即三维视觉地图下的全局定位位姿。

白色虚线圆表示融合定位位姿，将多个融合定位位姿组成的轨迹称为融合定位轨迹，即融合定位轨迹包括多个融合定位位姿，融合定位位姿可以是三维视觉地图坐标系S_G下的位姿，即融合定位轨迹中的每个融合定位位姿均是三维视觉地图坐标系S_G下的融合定位位姿，也即三维视觉地图下的融合定位位姿。

参见图5所示，由于目标图像包括多帧图像，每帧图像对应一个自定位位姿，并从多帧图像中选取部分图像作为待测图像，每帧待测图像对应一个全局定位位姿，因此，自定位位姿的数量大于全局定位位姿的数量。在基于自定位轨迹和全局定位轨迹得到融合定位轨迹时，每个自定位位姿对应一个融合定位位姿(即自定位位姿与融合定位位姿一一对应)，即自定位位姿的数量与融合定位位姿的数量相同，因此，融合定位位姿的数量也大于全局定位位姿的数量。

在一种可能的实施方式中，融合定位模块可以实现轨迹融合功能和位姿变换功能，参见图6所示，融合定位模块可以采用如下步骤实现轨迹融合功能和位姿变换功能，以得到终端设备在该三维视觉地图中的融合定位轨迹：

步骤601、从自定位轨迹包括的所有自定位位姿中选取出与目标时间段对应的N个自定位位姿，并从全局定位轨迹包括的所有全局定位位姿中选取出与目标时间段对应的P个全局定位位姿，示例性的，N可以大于P。

比如说，在对目标时间段的自定位轨迹和全局定位轨迹进行融合时，可以确定目标时间段对应的N个自定位位姿(即基于目标时间段采集的图像确定的自定位位姿)，并确定目标时间段对应的P个全局定位位姿(即基于目标时间段采集的图像确定的全局定位位姿)，参见图5所示，可以将

和

之间的自定位位姿作为目标时间段对应的N个自定位位姿，可以将

和

之间的全局定位位姿作为目标时间段对应的P个全局定位位姿。

步骤602、基于N个自定位位姿和P个全局定位位姿确定N个自定位位姿对应的N个融合定位位姿，N个自定位位姿与N个融合定位位姿一一对应。

比如说，参见图5所示，可以基于N个自定位位姿和P个全局定位位姿，确定自定位位姿

对应的融合定位位姿

确定自定位位姿

对应的融合定位位姿

确定自定位位姿

对应的融合定位位姿

以此类推。

在一种可能的实施方式中，假设存在N个自定位位姿、P个全局定位位姿和N个融合定位位姿，N个自定位位姿均为已知值，P个全局定位位姿均为已知值，N个融合定位位姿均是未知值，是需要求解的位姿值。如图5所示，自定位位姿

与融合定位位姿

对应，自定位位姿

与融合定位位姿

对应，自定位位姿

与融合定位位姿

对应，以此类推。全局定位位姿

与融合定位位姿

时应，全局定位位姿

与融合定位位姿

对应，以此类推。

可以基于N个自定位位姿和N个融合定位位姿确定第一约束值，第一约束值用于表示融合定位位姿与自定位位姿之间的残差值，如可以基于

与

的差值、

与

的差值、...、

与

的差值，计算第一约束值。关于第一约束值的计算公式，本实施例中不做限制，与上述各差值有关即可。

可以基于P个全局定位位姿和P个融合定位位姿(即从N个融合定位位姿中选取出与P个全局定位位姿对应的P个融合定位位姿)确定第二约束值，第二约束值用于表示融合定位位姿与全局定位位姿之间的残差值(即绝对差值)，如可以基于

与

的差值、...、

与

的差值，计算第二约束值。关于第二约束值的计算公式，本实施例中不做限制，与上述各差值有关即可。

可以基于第一约束值和第二约束值计算目标约束值，如目标约束值可以为第一约束值与第二约束值之和。由于N个自定位位姿和P个全局定位位姿均为已知值，N个融合定位位姿均是未知值，因此，通过调整N个融合定位位姿的取值，使得目标约束值为最小。当目标约束值为最小时，N个融合定位位姿的取值就是最终求解的位姿值，至此，得到N个融合定位位姿的取值。

在一种可能的实施方式中，可以采用公式(1)计算目标约束值：

在公式(1)中，F(T)表示目标约束值，加号前面的部分(后续记为第一部分)为第一约束值，加号后面的部分(后续记为第二部分)为第二约束值，当然，上述只是目标约束值、第一约束值和第二约束值的示例，对此不做限制。

Ω_i，i+1是针对自定位位姿的残差信息矩阵，可以根据经验配置，对此不做限制，Ω_k是针对全局定位位姿的残差信息矩阵，可以根据经验配置，对此不做限制。

第一部分表示自定位位姿与融合定位位姿的相对变换约束，可以通过第一约束值反映，N为自定位轨迹中的所有自定位位姿，即N个自定位位姿。第二部分表示全局定位位姿与融合定位位姿的全局定位约束，可以通过第二约束值反映，P为全局定位轨迹中的所有全局定位位姿，即P个全局定位位姿。

针对第一部分和第二部分，还可以通过公式(2)和公式(3)表示：

在公式(2)和公式(3)中，

和

为融合定位位姿(没有对应的全局定位位姿)，

和

为自定位位姿，

为两个自定位位姿之间的相对位姿变化约束，e_i，i+1为

和

相对位姿变化与

约束的残差。

为融合定位位姿(有对应的全局定位位姿

)，

为

对应的全局定位位姿，e_k表示融合定位位姿

相对于全局定位位姿

的残差。

由于自定位位姿和全局定位位姿为已知，融合定位位姿为未知，优化目标可以是使F(T)的取值为最小，从而可以得到融合定位位姿，即三维视觉地图坐标系下的融合定位轨迹可以参见公式(4)：arg min F(T)，通过使F(T)的取值为最小，就可以得到融合定位轨迹，且该融合定位轨迹可以包括多个融合定位位姿。

示例性的，为了使F(T)的取值为最小，可以采用高斯牛顿、梯度下降、LM(Levenberg-Marquardt)等算法求解，得到融合定位位姿，在此不再赘述。

步骤603、基于N个融合定位位姿生成终端设备在三维视觉地图中的融合定位轨迹，该融合定位轨迹包括三维视觉地图中的N个融合定位位姿。至此，融合定位模块可以得到三维视觉地图中的融合定位轨迹，即三维视觉地图坐标系中的融合定位轨迹，该融合定位轨迹中的融合定位位姿的数量大于全局定位轨迹中的全局定位位姿的数量，也就是说，能够得到高帧率的融合定位轨迹。

步骤604、从该融合定位轨迹中选取出初始融合定位位姿，并从该自定位轨迹中选取出与该初始融合定位位姿对应的初始自定位位姿。

步骤605、从该自定位轨迹中选取出目标自定位位姿，并基于该初始融合定位位姿、该初始自定位位姿和该目标自定位位姿确定目标融合定位位姿。

示例性的，在生成该融合定位轨迹之后，还可以对该融合定位轨迹进行更新，在轨迹更新过程中，可以从该融合定位轨迹中选取出初始融合定位位姿，并从该自定位轨迹中选取出初始自定位位姿，并从该自定位轨迹中选取出目标自定位位姿。在此基础上，可以基于该初始融合定位位姿、该初始自定位位姿和该目标自定位位姿确定目标融合定位位姿。然后，可以基于该目标融合定位位姿和该融合定位轨迹生成新的融合定位轨迹，以替换原融合定位轨迹。

比如说，在步骤601-步骤603中，参见图5所示，自定位轨迹包括

和

之间的自定位位姿，全局定位轨迹包括

和

之间的全局定位位姿，融合定位轨迹包括

和

之间的融合定位位姿，在此之后，若得到新的自定位位姿

但是，由于没有对应的全局定位位姿，则无法基于全局定位位姿和自定位位姿

确定出自定位位姿

对应的融合定位位姿

在此基础上，本实施例中，还可以采用如下公式(4)确定融合定位位姿

在公式(4)中，

表示自定位位姿

对应的融合定位位姿，即目标融合定位位姿，

表示融合定位位姿，即从融合定位轨迹中选取的初始融合定位位姿，

表示表示自定位位姿，即从自定位轨迹中选取的与

对应的初始自定位位姿，

表示表示自定位位姿，即从自定位轨迹中选取的目标自定位位姿。综上可以看出，可以基于该初始融合定位位姿

该初始自定位位姿

和该目标自定位位姿

确定目标融合定位位姿

在得到目标融合定位位姿

之后，可以生成新的融合定位轨迹，即新的融合定位轨迹可以包括目标融合定位位姿

从而对融合定位轨迹进行更新。

在上述过程中，步骤601-步骤603是轨迹融合过程，步骤604-步骤605是位姿变换过程，轨迹融合是将自定位轨迹与全局定位轨迹进行配准融合的过程，实现自定位轨迹从自定位坐标系到三维视觉地图坐标系的转换，利用全局定位结果对轨迹进行修正，当新的一帧能够获得全局定位轨迹，进行一次轨迹融合。由于不是所有的帧都能够成功获得全局定位轨迹，因此，这些帧的位姿是通过位姿变换的方式输出三维视觉地图坐标系的融合定位位姿，即位姿变换过程。

基于与上述方法同样的申请构思，本申请实施例中提出一种位姿确定装置，应用于终端设备，所述终端设备包括目标场景的三维视觉地图，在终端设备在目标场景的移动过程中，参见图7所示，为所述装置的结构图，所述装置包括：

获取模块71，用于获取所述目标场景的目标图像和所述终端设备的运动数据；确定模块72，用于基于所述目标图像和所述运动数据确定所述终端设备的自定位轨迹；从所述三维视觉地图中确定出与所述目标图像对应的目标地图点，基于所述目标地图点确定所述终端设备在所述三维视觉地图中的全局定位轨迹；生成模块73，用于基于所述自定位轨迹和所述全局定位轨迹生成所述终端设备在三维视觉地图中的融合定位轨迹，并输出融合定位轨迹；所述融合定位轨迹包括的融合定位位姿的帧率大于所述全局定位轨迹包括的全局定位位姿的帧率。

示例性的，所述确定模块72基于所述目标图像和所述运动数据确定所述终端设备的自定位轨迹时具体用于：若所述目标图像包括多帧图像，则从多帧图像中遍历出当前帧图像；基于当前帧图像前面的K帧图像对应的自定位位姿、所述终端设备在自定位坐标系中的地图位置和所述运动数据确定当前帧图像对应的自定位位姿；基于多帧图像对应的自定位位姿生成所述终端设备在自定位坐标系中的自定位轨迹；若当前帧图像是关键图像，则基于所述终端设备的当前位置生成自定位坐标系中的地图位置；若当前帧图像与当前帧图像的前一帧图像之间的匹配特征点数量未达到预设阈值，则确定当前帧图像是关键图像。

示例性的，所述确定模块72从三维视觉地图中确定出与所述目标图像对应的目标地图点，基于所述目标地图点确定所述终端设备在所述三维视觉地图中的全局定位轨迹时具体用于：若目标图像包括多帧图像，从多帧图像中选取M帧图像作为待测图像；针对每帧待测图像，基于所述待测图像与三维视觉地图对应的多帧样本图像之间的相似度，从多帧样本图像中选取出候选样本图像；从待测图像中获取多个特征点；针对每个特征点，从候选样本图像对应的多个地图点中确定出与该特征点对应的目标地图点；基于所述多个特征点和所述多个特征点对应的目标地图点确定所述待测图像对应的三维视觉地图中的全局定位位姿；基于M帧待测图像对应的全局定位位姿生成所述终端设备在三维视觉地图中的全局定位轨迹。

示例性的，所述确定模块72基于待测图像与三维视觉地图对应的多帧样本图像之间的相似度，从多帧样本图像中选取出候选样本图像时具体用于：确定待测图像对应的待测全局描述子，确定待测全局描述子与所述三维视觉地图对应的每帧样本图像对应的样本全局描述子之间的距离；其中，所述三维视觉地图包括每帧样本图像对应的样本全局描述子；基于所述待测全局描述子与每个样本全局描述子之间的距离，从所述多帧样本图像中选取出候选样本图像；所述待测全局描述子与候选样本图像对应的样本全局描述子之间的距离为最小距离；或所述待测全局描述子与候选样本图像对应的样本全局描述子之间的距离小于距离阈值。

示例性的，所述确定模块72确定待测图像对应的待测全局描述子时具体用于：基于已训练的字典模型确定所述待测图像对应的词袋向量，并将所述词袋向量确定为所述待测图像对应的待测全局描述子；或者，将所述待测图像输入给已训练的深度学习模型，得到所述待测图像对应的目标向量，并将所述目标向量确定为所述待测图像对应的待测全局描述子。

示例性的，所述确定模块72从所述候选样本图像对应的多个地图点中确定出与该特征点对应的目标地图点时具体用于：确定该特征点对应的待测局部描述子，所述待测局部描述子用于表示该特征点所处图像块的特征向量，且所述图像块位于所述待测图像中；确定所述待测局部描述子与所述候选样本图像对应的每个地图点对应的样本局部描述子之间的距离；其中，所述三维视觉地图至少包括所述候选样本图像对应的每个地图点对应的样本局部描述子；基于所述待测局部描述子与每个样本局部描述子之间的距离，从所述多个地图点中选取目标地图点；所述待测局部描述子与目标地图点对应的样本局部描述子之间的距离为最小距离，且所述最小距离小于距离阈值。

示例性的，所述生成模块73基于所述自定位轨迹和所述全局定位轨迹生成所述终端设备在所述三维视觉地图中的融合定位轨迹时具体用于：从所述自定位轨迹包括的所有自定位位姿中选取出与目标时间段对应的N个自定位位姿，从所述全局定位轨迹包括的所有全局定位位姿中选取出与所述目标时间段对应的P个全局定位位姿；所述N大于所述P；基于所述N个自定位位姿和所述P个全局定位位姿确定所述N个自定位位姿对应的N个融合定位位姿，所述N个自定位位姿与所述N个融合定位位姿一一对应；基于所述N个融合定位位姿生成所述终端设备在所述三维视觉地图中的融合定位轨迹；其中，所述融合定位轨迹包括的融合定位位姿的帧率等于所述自定位轨迹包括的自定位位姿的帧率。

示例性的，所述生成模块73基于自定位轨迹和全局定位轨迹生成终端设备在三维视觉地图中的融合定位轨迹后还用于：从所述融合定位轨迹中选取出初始融合定位位姿；从所述自定位轨迹中选取与所述初始融合定位位姿对应的初始自定位位姿；从所述自定位轨迹中选取出目标自定位位姿，并基于所述初始融合定位位姿、所述初始自定位位姿和所述目标自定位位姿确定目标融合定位位姿；基于所述目标融合定位位姿和所述融合定位轨迹生成新的融合定位轨迹。

基于与上述方法同样的申请构思，本申请实施例中提出一种终端设备，所述终端设备可以包括：处理器和机器可读存储介质，所述机器可读存储介质存储有能够被所述处理器执行的机器可执行指令；所述处理器用于执行机器可执行指令，以实现本申请上述示例公开的位姿确定方法。

基于与上述方法同样的申请构思，本申请实施例中提出一种终端设备，包括：视觉传感器，用于在所述终端设备在目标场景的移动过程中，获取所述目标场景的目标图像，将所述目标图像输入给处理器；运动传感器，用于在所述终端设备在目标场景的移动过程中，获取所述终端设备的运动数据，将所述运动数据输入给处理器；处理器，用于基于所述目标图像和所述运动数据确定所述终端设备的自定位轨迹；从所述目标场景的三维视觉地图中确定出与所述目标图像对应的目标地图点，基于所述目标地图点确定所述终端设备在所述三维视觉地图中的全局定位轨迹；基于所述自定位轨迹和所述全局定位轨迹生成所述终端设备在所述三维视觉地图中的融合定位轨迹，输出所述融合定位轨迹；所述融合定位轨迹包括的融合定位位姿的帧率大于全局定位轨迹包括的全局定位位姿的帧率。示例性的，所述终端设备为可穿戴设备，且所述视觉传感器和所述运动传感器部署在所述可穿戴设备上；或者，所述终端设备为记录仪，且所述视觉传感器和所述运动传感器部署在所述记录仪上；或者，所述终端设备为摄像机，且所述视觉传感器和所述运动传感器部署在所述摄像机上。

基于与上述方法同样的申请构思，本申请实施例还提供一种机器可读存储介质，所述机器可读存储介质上存储有若干计算机指令，所述计算机指令被处理器执行时，能够实现本申请上述示例公开的位姿确定方法。

其中，上述机器可读存储介质可以是任何电子、磁性、光学或其它物理存储装置，可以包含或存储信息，如可执行指令、数据，等等。例如，机器可读存储介质可以是：RAM(Radom Access Memory，随机存取存储器)、易失存储器、非易失性存储器、闪存、存储驱动器(如硬盘驱动器)、固态硬盘、任何类型的存储盘(如光盘、dvd等)，或者类似的存储介质，或者它们的组合。

上述实施例阐明的系统、装置、模块或单元，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。一种典型的实现设备为计算机，计算机的具体形式可以是个人计算机、膝上型计算机、蜂窝电话、相机电话、智能电话、个人数字助理、媒体播放器、导航设备、电子邮件收发设备、游戏控制台、平板计算机、可穿戴设备或者这些设备中的任意几种设备的组合。

为了描述的方便，描述以上装置时以功能分为各种单元分别描述。当然，在实施本申请时可以把各单元的功能在同一个或多个软件和/或硬件中实现。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请实施例可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可以由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其它可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其它可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可装载到计算机或其它可编程数据处理设备上，使得在计算机或者其它可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其它可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上所述仅为本申请的实施例而已，并不用于限制本申请。对于本领域技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本申请的权利要求范围之内。

Claims

1.一种位姿确定方法，其特征在于，应用于终端设备，所述终端设备包括目标场景的三维视觉地图，所述终端设备在目标场景的移动过程中，包括：

获取所述目标场景的目标图像和所述终端设备的运动数据；

2.根据权利要求1所述的方法，其特征在于，所述基于所述目标图像和所述运动数据确定所述终端设备的自定位轨迹，包括：

若所述目标图像包括多帧图像，则从多帧图像中遍历出当前帧图像；基于当前帧图像前面的K帧图像对应的自定位位姿、所述终端设备在自定位坐标系中的地图位置和所述运动数据确定当前帧图像对应的自定位位姿；基于多帧图像对应的自定位位姿生成所述终端设备在自定位坐标系中的自定位轨迹；

其中，若当前帧图像是关键图像，则基于所述终端设备的当前位置生成自定位坐标系中的地图位置；若当前帧图像与当前帧图像的前一帧图像之间的匹配特征点数量未达到预设阈值，则确定当前帧图像是关键图像。

3.根据权利要求1所述的方法，其特征在于，所述从所述三维视觉地图中确定出与所述目标图像对应的目标地图点，基于所述目标地图点确定所述终端设备在所述三维视觉地图中的全局定位轨迹，包括：

若所述目标图像包括多帧图像，从多帧图像中选取M帧图像作为待测图像；

针对每帧待测图像，基于所述待测图像与所述三维视觉地图对应的多帧样本图像之间的相似度，从所述多帧样本图像中选取出候选样本图像；

从所述待测图像中获取多个特征点；针对每个特征点，从所述候选样本图像对应的多个地图点中确定出与该特征点对应的目标地图点；

基于所述多个特征点和所述多个特征点对应的目标地图点确定所述待测图像对应的所述三维视觉地图中的全局定位位姿；基于M帧待测图像对应的全局定位位姿生成所述终端设备在所述三维视觉地图中的全局定位轨迹。

4.根据权利要求3所述的方法，其特征在于，

所述基于所述待测图像与所述三维视觉地图对应的多帧样本图像之间的相似度，从所述多帧样本图像中选取出候选样本图像，包括：

确定所述待测图像对应的待测全局描述子，确定所述待测全局描述子与所述三维视觉地图对应的每帧样本图像对应的样本全局描述子之间的距离；其中，所述三维视觉地图至少包括每帧样本图像对应的样本全局描述子；

基于所述待测全局描述子与每个样本全局描述子之间的距离，从所述多帧样本图像中选取出候选样本图像；其中，所述待测全局描述子与候选样本图像对应的样本全局描述子之间的距离为最小距离；或，所述待测全局描述子与候选样本图像对应的样本全局描述子之间的距离小于距离阈值。

5.根据权利要求4所述的方法，其特征在于，

所述确定所述待测图像对应的待测全局描述子，包括：

基于已训练的字典模型确定所述待测图像对应的词袋向量，并将所述词袋向量确定为所述待测图像对应的待测全局描述子；或者，

将所述待测图像输入给已训练的深度学习模型，得到所述待测图像对应的目标向量，将所述目标向量确定为所述待测图像对应的待测全局描述子。

6.根据权利要求3所述的方法，其特征在于，所述从所述候选样本图像对应的多个地图点中确定出与该特征点对应的目标地图点，包括：

确定该特征点对应的待测局部描述子，所述待测局部描述子用于表示该特征点所处图像块的特征向量，且所述图像块位于所述待测图像中；

确定所述待测局部描述子与所述候选样本图像对应的每个地图点对应的样本局部描述子之间的距离；其中，所述三维视觉地图至少包括所述候选样本图像对应的每个地图点对应的样本局部描述子；

基于所述待测局部描述子与每个样本局部描述子之间的距离，从所述多个地图点中选取目标地图点；所述待测局部描述子与目标地图点对应的样本局部描述子之间的距离为最小距离，且所述最小距离小于距离阈值。

7.根据权利要求1所述的方法，其特征在于，基于所述自定位轨迹和所述全局定位轨迹生成所述终端设备在所述三维视觉地图中的融合定位轨迹，包括：

从所述自定位轨迹包括的所有自定位位姿中选取出与目标时间段对应的N个自定位位姿，并从所述全局定位轨迹包括的所有全局定位位姿中选取出与所述目标时间段对应的P个全局定位位姿；其中，所述N大于所述P；

基于所述N个自定位位姿和所述P个全局定位位姿确定所述N个自定位位姿对应的N个融合定位位姿，N个自定位位姿与N个融合定位位姿一一对应；

基于N个融合定位位姿生成所述终端设备在三维视觉地图中的融合定位轨迹；其中，所述融合定位轨迹包括的融合定位位姿的帧率等于所述自定位轨迹包括的自定位位姿的帧率。

8.根据权利要求7所述的方法，其特征在于，

所述基于所述自定位轨迹和所述全局定位轨迹生成所述终端设备在所述三维视觉地图中的融合定位轨迹之后，所述方法还包括：

从所述融合定位轨迹中选取出初始融合定位位姿；

从所述自定位轨迹中选取与所述初始融合定位位姿对应的初始自定位位姿；

从所述自定位轨迹中选取出目标自定位位姿，并基于所述初始融合定位位姿、所述初始自定位位姿和所述目标自定位位姿确定目标融合定位位姿；

基于所述目标融合定位位姿和所述融合定位轨迹生成新的融合定位轨迹。

9.一种位姿确定装置，其特征在于，应用于终端设备，所述终端设备包括目标场景的三维视觉地图，在终端设备在目标场景的移动过程中，包括：

10.根据权利要求9所述的装置，其特征在于，

其中，所述确定模块基于所述目标图像和所述运动数据确定所述终端设备的自定位轨迹时具体用于：若所述目标图像包括多帧图像，则从多帧图像中遍历出当前帧图像；基于当前帧图像前面的K帧图像对应的自定位位姿、所述终端设备在自定位坐标系中的地图位置和所述运动数据确定当前帧图像对应的自定位位姿；基于多帧图像对应的自定位位姿生成所述终端设备在自定位坐标系中的自定位轨迹；其中，若当前帧图像是关键图像，则基于所述终端设备的当前位置生成自定位坐标系中的地图位置；若当前帧图像与当前帧图像的前一帧图像之间的匹配特征点数量未达到预设阈值，则确定当前帧图像是关键图像；

其中，所述确定模块从所述三维视觉地图中确定出与所述目标图像对应的目标地图点，基于所述目标地图点确定所述终端设备在所述三维视觉地图中的全局定位轨迹时具体用于：若目标图像包括多帧图像，从多帧图像中选取M帧图像作为待测图像；针对每帧待测图像，基于所述待测图像与三维视觉地图对应的多帧样本图像之间的相似度，从多帧样本图像中选取出候选样本图像；从待测图像中获取多个特征点；针对每个特征点，从候选样本图像对应的多个地图点中确定出与该特征点对应的目标地图点；基于所述多个特征点和所述多个特征点对应的目标地图点确定所述待测图像对应的三维视觉地图中的全局定位位姿；基于M帧待测图像对应的全局定位位姿生成所述终端设备在三维视觉地图中的全局定位轨迹；

其中，所述确定模块基于所述待测图像与三维视觉地图对应的多帧样本图像之间的相似度，从多帧样本图像中选取出候选样本图像时具体用于：确定待测图像对应的待测全局描述子，确定待测全局描述子与所述三维视觉地图对应的每帧样本图像对应的样本全局描述子之间的距离；其中，所述三维视觉地图包括每帧样本图像对应的样本全局描述子；基于所述待测全局描述子与每个样本全局描述子之间的距离，从所述多帧样本图像中选取出候选样本图像；所述待测全局描述子与候选样本图像对应的样本全局描述子之间的距离为最小距离；或，所述待测全局描述子与候选样本图像对应的样本全局描述子之间的距离小于距离阈值；

其中，所述确定模块确定待测图像对应的待测全局描述子时具体用于：基于已训练的字典模型确定所述待测图像对应的词袋向量，并将所述词袋向量确定为所述待测图像对应的待测全局描述子；或者，将所述待测图像输入给已训练的深度学习模型，得到所述待测图像对应的目标向量，并将所述目标向量确定为所述待测图像对应的待测全局描述子；

其中，所述确定模块从所述候选样本图像对应的多个地图点中确定出与该特征点对应的目标地图点时具体用于：确定该特征点对应的待测局部描述子，所述待测局部描述子用于表示该特征点所处图像块的特征向量，且所述图像块位于所述待测图像中；确定所述待测局部描述子与所述候选样本图像对应的每个地图点对应的样本局部描述子之间的距离；其中，所述三维视觉地图至少包括所述候选样本图像对应的每个地图点对应的样本局部描述子；基于所述待测局部描述子与每个样本局部描述子之间的距离，从所述多个地图点中选取目标地图点；所述待测局部描述子与目标地图点对应的样本局部描述子之间的距离为最小距离，且所述最小距离小于距离阈值；

其中，所述生成模块基于所述自定位轨迹和所述全局定位轨迹生成所述终端设备在所述三维视觉地图中的融合定位轨迹时具体用于：从所述自定位轨迹包括的所有自定位位姿中选取出与目标时间段对应的N个自定位位姿，从所述全局定位轨迹包括的所有全局定位位姿中选取出与所述目标时间段对应的P个全局定位位姿；所述N大于所述P；基于所述N个自定位位姿和所述P个全局定位位姿确定所述N个自定位位姿对应的N个融合定位位姿，所述N个自定位位姿与所述N个融合定位位姿一一对应；基于所述N个融合定位位姿生成所述终端设备在所述三维视觉地图中的融合定位轨迹；其中，所述融合定位轨迹包括的融合定位位姿的帧率等于所述自定位轨迹包括的自定位位姿的帧率；

其中，所述生成模块基于所述自定位轨迹和所述全局定位轨迹生成终端设备在三维视觉地图中的融合定位轨迹之后还用于：从所述融合定位轨迹中选取出初始融合定位位姿；从所述自定位轨迹中选取与所述初始融合定位位姿对应的初始自定位位姿；从所述自定位轨迹中选取出目标自定位位姿，并基于所述初始融合定位位姿、所述初始自定位位姿和所述目标自定位位姿确定目标融合定位位姿；基于所述目标融合定位位姿和所述融合定位轨迹生成新的融合定位轨迹。

11.一种终端设备，其特征在于，包括：处理器和机器可读存储介质，所述机器可读存储介质存储有能够被所述处理器执行的机器可执行指令；所述处理器用于执行机器可执行指令，以实现权利要求1-8任一项所述的方法步骤。

12.一种终端设备，其特征在于，包括：

处理器，用于基于所述目标图像和所述运动数据确定所述终端设备的自定位轨迹；从所述目标场景的三维视觉地图中确定出与所述目标图像对应的目标地图点，基于所述目标地图点确定所述终端设备在所述三维视觉地图中的全局定位轨迹；基于所述自定位轨迹和所述全局定位轨迹生成所述终端设备在所述三维视觉地图中的融合定位轨迹，并输出所述融合定位轨迹；其中，所述融合定位轨迹包括的融合定位位姿的帧率大于所述全局定位轨迹包括的全局定位位姿的帧率。

13.根据权利要求12所述的终端设备，其特征在于，

所述终端设备为可穿戴设备，且所述视觉传感器和所述运动传感器部署在所述可穿戴设备上；或者，所述终端设备为记录仪，且所述视觉传感器和所述运动传感器部署在所述记录仪上；或者，所述终端设备为摄像机，且所述视觉传感器和所述运动传感器部署在所述摄像机上。