CN113744308B - 位姿优化方法、装置、电子设备、介质及程序产品 - Google Patents

位姿优化方法、装置、电子设备、介质及程序产品 Download PDF

Info

Publication number
CN113744308B
CN113744308B CN202110903076.5A CN202110903076A CN113744308B CN 113744308 B CN113744308 B CN 113744308B CN 202110903076 A CN202110903076 A CN 202110903076A CN 113744308 B CN113744308 B CN 113744308B
Authority
CN
China
Prior art keywords
data
pose
key frame
pass
acquired data
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
CN202110903076.5A
Other languages
English (en)
Other versions
CN113744308A (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.)
Autonavi Software Co Ltd
Original Assignee
Autonavi Software 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 Autonavi Software Co Ltd filed Critical Autonavi Software Co Ltd
Priority to CN202110903076.5A priority Critical patent/CN113744308B/zh
Publication of CN113744308A publication Critical patent/CN113744308A/zh
Application granted granted Critical
Publication of CN113744308B publication Critical patent/CN113744308B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G06T7/248—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30241—Trajectory
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • Multimedia (AREA)
  • Remote Sensing (AREA)
  • Computer Graphics (AREA)
  • Image Analysis (AREA)

Abstract

本公开实施例公开了一种位姿优化方法、装置、电子设备、介质及程序产品，该方法包括：获取针对同一道路的两趟以上的采集数据，一趟采集数据包括：关键帧图像的特征点及其对应的初始位姿数据；先对不同趟采集数据进行分组，将同一个分组中的不同趟采集数据进行对齐，得到第一次优化位姿数据；对第一次优化位姿数据进行位姿图优化，得到第二次优化位姿数据，位姿图优化至少包括两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束；最后进行全局光束平差法优化，位姿数据的精度逐次升高。该技术方案能够提升整***姿的绝对精度和相对精度，恢复出更高精度的位姿数据，为高精地图要素重建提供良好的基础，以便重建精度更高的高精地图。

Description

位姿优化方法、装置、电子设备、介质及程序产品
技术领域
本公开实施例涉及高精地图技术领域，具体涉及一种位姿优化方法、装置、电子设备、介质及程序产品。
背景技术
近年来，自动驾驶或者智能驾驶技术研究成为热点和趋势，而自动驾驶或者智能驾驶的实现路径之一依赖于高精度地图（简称高精地图，亦可称为高清地图或者高标准地图）。高精地图可以为自动驾驶提供超视距感知，也是自动驾驶决策的重要依据。
但是由于现实世界是动态变化的，高精地图作为现实世界的数字化表达，需要不断更新以保证高精地图中的地理要素及其属性与现实世界保持一致。发明人发现，专业的高精地图采集车成本过高，无法大规模部署，因此，很难采用专业的高精地图采集车实现高精地图及时高效的更新。
为满足高精度地图的更新需求，低成本建图成为业界研究的热点。而准确的位姿数据是低成本建图获取高精度地图要素的关键。因此，如何提高位姿数据的准确度，是本领域技术人员需要持续解决和优化的问题。
发明内容
本公开实施例提供一种位姿优化方法、装置、电子设备、介质及程序产品。
第一方面，本公开实施例中提供了一种位姿优化方法。
具体的，所述位姿优化方法，包括：
获取针对同一道路的两趟以上的采集数据，一趟采集数据包括：关键帧图像的特征点和所述关键帧图像对应的初始位姿数据；
针对单趟采集数据，将所述单趟采集数据中的关键帧图像与其他趟采集数据中的关键帧图像进行特征点匹配，获得不同趟采集数据中的关键帧图像的匹配结果，所述匹配结果包括两个关键帧图像之间是否有共视区域的匹配结果；
基于所述不同趟采集数据中的关键帧图像的共视区域，对所述不同趟采集数据进行分组；
针对同一个分组中的不同趟采集数据，从中选择一趟采集数据，基于选择的一趟采集数据的初始位姿数据，将所述同一分组中的其他趟采集数据进行对齐，得到所述同一个分组中的关键帧图像的第一次优化位姿数据；
对所述关键帧图像的第一次优化位姿数据进行位姿图pose graph优化，得到所述关键帧图像的第二次优化位姿数据，所述位姿图优化至少包括两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束；
基于所述关键帧图像的特征点和所述关键帧图像的第二次优化位姿数据进行全局光束平差法BA优化，得到所述关键帧图像的三维特征点和第三次优化位姿数据，位姿数据的精度逐次升高。
结合第一方面，本公开实施例在第一方面的第一种实现方式中，其中，所述基于所述不同趟采集数据中的关键帧图像的共视区域，对所述不同趟采集数据进行分组，包括：
将所述不同趟采集数据中满足分组条件的各趟采集数据分为一组，其中，所述分组条件包括同一个分组中的每一趟采集数据有关键帧图像与其他一趟采集数据中的关键帧图像有共视区域。
结合第一方面和第一方面的第一种实现方式，本公开实施例在第一方面的第二种实现方式中，其中，所述针对同一个分组中的不同趟采集数据，从中选择一趟采集数据，基于选择的一趟采集数据的初始位姿数据，将所述同一分组中的其他趟采集数据进行对齐，得到所述同一个分组中的关键帧图像的第一次优化位姿数据，包括：
针对同一个分组中的不同趟采集数据，从中选择第一趟采集数据；
基于所述同一个分组中的第二趟采集数据与所述第一趟采集数据之间具有共视区域的各对共视关键帧图像的初始位姿数据，通过角度N点PnP，得到各对共视关键帧图像的第一帧间相对位姿信息；
对所述第二趟采集数据与所述第一趟采集数据之间的各对共视关键帧图像的第一帧间相对位姿信息进行统计，得所述第二趟采集数据与所述第一趟采集数据之间的第一帧间加权相对位姿信息；
基于所述第一帧间加权相对位姿信息，将所述第二趟采集数据向第一趟采集数据的初始位姿数据进行对齐，得到所述第二趟采集数据中的关键帧图像的第一次优化位姿数据；
基于具有共视区域的第三趟采集数据的关键帧图像的初始位姿数据与已对齐的一趟采集数据的关键帧图像的第一次优化位姿数据，通过PnP，得到各对共视关键帧图像的第二帧间相对位姿信息；
对第三趟采集数据与已对齐的一趟采集数据之间的各对共视关键帧图像的第二帧间相对位姿信息进行统计，得到所述第三趟采集数据与已对齐的一趟采集数据之间的第二帧间加权相对位姿信息；
基于所述第二帧间加权相对位姿信息，将所述第三趟采集数据向已对齐的一趟采集数据的第一次优化位姿数据进行对齐，得到所述第三趟采集数据中的关键帧图像的第一次优化位姿数据。
结合第一方面、第一方面的上述实现方式，本公开在第一方面的第三种实现方式中，其中，所述获取针对同一道路的两趟以上的采集数据，包括：
获取针对同一道路的两趟以上的图像数据和运动数据；
根据每趟的图像数据和运动数据，通过视觉惯性里程计VIO算法获取每趟的关键帧图像对应的初始位姿数据，所述每趟的关键帧图像的特征点在VIO算法的计算过程中获取。
结合第一方面、第一方面的上述实现方式，本公开在第一方面的第四种实现方式中，其中，所述对所述关键帧图像的第一次优化位姿数据进行位姿图pose graph优化，得到所述关键帧图像的第二次优化位姿数据，包括：
通过构建位姿先验约束、单趟采集数据的关键图像帧之间的帧间位姿约束和两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束，对所述关键帧图像的第一次优化位姿数据进行pose graph优化，得到所述关键帧图像的第二次优化位姿数据。
结合第一方面、第一方面的上述实现方式，本公开在第一方面的第五种实现方式中，其中，所述基于所述关键帧图像的特征点和所述关键帧图像的第二次优化位姿数据进行全局光束平差法BA优化，得到所述关键帧图像的三维特征点和第三次优化位姿数据，包括：
通过建立3D特征点重投影误差、单趟采集数据的关键图像帧之间的帧间位姿约束、位姿先验约束的因子图，对所述关键帧图像的特征点和所述关键帧图像的第二次优化位姿数据进行全局光束平差法BA优化，得到所述关键帧图像的三维特征点和第三次优化位姿数据。
第二方面，本公开实施例中提供了一种位姿优化装置。
具体的，所述位姿优化装置，包括：
获取模块，被配置为获取针对同一道路的两趟以上的采集数据，一趟采集数据包括：关键帧图像的特征点和所述关键帧图像对应的初始位姿数据；
匹配模块，被配置为针对单趟采集数据，将所述单趟采集数据中的关键帧图像与其他趟采集数据中的关键帧图像进行特征点匹配，获得不同趟采集数据中的关键帧图像的匹配结果，所述匹配结果包括两个关键帧图像之间是否有共视区域的匹配结果；
分组模块，被配置为基于所述不同趟采集数据中的关键帧图像的共视区域，对所述不同趟采集数据进行分组；
对齐模块，被配置为针对同一个分组中的不同趟采集数据，从中选择一趟采集数据，基于选择的一趟采集数据的初始位姿数据，将所述同一分组中的其他趟采集数据进行对齐，得到所述同一个分组中的关键帧图像的第一次优化位姿数据；
位姿图优化模块，被配置为对所述关键帧图像的第一次优化位姿数据进行位姿图pose graph优化，得到所述关键帧图像的第二次优化位姿数据，所述位姿图优化至少包括两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束；
全局优化模块，被配置为基于所述关键帧图像的特征点和所述关键帧图像的第二次优化位姿数据进行全局光束平差法BA优化，得到所述关键帧图像的三维特征点和第三次优化位姿数据，位姿数据的精度逐次升高。
结合第二方面，本公开实施例在第二方面的第一种实现方式中，其中，所述分组模块被配置为：
将所述不同趟采集数据中满足分组条件的各趟采集数据分为一组，其中，所述分组条件包括同一个分组中的每一趟采集数据有关键帧图像与其他一趟采集数据中的关键帧图像有共视区域。
结合第二方面和第二方面的第一种实现方式，本公开实施例在第二方面的第二种实现方式中，其中，所述对齐模块被配置为
针对同一个分组中的不同趟采集数据，从中选择第一趟采集数据；
基于所述同一个分组中的第二趟采集数据与所述第一趟采集数据之间具有共视区域的各对共视关键帧图像的初始位姿数据，通过角度N点PnP，得到各对共视关键帧图像的第一帧间相对位姿信息；
对所述第二趟采集数据与所述第一趟采集数据之间的各对共视关键帧图像的第一帧间相对位姿信息进行统计，得所述第二趟采集数据与所述第一趟采集数据之间的第一帧间加权相对位姿信息；
基于所述第一帧间加权相对位姿信息，将所述第二趟采集数据向第一趟采集数据的初始位姿数据进行对齐，得到所述第二趟采集数据中的关键帧图像的第一次优化位姿数据；
基于具有共视区域的第三趟采集数据的关键帧图像的初始位姿数据与已对齐的一趟采集数据的关键帧图像的第一次优化位姿数据，通过PnP，得到各对共视关键帧图像的第二帧间相对位姿信息；
对第三趟采集数据与已对齐的一趟采集数据之间的各对共视关键帧图像的第二帧间相对位姿信息进行统计，得到所述第三趟采集数据与已对齐的一趟采集数据之间的第二帧间加权相对位姿信息；
基于所述第二帧间加权相对位姿信息，将所述第三趟采集数据向已对齐的一趟采集数据的第一次优化位姿数据进行对齐，得到所述第三趟采集数据中的关键帧图像的第一次优化位姿数据。
结合第二方面、第二方面的上述实现方式，本公开在第二方面的第三种实现方式中，其中，所述获取模块被配置为：
获取针对同一道路的两趟以上的图像数据和运动数据；
根据每趟的图像数据和运动数据，通过视觉惯性里程计VIO算法获取每趟的关键帧图像对应的初始位姿数据，所述每趟的关键帧图像的特征点在VIO算法的计算过程中获取。
结合第二方面、第二方面的上述实现方式，本公开在第一方面的第四种实现方式中，其中，所述位姿图优化模块被配置为：
通过构建位姿先验约束、单趟采集数据的关键图像帧之间的帧间位姿约束和两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束，对所述关键帧图像的第一次优化位姿数据进行pose graph优化，得到所述关键帧图像的第二次优化位姿数据。
结合第二方面、第二方面的上述实现方式，本公开在第一方面的第五种实现方式中，其中，所述全局优化模块被配置为：
通过建立3D特征点重投影误差、单趟采集数据的关键图像帧之间的帧间位姿约束、位姿先验约束的因子图，对所述关键帧图像的特征点和所述关键帧图像的第二次优化位姿数据进行全局光束平差法BA优化，得到所述关键帧图像的三维特征点和第三次优化位姿数据。
第三方面，本公开实施例提供了一种电子设备，包括存储器和处理器，所述存储器用于存储一条或多条支持位姿优化装置执行上述位姿优化方法的计算机指令，所述处理器被配置为用于执行所述存储器中存储的计算机指令。所述位姿优化装置还可以包括通信接口，用于位姿优化装置与其他设备或通信网络通信。
第四方面，本公开实施例提供了一种计算机可读存储介质，用于存储位姿优化装置所用的计算机指令，其包含用于执行上述位姿优化方法为位姿优化装置所涉及的计算机指令。
第五方面，本公开实施例提供了一种计算机程序产品，包括计算机程序/指令，其中，该计算机程序/指令被处理器执行时实现上述位姿优化方法的方法步骤。
第六方面，本公开实施例提供了一种导航方法，其中，基于高精地图获取至少基于起点、终点及路况计算的导航路线，基于所述导航路线进行导航引导，所述高精地图是基于上述任意一项方法中获取的三维特征点和第三次优化位姿数据进行地图重建实现的。
本公开实施例提供的技术方案可包括以下有益效果：
上述技术方案基于各趟采集数据间的关键帧图像之间的帧间相对位姿关系和单趟采集数据的关键图像帧之间的帧间相对位姿关系，对各趟的初始位姿数据进行趟间位姿对齐、位姿图优化和全局BA优化这三次位姿优化，使得位姿数据的精度逐次升高。该技术方案能够提升整***姿的绝对精度和相对精度，恢复出更高精度的位姿数据，为高精度地图要素重建提供良好的基础，以便重建精度更高的高精地图。
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本公开实施例。
附图说明
结合附图，通过以下非限制性实施方式的详细描述，本公开实施例的其它特征、目的和优点将变得更加明显。以下是对附图的说明。
图1示出根据本公开一实施方式的位姿优化方法的流程图。
图2示出根据本公开一实施方式的两趟间位姿刚性对齐的过程示意图。
图3示出根据本公开一实施方式的3D特征点重投影的示意图。
图4示出根据本公开一实施方式的位姿优化装置的结构框图。
图5示出根据本公开一实施方式的电子设备的结构框图。
图6是适于用来实现根据本公开一实施方式的位姿优化方法的计算机***的结构示意图。
具体实施方式
下文中，将参考附图详细描述本公开实施例的示例性实施方式，以使本领域技术人员可容易地实现它们。此外，为了清楚起见，在附图中省略了与描述示例性实施方式无关的部分。
在本公开实施例中，应理解，诸如“包括”或“具有”等的术语旨在指示本说明书中所公开的特征、数字、步骤、行为、部件、部分或其组合的存在，并且不欲排除一个或多个其他特征、数字、步骤、行为、部件、部分或其组合存在或被添加的可能性。
另外还需要说明的是，在不冲突的情况下，本公开中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本公开实施例。
本公开实施例提供的技术方案基于各趟采集数据间的关键帧图像之间的帧间相对位姿关系和单趟采集数据的关键图像帧之间的帧间相对位姿关系，对各趟的初始位姿数据进行趟间位姿对齐、位姿图优化和全局BA优化这三次位姿优化，使得位姿数据的精度逐次升高。该技术方案能够提升整***姿的绝对精度和相对精度，恢复出更高精度的位姿数据，为高精度地图要素重建提供良好的基础，以便重建精度更高的高精地图。
图1示出根据本公开一实施方式的位姿优化方法的流程图，如图1所示，所述位姿优化方法包括以下步骤S101-S106：
在步骤S101中，获取针对同一道路的两趟以上的采集数据，一趟采集数据包括：关键帧图像的特征点和所述关键帧图像对应的初始位姿数据；
在步骤S102中，针对单趟采集数据，将所述单趟采集数据中的关键帧图像与其他趟采集数据中的关键帧图像进行特征点匹配，获得不同趟采集数据中的关键帧图像的匹配结果，所述匹配结果包括两个关键帧图像之间是否有共视区域的匹配结果；
在步骤S103中，基于所述不同趟采集数据中的关键帧图像的共视区域，对所述不同趟采集数据进行分组；
在步骤S104中，针对同一个分组中的不同趟采集数据，从中选择一趟采集数据，基于选择的一趟采集数据的初始位姿数据，将所述同一分组中的其他趟采集数据进行对齐，得到所述同一个分组中的关键帧图像的第一次优化位姿数据；
在步骤S105中，对所述关键帧图像的第一次优化位姿数据进行位姿图pose graph优化，得到所述关键帧图像的第二次优化位姿数据，所述位姿图优化至少包括两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束；
在步骤S106中，基于所述关键帧图像的特征点和所述关键帧图像的第二次优化位姿数据进行全局光束平差法BA优化，得到所述关键帧图像的三维特征点和第三次优化位姿数据，位姿数据的精度逐次升高。
上文提及，近年来，自动驾驶或者智能驾驶技术研究成为热点和趋势，而自动驾驶或者智能驾驶的实现路径之一依赖于高精度地图（简称高精地图，亦可称为高清地图或者高标准地图）。高精地图可以为自动驾驶提供超视距感知，也是自动驾驶决策的重要依据。但是由于现实世界是动态变化的，高精地图作为现实世界的数字化表达，需要不断更新以保证高精地图中的地理要素及其属性与现实世界保持一致。发明人发现，专业的高精地图采集车成本过高，无法大规模部署，因此，很难采用专业的高精地图采集车实现高精地图及时高效的更新。为满足高精地图的更新需求，低成本建图成为业界研究的热点。而准确的位姿数据是低成本建图获取高精度地图要素的关键。因此，如何提高位姿数据的准确度，是本领域技术人员需要持续解决和优化的问题。
考虑到上述问题，在该实施方式中，提出一种位姿优化方法，该方法基于各趟采集数据间的关键帧图像之间的帧间相对位姿关系和单趟采集数据的关键图像帧之间的帧间相对位姿关系，对各趟的初始位姿数据进行趟间位姿对齐、位姿图优化和全局BA优化这三次位姿优化，使得位姿数据的精度逐次升高。该技术方案能够提升整***姿的绝对精度和相对精度，恢复出更高精度的位姿数据，为高精度地图要素重建提供良好的基础，以便重建精度更高的高精地图。
在本公开一实施方式中，所述位姿优化方法可适用于可执行位姿优化的计算机、计算设备、电子设备、服务器、服务集群等。
在本公开一实施方式中，在构建某一道路的高精地图时，需要获取针对同一道路的两趟以上的采集数据，对这两趟以上的采集数据进行优化，得到更加精确的每趟的关键帧图像的特征点和所述关键帧图像对应的位姿数据，然后就能采用SFM（Structure FromMotion 运动恢复结构）恢复该道路所在环境的空间三维信息，构建高精地图。
在本公开一实施方式中，该关键帧图像是从采集的一序列有时序的图像数据中选择一些图像的作为关键帧图像，该关键帧图像的选择规则可以是关键帧图像不能太近也不能太远，需要在一定的间隔范围内选择关键帧图像。
在本公开一实施方式中，该关键帧图像的特征点可以用来标识该关键帧图像上一些目标物体，一般是将图像上灰度值发生剧烈变化的点或在图像边缘上曲率较大的点（如两个边缘的交点）看作为图像的特征点。该特征点包含关键点 (Key-points) 和描述子(Descriptor) 两部分，关键点表达了特征点的三维位置，而描述子则是对于特征点视觉特性的描述，大多为向量形式。
在本公开一实施方式中，位姿(pose)数据为采集该关键帧图像的采集设备的位置和姿态(朝向)信息，例如：在三维中一般是 (x，y，z，yaw，pitch，roll)，前三个元素描述物体的三维位置，后三个元素描述物体的姿态。其中yaw是航向角，绕Z轴旋转，pitch是俯仰角，绕 Y轴旋转，roll是翻滚角，绕X轴旋转。根据每趟采集数据中的关键帧图像进行相机标定(相机标定指的是利用相机所拍摄到的关键帧图像来还原空间中的物体)，根据相机标定的结果及采集设备的运动数据，可以确定拍摄到关键帧图像时该采集设备的位姿数据，即关键帧图像对应的初始位姿数据。
在本公开一实施方式中，各趟采集数据的关键帧图像的特征点用于标识关键帧图像中的目标物，进行特征点匹配指的是计算两个特征点的描述子的相似度（即计算两个特征点的描述子在向量空间上的距离），当相似度超过预设阈值如99%时，则这两个特征点是匹配的，可以记为共同特征点。如果两个关键帧图像之间的共同特征点个数超过预设个数，则两个关键帧图像之间的匹配结果是具有共视区域，如果两个关键帧图像之间的共同特征点个数未超过预设个数，则两个关键帧图像之间的匹配结果是不具有共视区域。共视区域是指两趟采集数据的关键帧图像显示有共同的目标物（可能视角不同），两个关键帧图像之间具有共视区域就表明两趟采集设备在采集这两个关键帧图像时采用不同的位姿拍摄到了同一个目标物。
在本公开一实施方式中，可以针对任一单趟采集数据，将单趟采集数据中的关键帧图像与其他趟采集数据中的关键帧图像均进行特征点匹配，得到任意两趟采集数据之间的任意两个关键帧图像之间的匹配结果。优选的，如果两个关键帧图像的采集位置之间的距离较远，则这两个关键帧图像之间就不会存在共视区域，故为了减少计算量，在进行特征点匹配时，可以针对单趟采集数据中的一关键帧图像，仅从其他趟采集数据中筛选出采集位置与该一关键帧图像的采集位置在一定距离范围内的关键帧图像进行特征点匹配，获取该一关键帧图像与对应筛选出的关键帧图像之间的匹配结果。
在本公开一实施方式中，基于所述不同趟采集数据中的关键帧图像的共视区域，可以将满足分组条件的各趟采集数据分为一组，其中，所述分组条件包括同一个分组中的每一趟采集数据有关键帧图像与其他趟采集数据中的关键帧图像有共视区域。
在本公开一实施方式中，对该关键帧图像对应的初始位姿数据的第一次位姿优化是基于同一分组中选择的一趟采集数据的初始位姿数据，将同一分组中的其他趟采集数据进行刚性对齐，该刚性对齐指的是每趟采集数据的各关键帧图像对应的初始位姿数据都变换同一位移和朝向。示例的，如图2所示，同一分组中包括两趟采集数据S1和S2，该S1的初始位姿数据形成的位姿轨迹和S2的初始位姿数据形成的位姿轨迹之间是有偏移的，将该S2的各初始位姿信息中的位置和朝向按照同一个标准参数移动，使其与该S1的位姿数据形成的位姿轨迹刚性对齐，得到S2的第一次优化位姿数据，该S1的第一次优化位姿数据为该S1的初始位姿数据，如此得到对齐后的S1和S2。
在本公开一实施方式中，两个关键帧图像具有共视区域，则表明两个关键帧图像中显示有同一物体只是显示视角不同，故可以据此获取两个关键帧图像的相对位姿关系，如此根据同一分组中两趟采集数据之间的具有共视区域的关键帧图像，就可以统计得到两趟采集数据之间的相对位姿关系，可以按照该两趟采集数据之间的相对位姿关系将这两趟采集数据的初始位姿数据进行刚性对齐，得到两趟采集数据的关键帧图像的第一次优化位姿数据；然后，由于在同一个分组中必然有一趟或多趟采集数据与这两趟采集数据中某一趟采集数据之间的关键帧图像具有共视区域，则可以统计获取其他一趟采集数据与这两趟采集数据中某一趟采集数据之间的相对位姿关系，并据此相对位姿关系将该其他一趟采集数据向该某一趟采集数据的初始位姿数据进行刚性对齐，如此，按照上述方法将该同一个分组中的不同趟采集数据均进行刚性对齐后，就得到了所述同一个分组中的关键帧图像的第一次优化位姿数据。
这里需要说明的是，各趟采集数据间由于采集时间不连续，采集设备不同，得到的各趟采集数据中的位姿轨迹都存在一定的偏移，将各趟采集数据中的初始位姿数据进行刚性对齐，可以为后续的位姿图优化能提供好的初始值，避免出现局部最优的情况。
在本公开一实施方式中，pose graph（位姿图）优化利用短间隔可靠的相对位姿测量来构建一个涵盖长时间跨度的关键帧图像们的全局优化问题，平摊累积误差，以各关键帧图像对应的位姿数据为节点，以关键帧图像间相对位姿计算结果作为测量进行极大似然估计。pose graph优化的优化变量是位姿，需要构建一个只有位姿轨迹的图优化，而位姿节点之间的边，由两个关键帧图像之间通过特征匹配后得到的运动估计来给定初始值，一旦初始值完成，就不再优化特征点的位置，只关心采集设备位姿之间的联系，可以基于单趟采集数据的关键图像帧之间的帧间相对位姿约束和任意两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束，对各趟关键帧图像的第一次优化位姿数据进行第二次位姿优化，得到关键帧图像的第二次优化位姿数据，该第二次优化位姿数据恢复了每趟关键帧图像的位姿数据的绝对精度同时保证每趟关键帧图像之间位姿数据的相对精度。第二次优化位姿数据的精度大于第一次优化位姿数据的精度。
在本公开一实施方式中，BA（Bundle Adjustment，光束平差法）优化不仅优化位姿还优化特征点，BA优化包括重投影误差（reprojection error）的优化；根据特征点的三维位置坐标、位姿数据，把三维的特征点投影到图像平面中，然后我们来算它跟图像中，通过特征提取出来的那个点之间的误差，BA优化就是要最小化这个误差。该BA优化可以调整第二次优化位姿数据和特征点使得所观测的图像位置和预测的图像位置之间的误差最小，如此得到关键帧图像的三维特征点和第三次优化位姿数据，第三次优化位姿数据的精度大于第二次优化位姿数据的精度。
在上述实施方式中，首先获取针对同一道路的两趟以上的采集数据，一趟采集数据包括：关键帧图像的特征点和所述关键帧图像对应的初始位姿数据，针对单趟采集数据，将所述单趟采集数据中的关键帧图像与其他趟采集数据中的关键帧图像进行特征点匹配，获得不同趟采集数据中的关键帧图像的匹配结果，所述匹配结果包括两个关键帧图像之间是否有共视区域的匹配结果；基于所述不同趟采集数据中的关键帧图像的共视区域，对所述不同趟采集数据进行分组；然后，针对同一个分组中的不同趟采集数据，从中选择一趟采集数据，基于选择的一趟采集数据的初始位姿数据，将所述同一分组中的其他趟采集数据进行对齐，得到所述同一个分组中的关键帧图像的第一次优化位姿数据；对齐后的第一次优化位姿数据为pose graph提供了好的初始值，对每趟的关键帧图像的第一次优化位姿数据进行pose graph恢复了每趟关键帧图像的位姿数据的绝对精度同时保证每趟关键帧图像之间位姿数据的相对精度，能为后续的BA优化提供更好的初值；最后对关键帧图像的特征点和所述关键帧图像的第二次优化位姿数据进行全局BA优化，可以获取到高精度的关键帧图像的三维特征点和第三次优化位姿数据，为后续构建高精度地图提供良好的基础，以便重建精度更高的高精地图。另外，本实施例在进行位姿优化时只利用到图像的特征点，使用数据更加精简，而且本方法不需要采集太多趟的采集数据（示例的可以仅使用两趟采集数据），就能恢复更高精度的位姿数据。
在本公开一实施方式中，步骤S103，即基于所述不同趟采集数据中的关键帧图像的共视区域，对所述不同趟采集数据进行分组的步骤，可包括以下步骤：
将所述不同趟采集数据中满足分组条件的各趟采集数据分为一组，其中，所述分组条件包括同一个分组中的每一趟采集数据有关键帧图像与其他趟采集数据中的关键帧图像有共视区域。
在该实施方式中，同一个分组中的每一趟采集数据必然与其他某一趟或多趟采集数据中的关键帧图像有共视区域，如果是不同分组，则同一组中的任一趟采集数据中均没有关键帧图像与另一组中的各趟采集数据的关键帧图像有共视区域。
在该实施方式中，假设共获取到6趟采集数据S1、S2、S3、S4、S5和S6，其中，S1和S2之间有关键帧图像具有共视区域；S3和S2之间有关键帧图像具有共视区域，但是S3和S1之间没有关键帧图像具有共视区域；S4和S3之间有关键帧图像具有共视区域，但是S4和S1、S2之间没有关键帧图像具有共视区域，S5和S6之间有关键帧图像具有共视区域，但是S5与S1至S4之间均没有关键帧图像具有共视区域，S6与S1至S4之间也均没有关键帧图像具有共视区域，则可以将S1、S2、S3和S4分为第一组，将S5和S6分为第二组。在该第一组中每一趟采集数据必然有一个或多个关键帧图像与其他某一趟或多趟采集数据中的关键帧图像有共视区域，如S1的某关键帧图像与S2的某关键帧图像之间有共视区域，S2的某关键帧图像与S1或S3的某关键帧图像之间有共视区域，S3的某关键帧图像与S2或S4的某关键帧图像之间有共视区域，S4的某关键帧图像与S3的某关键帧图像之间有共视区域。
在本公开一实施方式中，步骤S104，即针对同一个分组中的不同趟采集数据，从中选择一趟采集数据，基于选择的一趟采集数据的初始位姿数据，将所述同一分组中的其他趟采集数据进行对齐，得到所述同一个分组中的关键帧图像的第一次优化位姿数据，可包括以下步骤：
在步骤A1中，针对同一个分组中的不同趟采集数据，从中选择第一趟采集数据；
在步骤A2中，基于所述同一个分组中的第二趟采集数据与所述第一趟采集数据之间具有共视区域的各对共视关键帧图像的初始位姿数据，通过角度N点PnP，得到各对共视关键帧图像的第一帧间相对位姿信息；
在步骤A3中，对所述第二趟采集数据与所述第一趟采集数据之间的各对共视关键帧图像的第一帧间相对位姿信息进行统计，得所述第二趟采集数据与所述第一趟采集数据之间的第一帧间加权相对位姿信息；
在步骤A4中，基于所述第一帧间加权相对位姿信息，将所述第二趟采集数据向第一趟采集数据的初始位姿数据进行对齐，得到所述第二趟采集数据中的关键帧图像的第一次优化位姿数据；
在步骤A5中，基于具有共视区域的第三趟采集数据的关键帧图像的初始位姿数据与已对齐的一趟采集数据的关键帧图像的第一次优化位姿数据，通过PnP，得到各对共视关键帧图像的第二帧间相对位姿信息；
在步骤A6中，对第三趟采集数据与已对齐的一趟采集数据之间的各对共视关键帧图像的第二帧间相对位姿信息进行统计，得到所述第三趟采集数据与已对齐的一趟采集数据之间的第二帧间加权相对位姿信息；
在步骤A7中，基于所述第二帧间加权相对位姿信息，将所述第三趟采集数据向已对齐的一趟采集数据的第一次优化位姿数据进行对齐，得到所述第三趟采集数据中的关键帧图像的第一次优化位姿数据。
在该实施例方式中，可以从该同一个分组中的不同趟采集数据中随机选择一趟采集数据作为第一趟采集数据，该第二趟采集数据为该同一个分组中与该第一趟采集数据具有共视区域的任意一趟采集数据，可以通过角度N点PnP，基于第二趟采集数据与第一趟采集数据之间具有共视区域的各对共视关键帧图像的初始位姿数据，得到各对共视关键帧图像的第一帧间相对位姿信息，进而统计得到第二趟采集数据与所述第一趟采集数据之间的第一帧间加权相对位姿信息，该统计方法可以是将各第一帧间相对位姿信息进行加权平均。将所述第二趟采集数据的初始位姿数据均按照该第一帧间加权相对位姿信息进行变换，如此向第一趟采集数据的初始位姿数据进行对齐，得到所述第二趟采集数据中的关键帧图像的第一次优化位姿数据，该第一趟采集数据中的关键帧图像的第一次优化位姿数据为该第一趟采集数据的初始位姿数据。此时，已对齐的采集数据就是第一趟采集数据和第二趟采集数据。
在该实施方式中，该第三趟采集数据为该同一个分组中与该已对齐的采集数据中的任一趟采集数据具有共视区域的一趟采集数据。可以基于具有共视区域的第三趟采集数据的关键帧图像的初始位姿数据与已对齐的一趟采集数据的关键帧图像的第一次优化位姿数据，通过PnP，得到第三趟采集数据与该已对齐的一趟采集数据之间的各对共视关键帧图像的第二帧间相对位姿信息；对这些第二帧间相对位姿信息进行加权统计，得到所述第三趟采集数据与已对齐的一趟采集数据之间的第二帧间加权相对位姿信息；将所述第三趟采集数据的初始位姿数据均按照该第二帧间加权相对位姿信息进行变换，如此向已对齐的一趟采集数据的第一次优化位姿数据进行对齐，得到所述第三趟采集数据中的关键帧图像的第一次优化位姿数据。此时，已对齐的采集数据就是第一趟采集数据、第二趟采集数据和第三趟采集数据，同一分组中的其他剩余趟的采集数据按照步骤A5至A7重复进行，直至将同一分组中的各趟的采集数据均对齐。
这里需要说明的是，根据一对共视关键帧各自的共同特征点和位姿数据，使用PnP算法可以估计出这一对共视关键帧的帧间相对位姿信息。
示例的，假设共获取到4趟采集数据S1、S2、S3、S4，其中，S1和S2之间有关键帧图像具有共视区域；S3和S2之间有关键帧图像具有共视区域，但是S3和S1之间没有关键帧图像具有共视区域；S4和S3之间有关键帧图像具有共视区域，但是S4和S1、S2之间没有关键帧图像具有共视区域，则可以将S1、S2、S3和S4分为同一分组。可以从该同一个分组中的随机选择S2作为第一趟采集数据，该第二趟采集数据可以为该同一个分组中与该S2具有共视区域的S1和S3中的一趟如S1，可以通过角度N点PnP算法，基于S1与S2之间具有共视区域的各对共视关键帧图像的初始位姿数据，得到各对共视关键帧图像的第一帧间相对位姿信息，进而统计得到S1与S2之间的第一帧间加权相对位姿信息。将S1的初始位姿数据均按照该第一帧间加权相对位姿信息进行变换，如此向S2的初始位姿数据进行对齐，得到所述S1中关键帧图像的第一次优化位姿数据，该S2中关键帧图像的第一次优化位姿数据为该S2的初始位姿数据。此时，已对齐的采集数据就是S1和S2。该第三趟采集数据为该同一个分组中与S1和S2中S2具有共视区域的一趟采集数据S3。可以基于具有共视区域的S3的关键帧图像的初始位姿数据与S2的关键帧图像的第一次优化位姿数据，通过PnP算法，得到S2与S3之间的各对共视关键帧图像的第二帧间相对位姿信息；对这些第二帧间相对位姿信息进行加权统计，得到S2与S3之间的第二帧间加权相对位姿信息；将S3的初始位姿数据均按照该第二帧间加权相对位姿信息进行变换，如此向S2的第一次优化位姿数据进行对齐，得到S3中的关键帧图像的第一次优化位姿数据。此时，已对齐的采集数据就是S1、S2和S3。其他剩余趟的采集数据S4按照步骤A5至A7重复进行如下：该S4与S3之间有共视区域，则可以基于具有共视区域的S4的关键帧图像的初始位姿数据与已对齐的一趟采集数据S3的关键帧图像的第一次优化位姿数据，通过PnP算法，得到S4与S3之间的各对共视关键帧图像的第二帧间相对位姿信息；对这些第二帧间相对位姿信息进行加权统计，得到S4与S3之间的第二帧间加权相对位姿信息；将S4的初始位姿数据均按照该第二帧间加权相对位姿信息进行变换，如此向S3的第一次优化位姿数据进行对齐，得到所述S4中的关键帧图像的第一次优化位姿数据，如此就将同一分组中的S1至S4的采集数据均对齐。
这里需要说明的是，每趟采集数据与同一分组中N趟采集数据具有共视区域，每趟采集数据对应的N值不同，所述第一趟采集数据就是N值最大的一趟采集数据。若同一分组中其他趟采集数据均与某一趟采集数据具有共视区域，则可以选择该某一趟采集数据为第一趟采集数据，按照上述方法分别基于其他趟采集数据与第一趟采集数据之间具有共视区域的各对共视关键帧图像，通过角度N点PnP，得到各对共视关键帧图像的帧间相对位姿信息，然后对其他趟采集数据与所述第一趟采集数据之间的各对共视关键帧图像的帧间相对位姿信息进行统计，得所述其他趟采集数据与所述第一趟采集数据之间的帧间加权相对位姿信息，最后基于该帧间加权相对位姿信息，将其他趟采集数据向第一趟采集数据的初始位姿数据进行对齐，得到所述其他趟采集数据中的关键帧图像的第一次优化位姿数据。
在本公开一实施方式中，所述步骤S101，即所述获取针对同一道路的两趟以上的采集数据的步骤，可包括以下步骤：
获取针对同一道路的两趟以上的图像数据和运动数据；
根据每趟的图像数据和运动数据，通过视觉惯性里程计VIO算法获取每趟的关键帧图像对应的初始位姿数据，每趟的关键帧图像的特征点在VIO算法的计算过程中获取。
在该实施方式中，通过在采集设备上安装传感器 (比如，相机、IMU（InertialMeasurement Unit，惯性测量单元）、RTK（Real Time Kinematic，实时动态测量）等)，以采集该采集设备在运动过程的图像数据和运动数据，该图像数据是相机在每趟采集的运动过程中采集到的一序列图像，该运动数据包括相机在每趟采集的运动过程中的RTK数据，或者RTK数据和IMU数据等，该IMU是主要用来检测和测量加速度与旋转运动的传感器，该IMU通过集成多个惯性传感器以生成沿多个轴或自由度的加速度和角速度测量值，该IMU数据就是该IMU实时测量到该采集设备的加速度和角速度测量值。该RTK是以载波相位观测为根据的实时差分GPS技术，可以实时得到厘米级定位精度的三维位姿数据，该RTK数据就是采集设备的实时三维位姿数据。
在该实施方式中，为了实现低成本建图，针对同一道路的两趟以上的图像数据和运动数据可以通过众包资料获取，该众包资料是各种采集设备如车载相机、手机相机、数码相机或者摄像机等拍摄获取的。需要了解的是，众包资料是指由大众经过一定方法(如使用车载相机拍摄)获取后通过互联网向公众或相关机构提供的一种开放式资料数据。大众可以自愿上传众包资料或者通过参与相关机构下发的众包任务的方式提供众包资料。该众包资料里就包括大众进行每趟采集时获取的图像数据和运动数据，一趟采集数据是指基于连续时间内的一次采集获取的图像数据和运动数据得到的关键帧图像的特征点和关键帧图像对应的初始位姿数据，每趟采集数据是有时序性的。在需要对某条道路进行众包建图时，可以从众包资料里获取针对该道路的两趟以上的采集数据。
在该实施方式中，VIO（Visual Inertial Odometry，视觉里程计）算法在计算过程中会先提取关键帧图像中的特征点，然后计算关键帧图像对应的初始位姿数据，故可以在VIO计算的过程中获取每趟的关键帧图像的特征点，而不需额外再进行特征点提取，减少计算步骤。
在本公开一实施方式中，所述步骤S105，即对所述关键帧图像的第一次优化位姿数据进行位姿图pose graph优化，得到所述关键帧图像的第二次优化位姿数据的步骤，可包括以下步骤：
通过构建位姿先验约束、单趟采集数据的关键图像帧之间的帧间位姿约束和两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束，对所述关键帧图像的第一次优化位姿数据进行pose graph优化，得到所述关键帧图像的第二次优化位姿数据。
在该实施方式中，在应用位姿图优化来优化该第一次优化位姿数据时，要在位姿先验约束、单趟采集数据的关键图像帧之间的帧间位姿约束和两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束下来进行优化调整，该位姿先验约束用于约束对该关键帧图像对应的第一次优化位姿数据的调整只能在一定范围，该单趟采集数据的关键图像帧之间的帧间位姿约束是指在调整某关键帧图像对应的第一次优化位姿数据时，该关键帧图像与其所在趟内的有共视区域的关键帧图像之间的帧间相对位姿要满足一定的约束条件，不能超过根据这些关键帧图像的初始位姿数据计算的帧间相对位姿太多；该两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束是指在调整关键帧图像对应的第一次优化位姿数据时，该关键帧图像与其他趟内的有共视区域的关键帧图像之间的帧间相对位姿也要满足一定的约束条件，不能超过根据这些关键帧图像的初始位姿数据计算的帧间相对位姿太多。这样就可以恢复每趟关键帧图像的位姿数据的绝对精度同时保证各趟关键帧图像之间位姿数据的相对精度。
在本公开一实施方式中，所述步骤S106，即基于所述关键帧图像的特征点和所述关键帧图像的第二次优化位姿数据进行全局光束平差法BA优化，得到所述关键帧图像的三维特征点和第三次优化位姿数据的步骤，可包括以下步骤：
通过建立3D特征点重投影误差、单趟采集数据的关键图像帧之间的帧间位姿约束、位姿先验约束的因子图，对所述关键帧图像的特征点和所述关键帧图像的第二次优化位姿数据进行全局光束平差法BA优化，得到所述关键帧图像的三维特征点和第三次优化位姿数据。
在该实施方式中，3D特征点重投影误差是指根据特征点的三维位置坐标、采集设备的位姿数据，把特征点投影到二维图像平面中，然后来算它跟二维图像中通过特征提取出来的那个点之间的距离，在进行BA优化时要最小化该距离。示例的，图3示出根据本公开一实施方式的3D特征点重投影的示意图，如图3所示，特征点P1、P2、P3投影到位姿分别是T1、T2、T3的关键帧图像上，将该投影位置和该关键帧图像上提取P1、P2、P3的实际位置进行比对，计算两者之间的距离，调整特征点和位姿数据，将这个距离最小化。
在该实施方式中，在最小化重投影误差时，还会考虑单趟采集数据的关键图像帧之间的帧间位姿约束、位姿先验约束，如此调整特征点和第二次优化位姿数据进行全局BA优化，得到三维特征点和第三次优化位姿数据。
下述为本公开装置实施例，可以用于执行本公开方法实施例。
图4示出根据本公开一实施方式的位姿优化装置的结构框图，该装置可以通过软件、硬件或者两者的结合实现成为电子设备的部分或者全部。如图4所示，所述位姿优化装置包括：
获取模块401，被配置为获取针对同一道路的两趟以上的采集数据，一趟采集数据包括：关键帧图像的特征点和所述关键帧图像对应的初始位姿数据；
匹配模块402，被配置为针对单趟采集数据，将所述单趟采集数据中的关键帧图像与其他趟采集数据中的关键帧图像进行特征点匹配，获得不同趟采集数据中的关键帧图像的匹配结果，所述匹配结果包括两个关键帧图像之间是否有共视区域的匹配结果；
分组模块403，被配置为基于所述不同趟采集数据中的关键帧图像的共视区域，对所述不同趟采集数据进行分组；
对齐模块404，被配置为针对同一个分组中的不同趟采集数据，从中选择一趟采集数据，基于选择的一趟采集数据的初始位姿数据，将所述同一分组中的其他趟采集数据进行对齐，得到所述同一个分组中的关键帧图像的第一次优化位姿数据；
位姿图优化模块405，被配置为对所述关键帧图像的第一次优化位姿数据进行位姿图pose graph优化，得到所述关键帧图像的第二次优化位姿数据，所述位姿图优化至少包括两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束；
全局优化模块406，被配置为基于所述关键帧图像的特征点和所述关键帧图像的第二次优化位姿数据进行全局光束平差法BA优化，得到所述关键帧图像的三维特征点和第三次优化位姿数据，位姿数据的精度逐次升高。
上文提及，近年来，自动驾驶或者智能驾驶技术研究成为热点和趋势，而自动驾驶或者智能驾驶的实现路径之一依赖于高精度地图（简称高精地图，亦可称为高清地图或者高标准地图）。高精地图可以为自动驾驶提供超视距感知，也是自动驾驶决策的重要依据。但是由于现实世界是动态变化的，高精度地图作为现实世界的数字化表达，需要不断更新以保证高精地图中的地理要素及其属性与现实世界保持一致。发明人发现，专业的高精地图采集车成本过高，无法大规模部署，因此，很难采用专业的高精地图采集车实现高精地图及时高效的更新。为满足高精地图的更新需求，低成本建图成为业界研究的热点。而准确的位姿数据是低成本建图获取高精度地图要素的关键。因此，如何提高位姿数据的准确度，是本领域技术人员需要持续解决和优化的问题。
考虑到上述问题，在该实施方式中，提出一种位姿优化装置，该装置基于各趟采集数据间的关键帧图像之间的帧间相对位姿关系和单趟采集数据的关键图像帧之间的帧间相对位姿关系，对各趟的初始位姿数据进行趟间位姿对齐、位姿图优化和全局BA优化这三次位姿优化，使得位姿数据的精度逐次升高。该技术方案能够提升整***姿的绝对精度和相对精度，恢复出更高精度的位姿数据，为高精度地图要素重建提供良好的基础，以便重建精度更高的高精地图。
在本公开一实施方式中，所述位姿优化装置可适用于可执行位姿优化的计算机、计算设备、电子设备、服务器、服务集群等。
在本公开一实施方式中，在构建某一道路的高精地图时，需要获取针对同一道路的两趟以上的采集数据，对这两趟以上的采集数据进行优化，得到更加精确的每趟的关键帧图像的特征点和所述关键帧图像对应的位姿数据，然后就能采用SFM（Structure FromMotion 运动恢复结构）恢复该道路所在环境的空间三维信息，构建高精地图。
在本公开一实施方式中，该关键帧图像是从采集的一序列有时序的图像数据中选择一些图像的作为关键帧图像，该关键帧图像的选择规则可以是关键帧图像不能太近也不能太远，需要在一定的间隔范围内选择关键帧图像。
在本公开一实施方式中，该关键帧图像的特征点可以用来标识该关键帧图像上一些目标物体，一般是将图像上灰度值发生剧烈变化的点或在图像边缘上曲率较大的点（如两个边缘的交点）看作为图像的特征点。该特征点包含关键点 (Key-points) 和描述子(Descriptor) 两部分，关键点表达了特征点的三维位置，而描述子则是对于特征点视觉特性的描述，大多为向量形式。
在本公开一实施方式中，位姿(pose)数据为采集该关键帧图像的采集设备的位置和姿态(朝向)信息，例如：在三维中一般是 (x，y，z，yaw，pitch，roll)，前三个元素描述物体的三维位置，后三个元素描述物体的姿态。其中yaw是航向角，绕Z轴旋转，pitch是俯仰角，绕 Y轴旋转，roll是翻滚角，绕X轴旋转。根据每趟采集数据中的关键帧图像进行相机标定(相机标定指的是利用相机所拍摄到的关键帧图像来还原空间中的物体)，根据相机标定的结果及采集设备的运动数据，可以确定拍摄到关键帧图像时该采集设备的位姿数据，即关键帧图像对应的初始位姿数据。
在本公开一实施方式中，各趟采集数据的关键帧图像的特征点用于标识关键帧图像中的目标物，进行特征点匹配指的是计算两个特征点的描述子的相似度（即计算两个特征点的描述子在向量空间上的距离），当相似度超过预设阈值如99%时，则这两个特征点是匹配的，可以记为共同特征点。如果两个关键帧图像之间的共同特征点个数超过预设个数，则两个关键帧图像之间的匹配结果是具有共视区域，如果两个关键帧图像之间的共同特征点个数未超过预设个数，则两个关键帧图像之间的匹配结果是不具有共视区域。共视区域是指两趟采集数据的关键帧图像显示有共同的目标物（可能视角不同），两个关键帧图像之间具有共视区域就表明两趟采集设备在采集这两个关键帧图像时采用不同的位姿拍摄到了同一个目标物。
在本公开一实施方式中，可以针对任一单趟采集数据，将单趟采集数据中的关键帧图像与其他趟采集数据中的关键帧图像均进行特征点匹配，得到任意两趟采集数据之间的任意两个关键帧图像之间的匹配结果。优选的，如果两个关键帧图像的采集位置之间的距离较远，则这两个关键帧图像之间就不会存在共视区域，故为了减少计算量，在进行特征点匹配时，可以针对单趟采集数据中的一关键帧图像，仅从其他趟采集数据中筛选出采集位置与该一关键帧图像的采集位置在一定距离范围内的关键帧图像进行特征点匹配，获取该一关键帧图像与对应筛选出的关键帧图像之间的匹配结果。
在本公开一实施方式中，基于所述不同趟采集数据中的关键帧图像的共视区域，可以将满足分组条件的各趟采集数据分为一组，其中，所述分组条件包括同一个分组中的每一趟采集数据有关键帧图像与其他趟采集数据中的关键帧图像有共视区域。
在本公开一实施方式中，对该关键帧图像对应的初始位姿数据的第一次位姿优化是基于同一分组中选择的一趟采集数据的初始位姿数据，将同一分组中的其他趟采集数据进行刚性对齐，该刚性对齐指的是每趟采集数据的各关键帧图像对应的初始位姿数据都变换同一位移和朝向。示例的，图2示出根据本公开一实施方式的两趟间位姿刚性对齐的过程示意图，如图2所示，同一分组中包括两趟采集数据S1和S2，该S1的初始位姿数据形成的位姿轨迹和S2的初始位姿数据形成的位姿轨迹之间是有偏移的，将该S2的各初始位姿信息中的位置和朝向按照同一个标准参数移动，使其与该S1的位姿数据形成的位姿轨迹刚性对齐，得到S2的第一次优化位姿数据，该S1的第一次优化位姿数据为该S1的初始位姿数据，如此得到对齐后的S1和S2。
在本公开一实施方式中，两个关键帧图像具有共视区域，则表明两个关键帧图像中显示有同一物体只是显示视角不同，故可以据此获取两个关键帧图像的相对位姿关系，如此根据同一分组中两趟采集数据之间的具有共视区域的关键帧图像，就可以统计得到两趟采集数据之间的相对位姿关系，可以按照该两趟采集数据之间的相对位姿关系将这两趟采集数据的初始位姿数据进行刚性对齐，得到两趟采集数据的关键帧图像的第一次优化位姿数据；然后，由于在同一个分组中必然有一趟或多趟采集数据与这两趟采集数据中某一趟采集数据之间的关键帧图像具有共视区域，则可以统计获取其他一趟采集数据与这两趟采集数据中某一趟采集数据之间的相对位姿关系，并据此相对位姿关系将该其他一趟采集数据向该某一趟采集数据的初始位姿数据进行刚性对齐，如此，按照上述方案将该同一个分组中的不同趟采集数据均进行刚性对齐后，就得到了所述同一个分组中的关键帧图像的第一次优化位姿数据。
这里需要说明的是，各趟采集数据间由于采集时间不连续，采集设备不同，得到的各趟采集数据中的位姿轨迹都存在一定的偏移，将各趟采集数据中的初始位姿数据进行刚性对齐，可以为后续的位姿图优化能提供好的初始值，避免出现局部最优的情况。
在本公开一实施方式中，pose graph（位姿图）优化利用短间隔可靠的相对位姿测量来构建一个涵盖长时间跨度的关键帧图像们的全局优化问题，平摊累积误差，以各关键帧图像对应的位姿数据为节点，以关键帧图像间相对位姿计算结果作为测量进行极大似然估计。pose graph优化的优化变量是位姿，需要构建一个只有位姿轨迹的图优化，而位姿节点之间的边，由两个关键帧图像之间通过特征匹配后得到的运动估计来给定初始值，一旦初始值完成，就不再优化特征点的位置，只关心采集设备位姿之间的联系，可以基于单趟采集数据的关键图像帧之间的帧间相对位姿约束和任意两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束，对各趟关键帧图像的第一次优化位姿数据进行第二次位姿优化，得到关键帧图像的第二次优化位姿数据，该第二次优化位姿数据恢复了每趟关键帧图像的位姿数据的绝对精度同时保证每趟关键帧图像之间位姿数据的相对精度。第二次优化位姿数据的精度大于第一次优化位姿数据的精度。
在本公开一实施方式中，BA（Bundle Adjustment，光束平差法）优化不仅优化位姿还优化特征点，BA优化包括重投影误差（reprojection error）的优化；根据特征点的三维位置坐标、位姿数据，把三维的特征点投影到图像平面中，然后我们来算它跟图像中，通过特征提取出来的那个点之间的误差，BA优化就是要最小化这个误差。该BA优化可以调整第二次优化位姿数据和特征点使得所观测的图像位置和预测的图像位置之间的误差最小，如此得到关键帧图像的三维特征点和第三次优化位姿数据，第三次优化位姿数据的精度大于第二次优化位姿数据的精度。
在上述实施方式中，首先获取针对同一道路的两趟以上的采集数据，一趟采集数据包括：关键帧图像的特征点和所述关键帧图像对应的初始位姿数据，针对单趟采集数据，将所述单趟采集数据中的关键帧图像与其他趟采集数据中的关键帧图像进行特征点匹配，获得不同趟采集数据中的关键帧图像的匹配结果，所述匹配结果包括两个关键帧图像之间是否有共视区域的匹配结果；基于所述不同趟采集数据中的关键帧图像的共视区域，对所述不同趟采集数据进行分组；然后，针对同一个分组中的不同趟采集数据，从中选择一趟采集数据，基于选择的一趟采集数据的初始位姿数据，将所述同一分组中的其他趟采集数据进行对齐，得到所述同一个分组中的关键帧图像的第一次优化位姿数据；对齐后的第一次优化位姿数据为pose graph提供了好的初始值，对每趟的关键帧图像的第一次优化位姿数据进行pose graph恢复了每趟关键帧图像的位姿数据的绝对精度同时保证每趟关键帧图像之间位姿数据的相对精度，能为后续的BA优化提供更好的初值；最后对关键帧图像的特征点和所述关键帧图像的第二次优化位姿数据进行全局BA优化，可以获取到高精度的关键帧图像的三维特征点和第三次优化位姿数据，为后续构建高精度地图提供良好的基础，以便重建精度更高的高精地图。另外，本实施例在进行位姿优化时只利用到图像的特征点，使用数据更加精简，而且本方法不需要采集太多趟的采集数据（示例的可以仅使用两趟采集数据），就能恢复更高精度的位姿数据。
在本公开一实施方式中，所述分组模块403被配置为：
将所述不同趟采集数据中满足分组条件的各趟采集数据分为一组，其中，所述分组条件包括同一个分组中的每一趟采集数据有关键帧图像与其他一趟采集数据中的关键帧图像有共视区域。
在该实施方式中，同一个分组中的每一趟采集数据必然与其他某一趟或多趟采集数据中的关键帧图像有共视区域，如果是不同分组，则同一组中的任一趟采集数据中均没有关键帧图像与另一组中的各趟采集数据的关键帧图像有共视区域。
在该实施方式中，假设共获取到6趟采集数据S1、S2、S3、S4、S5和S6，其中，S1和S2之间有关键帧图像具有共视区域；S3和S2之间有关键帧图像具有共视区域，但是S3和S1之间没有关键帧图像具有共视区域；S4和S3之间有关键帧图像具有共视区域，但是S4和S1、S2之间没有关键帧图像具有共视区域，S5和S6之间有关键帧图像具有共视区域，但是S5与S1至S4之间均没有关键帧图像具有共视区域，S6与S1至S4之间也均没有关键帧图像具有共视区域，则可以将S1、S2、S3和S4分为第一组，将S5和S6分为第二组。在该第一组中每一趟采集数据必然有一个或多个关键帧图像与其他某一趟或多趟采集数据中的关键帧图像有共视区域，如S1的某关键帧图像与S2的某关键帧图像之间有共视区域，S2的某关键帧图像与S1或S3的某关键帧图像之间有共视区域，S3的某关键帧图像与S2或S4的某关键帧图像之间有共视区域，S4的某关键帧图像与S3的某关键帧图像之间有共视区域。
在本公开一实施方式中，所述对齐模块404被配置为
针对同一个分组中的不同趟采集数据，从中选择第一趟采集数据；
基于所述同一个分组中的第二趟采集数据与所述第一趟采集数据之间具有共视区域的各对共视关键帧图像的初始位姿数据，通过角度N点PnP，得到各对共视关键帧图像的第一帧间相对位姿信息；
对所述第二趟采集数据与所述第一趟采集数据之间的各对共视关键帧图像的第一帧间相对位姿信息进行统计，得所述第二趟采集数据与所述第一趟采集数据之间的第一帧间加权相对位姿信息；
基于所述第一帧间加权相对位姿信息，将所述第二趟采集数据向第一趟采集数据的初始位姿数据进行对齐，得到所述第二趟采集数据中的关键帧图像的第一次优化位姿数据；
基于具有共视区域的第三趟采集数据的关键帧图像的初始位姿数据与已对齐的一趟采集数据的关键帧图像的第一次优化位姿数据，通过PnP，得到各对共视关键帧图像的第二帧间相对位姿信息；
对第三趟采集数据与已对齐的一趟采集数据之间的各对共视关键帧图像的第二帧间相对位姿信息进行统计，得到所述第三趟采集数据与已对齐的一趟采集数据之间的第二帧间加权相对位姿信息；
基于所述第二帧间加权相对位姿信息，将所述第三趟采集数据向已对齐的一趟采集数据的第一次优化位姿数据进行对齐，得到所述第三趟采集数据中的关键帧图像的第一次优化位姿数据。
在该实施例方式中，可以从该同一个分组中的不同趟采集数据中随机选择一趟采集数据作为第一趟采集数据，该第二趟采集数据为该同一个分组中与该第一趟采集数据具有共视区域的任意一趟采集数据，可以通过角度N点PnP，基于第二趟采集数据与第一趟采集数据之间具有共视区域的各对共视关键帧图像的初始位姿数据，得到各对共视关键帧图像的第一帧间相对位姿信息，进而统计得到第二趟采集数据与所述第一趟采集数据之间的第一帧间加权相对位姿信息，该统计方法可以是将各第一帧间相对位姿信息进行加权平均。将所述第二趟采集数据的初始位姿数据均按照该第一帧间加权相对位姿信息进行变换，如此向第一趟采集数据的初始位姿数据进行对齐，得到所述第二趟采集数据中的关键帧图像的第一次优化位姿数据，该第一趟采集数据中的关键帧图像的第一次优化位姿数据为该第一趟采集数据的初始位姿数据。此时，已对齐的采集数据就是第一趟采集数据和第二趟采集数据。
在该实施方式中，该第三趟采集数据为该同一个分组中与该已对齐的采集数据中的任一趟采集数据具有共视区域的一趟采集数据。可以基于具有共视区域的第三趟采集数据的关键帧图像的初始位姿数据与已对齐的一趟采集数据的关键帧图像的第一次优化位姿数据，通过PnP，得到第三趟采集数据与该已对齐的一趟采集数据之间的各对共视关键帧图像的第二帧间相对位姿信息；对这些第二帧间相对位姿信息进行加权统计，得到所述第三趟采集数据与已对齐的一趟采集数据之间的第二帧间加权相对位姿信息；将所述第三趟采集数据的初始位姿数据均按照该第二帧间加权相对位姿信息进行变换，如此向已对齐的一趟采集数据的第一次优化位姿数据进行对齐，得到所述第三趟采集数据中的关键帧图像的第一次优化位姿数据。此时，已对齐的采集数据就是第一趟采集数据、第二趟采集数据和第三趟采集数据，同一分组中的其他剩余趟的采集数据按照步骤A5至A7重复进行，直至将同一分组中的各趟的采集数据均对齐。
这里需要说明的是，根据一对共视关键帧各自的共同特征点和位姿数据，使用PnP算法可以估计出这一对共视关键帧的帧间相对位姿信息。
示例的，假设共获取到4趟采集数据S1、S2、S3、S4，其中，S1和S2之间有关键帧图像具有共视区域；S3和S2之间有关键帧图像具有共视区域，但是S3和S1之间没有关键帧图像具有共视区域；S4和S3之间有关键帧图像具有共视区域，但是S4和S1、S2之间没有关键帧图像具有共视区域，则可以将S1、S2、S3和S4分为同一分组。可以从该同一个分组中的随机选择S2作为第一趟采集数据，该第二趟采集数据可以为该同一个分组中与该S2具有共视区域的S1和S3中的一趟如S1，可以通过角度N点PnP算法，基于S1与S2之间具有共视区域的各对共视关键帧图像的初始位姿数据，得到各对共视关键帧图像的第一帧间相对位姿信息，进而统计得到S1与S2之间的第一帧间加权相对位姿信息。将S1的初始位姿数据均按照该第一帧间加权相对位姿信息进行变换，如此向S2的初始位姿数据进行对齐，得到所述S1中关键帧图像的第一次优化位姿数据，该S2中关键帧图像的第一次优化位姿数据为该S2的初始位姿数据。此时，已对齐的采集数据就是S1和S2。该第三趟采集数据为该同一个分组中与S1和S2中S2具有共视区域的一趟采集数据S3。可以基于具有共视区域的S3的关键帧图像的初始位姿数据与S2的关键帧图像的第一次优化位姿数据，通过PnP算法，得到S2与S3之间的各对共视关键帧图像的第二帧间相对位姿信息；对这些第二帧间相对位姿信息进行加权统计，得到S2与S3之间的第二帧间加权相对位姿信息；将S3的初始位姿数据均按照该第二帧间加权相对位姿信息进行变换，如此向S2的第一次优化位姿数据进行对齐，得到S3中的关键帧图像的第一次优化位姿数据。此时，已对齐的采集数据就是S1、S2和S3。其他剩余趟的采集数据S4按照步骤A5至A7重复进行如下：该S4与S3之间有共视区域，则可以基于具有共视区域的S4的关键帧图像的初始位姿数据与已对齐的一趟采集数据S3的关键帧图像的第一次优化位姿数据，通过PnP算法，得到S4与S3之间的各对共视关键帧图像的第二帧间相对位姿信息；对这些第二帧间相对位姿信息进行加权统计，得到S4与S3之间的第二帧间加权相对位姿信息；将S4的初始位姿数据均按照该第二帧间加权相对位姿信息进行变换，如此向S3的第一次优化位姿数据进行对齐，得到所述S4中的关键帧图像的第一次优化位姿数据，如此就将同一分组中的S1至S4的采集数据均对齐。
这里需要说明的是，每趟采集数据与同一分组中N趟采集数据具有共视区域，每趟采集数据对应的N值不同，所述第一趟采集数据就是N值最大的一趟采集数据。若同一分组中其他趟采集数据均与某一趟采集数据具有共视区域，则可以选择该某一趟采集数据为第一趟采集数据，按照上述方法分别基于其他趟采集数据与第一趟采集数据之间具有共视区域的各对共视关键帧图像，通过角度N点PnP，得到各对共视关键帧图像的帧间相对位姿信息，然后对其他趟采集数据与所述第一趟采集数据之间的各对共视关键帧图像的帧间相对位姿信息进行统计，得所述其他趟采集数据与所述第一趟采集数据之间的帧间加权相对位姿信息，最后基于该帧间加权相对位姿信息，将其他趟采集数据向第一趟采集数据的初始位姿数据进行对齐，得到所述其他趟采集数据中的关键帧图像的第一次优化位姿数据。
在本公开一实施方式中，所述获取模块401被配置为：
获取针对同一道路的两趟以上的图像数据和运动数据；
根据每趟的图像数据和运动数据，通过视觉惯性里程计VIO算法获取每趟的关键帧图像对应的初始位姿数据，所述每趟的关键帧图像的特征点在VIO算法的计算过程中获取。
在该实施方式中，通过在采集设备上安装传感器 (比如，相机、IMU（InertialMeasurement Unit，惯性测量单元）、RTK（Real Time Kinematic，实时动态测量）等)，以采集该采集设备在运动过程的图像数据和运动数据，该图像数据是相机在每趟采集的运动过程中采集到的一序列图像，该运动数据包括相机在每趟采集的运动过程中的RTK数据，或者RTK数据和IMU数据等，该IMU是主要用来检测和测量加速度与旋转运动的传感器，该IMU通过集成多个惯性传感器以生成沿多个轴或自由度的加速度和角速度测量值，该IMU数据就是该IMU实时测量到该采集设备的加速度和角速度测量值。该RTK是以载波相位观测为根据的实时差分GPS技术，可以实时得到厘米级定位精度的三维位姿数据，该RTK数据就是采集设备的实时三维位姿数据。
在该实施方式中，为了实现低成本建图，针对同一道路的两趟以上的图像数据和运动数据可以通过众包资料获取，该众包资料是各种采集设备如车载相机、手机相机、数码相机或者摄像机等拍摄获取的。需要了解的是，众包资料是指由大众经过一定方法(如使用车载相机拍摄)获取后通过互联网向公众或相关机构提供的一种开放式资料数据。大众可以自愿上传众包资料或者通过参与相关机构下发的众包任务的方式提供众包资料。该众包资料里就包括大众进行每趟采集时获取的图像数据和运动数据，一趟采集数据是指基于连续时间内的一次采集获取的图像数据和运动数据得到的关键帧图像的特征点和关键帧图像对应的初始位姿数据，每趟采集数据是有时序性的。在需要对某条道路进行众包建图时，可以从众包资料里获取针对该道路的两趟以上的采集数据。
在该实施方式中，VIO（Visual Inertial Odometry，视觉里程计）算法在计算过程中会先提取关键帧图像中的特征点，然后计算关键帧图像对应的初始位姿数据，故可以在VIO计算的过程中获取每趟的关键帧图像的特征点，而不需额外再进行特征点提取，减少计算步骤。
在本公开一实施方式中，所述位姿图优化模块405被配置为：
通过构建位姿先验约束、单趟采集数据的关键图像帧之间的帧间位姿约束和两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束，对所述关键帧图像的第一次优化位姿数据进行pose graph优化，得到所述关键帧图像的第二次优化位姿数据。
在该实施方式中，在应用位姿图优化来优化该第一次优化位姿数据时，要在位姿先验约束、单趟采集数据的关键图像帧之间的帧间位姿约束和两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束下来进行优化调整，该位姿先验约束用于约束对该关键帧图像对应的第一次优化位姿数据的调整只能在一定范围，该单趟采集数据的关键图像帧之间的帧间位姿约束是指在调整某关键帧图像对应的第一次优化位姿数据时，该关键帧图像与其所在趟内的有共视区域的关键帧图像之间的帧间相对位姿要满足一定的约束条件，不能超过根据这些关键帧图像的初始位姿数据计算的帧间相对位姿太多；该两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束是指在调整关键帧图像对应的第一次优化位姿数据时，该关键帧图像与其他趟内的有共视区域的关键帧图像之间的帧间相对位姿也要满足一定的约束条件，不能超过根据这些关键帧图像的初始位姿数据计算的帧间相对位姿太多。这样就可以恢复每趟关键帧图像的位姿数据的绝对精度同时保证各趟关键帧图像之间位姿数据的相对精度。
在本公开一实施方式中，所述全局优化模块406被配置为：
通过建立3D特征点重投影误差、单趟采集数据的关键图像帧之间的帧间位姿约束、位姿先验约束的因子图，对所述关键帧图像的特征点和所述关键帧图像的第二次优化位姿数据进行全局光束平差法BA优化，得到所述关键帧图像的三维特征点和第三次优化位姿数据。
在该实施方式中，3D特征点重投影误差是指根据特征点的三维位置坐标、采集设备的位姿数据，把特征点投影到二维图像平面中，然后来算它跟二维图像中通过特征提取出来的那个点之间的距离，在进行BA优化时要最小化该距离。示例的，如图3所示，特征点P1、P2、P3投影到位姿分别是T1、T2、T3的关键帧图像上，将该投影位置和该关键帧图像上提取P1、P2、P3的实际位置进行比对，计算两者之间的距离，调整特征点和位姿数据，将这个距离最小化。
在该实施方式中，在最小化重投影误差时，还会考虑单趟采集数据的关键图像帧之间的帧间位姿约束、位姿先验约束，如此调整特征点和第二次优化位姿数据进行全局BA优化，得到三维特征点和第三次优化位姿数据。
本公开实施例还公开了一种导航服务，其中，基于上述的位姿优化方法，获得被导航对象所在区域的高精地图，并基于所述高精地图为所述被导航对象提供相应场景的导航引导服务。其中，所述相应场景为AR导航或者高架导航或者主辅路导航中的一种或多种的组合。
本公开实施例还公开了一种导航方法，其中，基于高精地图获取至少基于起点、终点及路况计算的导航路线，基于所述导航路线进行导航引导，所述高精地图是基于上述的任意一种方法中获取的三维特征点和第三次优化位姿数据进行地图重建实现的。
本公开还公开了一种电子设备，图5示出根据本公开一实施方式的电子设备的结构框图，如图5所示，所述电子设备500包括存储器501和处理器502；其中，
所述存储器501用于存储一条或多条计算机指令，其中，所述一条或多条计算机指令被所述处理器502执行以实现上述方法步骤。
图6是适于用来实现根据本公开一实施方式的位姿优化方法的计算机***的结构示意图。
如图6所示，计算机***600包括处理单元601，其可以根据存储在只读存储器（ROM）602中的程序或者从存储部分608加载到随机访问存储器（RAM）603中的程序而执行上述实施方式中的各种处理。在RAM603中，还存储有***600操作所需的各种程序和数据。处理单元601、ROM602以及RAM603通过总线604彼此相连。输入/输出（I/O）接口605也连接至总线604。
以下部件连接至I/O接口605：包括键盘、鼠标等的输入部分606；包括诸如阴极射线管（CRT）、液晶显示器（LCD）等以及扬声器等的输出部分607；包括硬盘等的存储部分608；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分609。通信部分609经由诸如因特网的网络执行通信处理。驱动器610也根据需要连接至I/O接口605。可拆卸介质611，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器610上，以便于从其上读出的计算机程序根据需要被安装入存储部分608。其中，所述处理单元601可实现为CPU、GPU、TPU、FPGA、NPU等处理单元。
特别地，根据本公开的实施方式，上文描述的方法可以被实现为计算机软件程序。例如，本公开的实施方式包括一种计算机程序产品，其包括有形地包含在及其可读介质上的计算机程序，所述计算机程序包含用于执行所述位姿优化方法的程序代码。在这样的实施方式中，该计算机程序可以通过通信部分609从网络上被下载和安装，和/或从可拆卸介质611被安装。
附图中的流程图和框图，图示了按照本公开各种实施方式的***、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，路程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的***来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本公开实施方式中所涉及到的单元或模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的单元或模块也可以设置在处理器中，这些单元或模块的名称在某种情况下并不构成对该单元或模块本身的限定。
作为另一方面，本公开实施例还提供了一种计算机可读存储介质，该计算机可读存储介质可以是上述实施方式中所述装置中所包含的计算机可读存储介质；也可以是单独存在，未装配入设备中的计算机可读存储介质。计算机可读存储介质存储有一个或者一个以上程序，所述程序被一个或者一个以上的处理器用来执行描述于本公开实施例的方法。
以上描述仅为本公开的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本公开实施例中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离所述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本公开实施例中公开的（但不限于）具有类似功能的技术特征进行互相替换而形成的技术方案。

Claims (11)

1.一种位姿优化方法，包括：
获取针对同一道路的两趟以上的采集数据，一趟采集数据包括：关键帧图像的特征点和所述关键帧图像对应的初始位姿数据；
针对单趟采集数据，将所述单趟采集数据中的关键帧图像与其他趟采集数据中的关键帧图像进行特征点匹配，获得不同趟采集数据中的关键帧图像的匹配结果，所述匹配结果包括两个关键帧图像之间是否有共视区域的匹配结果；
基于所述不同趟采集数据中的关键帧图像的共视区域，对所述不同趟采集数据进行分组；
针对同一个分组中的不同趟采集数据，从中选择一趟采集数据，基于选择的一趟采集数据的初始位姿数据，将所述同一分组中的其他趟采集数据进行对齐，得到所述同一个分组中的关键帧图像的第一次优化位姿数据；
对所述关键帧图像的第一次优化位姿数据进行位姿图pose graph优化，得到所述关键帧图像的第二次优化位姿数据，所述位姿图优化至少包括两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束；
基于所述关键帧图像的特征点和所述关键帧图像的第二次优化位姿数据进行全局光束平差法BA优化，得到所述关键帧图像的三维特征点和第三次优化位姿数据，位姿数据的精度逐次升高。
2.根据权利要求1所述的方法，所述基于所述不同趟采集数据中的关键帧图像的共视区域，对所述不同趟采集数据进行分组，包括：
将所述不同趟采集数据中满足分组条件的各趟采集数据分为一组，其中，所述分组条件包括同一个分组中的每一趟采集数据有关键帧图像与其他一趟采集数据中的关键帧图像有共视区域。
3.根据权利要求1或2所述的方法，所述针对同一个分组中的不同趟采集数据，从中选择一趟采集数据，基于选择的一趟采集数据的初始位姿数据，将所述同一分组中的其他趟采集数据进行对齐，得到所述同一个分组中的关键帧图像的第一次优化位姿数据，包括：
针对同一个分组中的不同趟采集数据，从中选择第一趟采集数据；
基于所述同一个分组中的第二趟采集数据与所述第一趟采集数据之间具有共视区域的各对共视关键帧图像的初始位姿数据，通过角度N点PnP，得到各对共视关键帧图像的第一帧间相对位姿信息；
对所述第二趟采集数据与所述第一趟采集数据之间的各对共视关键帧图像的第一帧间相对位姿信息进行统计，得所述第二趟采集数据与所述第一趟采集数据之间的第一帧间加权相对位姿信息；
基于所述第一帧间加权相对位姿信息，将所述第二趟采集数据向第一趟采集数据的初始位姿数据进行对齐，得到所述第二趟采集数据中的关键帧图像的第一次优化位姿数据；
基于具有共视区域的第三趟采集数据的关键帧图像的初始位姿数据与已对齐的一趟采集数据的关键帧图像的第一次优化位姿数据，通过PnP，得到各对共视关键帧图像的第二帧间相对位姿信息；
对所述第三趟采集数据与已对齐的一趟采集数据之间的各对共视关键帧图像的第二帧间相对位姿信息进行统计，得到所述第三趟采集数据与已对齐的一趟采集数据之间的第二帧间加权相对位姿信息；
基于所述第二帧间加权相对位姿信息，将所述第三趟采集数据向已对齐的一趟采集数据的第一次优化位姿数据进行对齐，得到所述第三趟采集数据中的关键帧图像的第一次优化位姿数据。
4.根据权利要求1所述的方法，所述获取针对同一道路的两趟以上的采集数据，包括：
获取针对同一道路的两趟以上的图像数据和运动数据；
根据每趟的图像数据和运动数据，通过视觉惯性里程计VIO算法获取每趟的关键帧图像对应的初始位姿数据，每趟的关键帧图像的特征点在VIO算法的计算过程中获取。
5.根据权利要求1所述的方法，所述对所述关键帧图像的第一次优化位姿数据进行位姿图pose graph优化，得到所述关键帧图像的第二次优化位姿数据，包括：
通过构建位姿先验约束、单趟采集数据的关键图像帧之间的帧间位姿约束和两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束，对所述关键帧图像的第一次优化位姿数据进行pose graph优化，得到所述关键帧图像的第二次优化位姿数据。
6.根据权利要求1所述的方法，所述基于所述关键帧图像的特征点和所述关键帧图像的第二次优化位姿数据进行全局光束平差法BA优化，得到所述关键帧图像的三维特征点和第三次优化位姿数据，包括：
通过建立3D特征点重投影误差、单趟采集数据的关键图像帧之间的帧间位姿约束、位姿先验约束的因子图，对所述关键帧图像的特征点和所述关键帧图像的第二次优化位姿数据进行全局BA优化，得到所述关键帧图像的三维特征点和第三次优化位姿数据。
7.一种位姿优化装置，包括：
获取模块，被配置为获取针对同一道路的两趟以上的采集数据，一趟采集数据包括：关键帧图像的特征点和所述关键帧图像对应的初始位姿数据；
匹配模块，被配置为针对单趟采集数据，将所述单趟采集数据中的关键帧图像与其他趟采集数据中的关键帧图像进行特征点匹配，获得不同趟采集数据中的关键帧图像的匹配结果，所述匹配结果包括两个关键帧图像之间是否有共视区域的匹配结果；
分组模块，被配置为基于所述不同趟采集数据中的关键帧图像的共视区域，对所述不同趟采集数据进行分组；
对齐模块，被配置为针对同一个分组中的不同趟采集数据，从中选择一趟采集数据，基于选择的一趟采集数据的初始位姿数据，将所述同一分组中的其他趟采集数据进行对齐，得到所述同一个分组中的关键帧图像的第一次优化位姿数据；
位姿图优化模块，被配置为对所述关键帧图像的第一次优化位姿数据进行位姿图posegraph优化，得到所述关键帧图像的第二次优化位姿数据，所述位姿图优化至少包括两趟采集数据之间具有共视区域的关键帧图像之间的帧间相对位姿约束；
全局优化模块，被配置为基于所述关键帧图像的特征点和所述关键帧图像的第二次优化位姿数据进行全局光束平差法BA优化，得到所述关键帧图像的三维特征点和第三次优化位姿数据，位姿数据的精度逐次升高。
8.一种电子设备，包括存储器和至少一个处理器；其中，所述存储器用于存储一条或多条计算机指令，其中，所述一条或多条计算机指令被所述至少一个处理器执行以实现权利要求1-6任一项所述的方法步骤。
9.一种计算机可读存储介质，其上存储有计算机指令，该计算机指令被处理器执行时实现权利要求1-6任一项所述的方法步骤。
10.一种计算机程序产品，包括计算机程序/指令，其中，该计算机程序/指令被处理器执行时实现权利要求1-6任一项所述的方法步骤。
11.一种导航方法，其中，基于高精地图获取至少基于起点、终点及路况计算的导航路线，基于所述导航路线进行导航引导，所述高精地图是基于权利要求1-6所述的任意一项方法中获取的三维特征点和第三次优化位姿数据进行地图重建实现的。
CN202110903076.5A 2021-08-06 2021-08-06 位姿优化方法、装置、电子设备、介质及程序产品 Active CN113744308B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110903076.5A CN113744308B (zh) 2021-08-06 2021-08-06 位姿优化方法、装置、电子设备、介质及程序产品

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110903076.5A CN113744308B (zh) 2021-08-06 2021-08-06 位姿优化方法、装置、电子设备、介质及程序产品

Publications (2)

Publication Number Publication Date
CN113744308A CN113744308A (zh) 2021-12-03
CN113744308B true CN113744308B (zh) 2024-02-20

Family

ID=78730389

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110903076.5A Active CN113744308B (zh) 2021-08-06 2021-08-06 位姿优化方法、装置、电子设备、介质及程序产品

Country Status (1)

Country Link
CN (1) CN113744308B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114742884B (zh) * 2022-06-09 2022-11-22 杭州迦智科技有限公司 一种基于纹理的建图、里程计算、定位方法及***
CN117128985B (zh) * 2023-04-27 2024-05-31 荣耀终端有限公司 点云地图更新的方法及设备

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019157925A1 (zh) * 2018-02-13 2019-08-22 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及***
CN110866496A (zh) * 2019-11-14 2020-03-06 合肥工业大学 基于深度图像的机器人定位与建图方法和装置
WO2020259248A1 (zh) * 2019-06-28 2020-12-30 Oppo广东移动通信有限公司 基于深度信息的位姿确定方法、装置、介质与电子设备

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019157925A1 (zh) * 2018-02-13 2019-08-22 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及***
WO2020259248A1 (zh) * 2019-06-28 2020-12-30 Oppo广东移动通信有限公司 基于深度信息的位姿确定方法、装置、介质与电子设备
CN110866496A (zh) * 2019-11-14 2020-03-06 合肥工业大学 基于深度图像的机器人定位与建图方法和装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
CFD-SLAM:融合特征法与直接法的快速鲁棒SLAM***;王化友;代波;何玉庆;;高技术通讯(12);全文 *

Also Published As

Publication number Publication date
CN113744308A (zh) 2021-12-03

Similar Documents

Publication Publication Date Title
CN110243358B (zh) 多源融合的无人车室内外定位方法及***
JP7326720B2 (ja) 移動***置推定システムおよび移動***置推定方法
CN106679648B (zh) 一种基于遗传算法的视觉惯性组合的slam方法
CN110146909B (zh) 一种定位数据处理方法
CN112197770B (zh) 一种机器人的定位方法及其定位装置
CN112907678B (zh) 车载相机外参姿态动态估计方法、装置、计算机设备
US20220292711A1 (en) Pose estimation method and device, related equipment and storage medium
CN108986037A (zh) 基于半直接法的单目视觉里程计定位方法及定位***
CN109710724B (zh) 一种构建点云地图的方法和设备
CN110617821B (zh) 定位方法、装置及存储介质
CN113744308B (zh) 位姿优化方法、装置、电子设备、介质及程序产品
CN108519102B (zh) 一种基于二次投影的双目视觉里程计算方法
CN111261016B (zh) 道路地图的构建方法、装置和电子设备
CN111830953A (zh) 车辆自定位方法、装置及***
JP2022542289A (ja) 地図作成方法、地図作成装置、電子機器、記憶媒体及びコンピュータプログラム製品
CN114623817A (zh) 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法
JP2012118666A (ja) 三次元地図自動生成装置
Zhang et al. Vision-aided localization for ground robots
CN115326084A (zh) 车辆定位方法、装置、计算机设备、存储介质
CN116295412A (zh) 一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法
CN113838129B (zh) 一种获得位姿信息的方法、装置以及***
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
CN113345032B (zh) 一种基于广角相机大畸变图的初始化建图方法及***
KR101006977B1 (ko) 수치지도 제작 시 수치데이터를 보상하는 방법
CN112651991A (zh) 视觉定位方法、装置及计算机***

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