CN114842080A - 位姿优化方法、装置、电子设备及存储介质 - Google Patents

位姿优化方法、装置、电子设备及存储介质 Download PDF

Info

Publication number
CN114842080A
CN114842080A CN202210507633.6A CN202210507633A CN114842080A CN 114842080 A CN114842080 A CN 114842080A CN 202210507633 A CN202210507633 A CN 202210507633A CN 114842080 A CN114842080 A CN 114842080A
Authority
CN
China
Prior art keywords
vector
matching
semantic
pose
features
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.)
Pending
Application number
CN202210507633.6A
Other languages
English (en)
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.)
Suzhou Yihang Yuanzhi Intelligent Technology Co ltd
Original Assignee
Suzhou Yihang Yuanzhi Intelligent Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Suzhou Yihang Yuanzhi Intelligent Technology Co ltd filed Critical Suzhou Yihang Yuanzhi Intelligent Technology Co ltd
Priority to CN202210507633.6A priority Critical patent/CN114842080A/zh
Publication of CN114842080A publication Critical patent/CN114842080A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/22—Matching criteria, e.g. proximity measures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
    • G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
    • G06V10/751—Comparing pixel values or logical combinations thereof, or feature values having positional relevance, e.g. template matching

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • General Physics & Mathematics (AREA)
  • Physics & Mathematics (AREA)
  • Evolutionary Computation (AREA)
  • Data Mining & Analysis (AREA)
  • Artificial Intelligence (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Biology (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Health & Medical Sciences (AREA)
  • Computing Systems (AREA)
  • Databases & Information Systems (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Multimedia (AREA)
  • Image Analysis (AREA)

Abstract

本公开提供了一种位姿优化方法、装置、电子设备及存储介质。本公开的位姿优化方法包括：获取对应第一对象当前所处场景的第一图像和第一对象的第一位姿；获取第一图像的检测矢量特征；获取矢量地图中第一位姿的邻域范围内的投影矢量特征；将检测矢量特征与投影矢量特征匹配，以获得匹配结果；最小化基于匹配结果确定的重投影误差，以优化第一位姿，获得第一对象的第二位姿。本公开能够有效提升位姿优化的精度。

Description

位姿优化方法、装置、电子设备及存储介质
技术领域
本公开涉及计算机视觉技术领域，尤其涉及一种位姿优化方法、装置、电子设备及存储介质。
背景技术
视觉定位技术是目前自动驾驶、智能机器人领域的热门研究方向，主流的视觉定位方法通常分为两大类：全局重定位技术和基于传感器估计位姿的位姿优化方法。其中，结合高精地图的传感器位姿优化方法是目前的研究热点之一。
如何从包含光照变化、场景变化的图像中获取稳定的高精矢量特征，如何实现当前场景和高精地图之间的数据关联，以及结合高精地图特征如何进行位姿优化，都是基于高精地图的视觉定位方法需要解决的问题。然而，上述问题目前尚未得到有效解决。
发明内容
为了解决上述技术问题中的至少一个，本公开提供了一种位姿优化方法、装置、电子设备及存储介质。
本公开的第一方面提供了一种位姿优化方法，包括：
获取对应第一对象当前所处场景的第一图像和第一对象的第一位姿；
获取所述第一图像的检测矢量特征；
获取矢量地图中所述第一位姿的邻域范围内的投影矢量特征；
将所述检测矢量特征与所述投影矢量特征匹配，以获得匹配结果；
最小化基于所述匹配结果确定的重投影误差，以优化所述第一位姿，获得所述第一对象的第二位姿。
本公开的一些实施方式中，所述第一图像的检测矢量特征包括所述第一图像中各个语义对象的矢量描述子，所述矢量描述子用于描述所述语义对象的语义标签、形状点位置、中心点位置、邻域特征位置向量和方向向量。
本公开的一些实施方式中，所述获取所述第一图像的检测矢量特征，包括：
对连续k帧的所述第一图像进行目标检测和多帧融合，以获得第一图像中各个语义对象的特征，所述语义对象的特征少包括所述语义对象的语义标签，k为大于1的整数；
根据所述语义对象的语义标签对各个所述语义对象的特征进行矢量描述，以获得各个所述语义对象的矢量描述子，从而形成所述第一图像的检测矢量特征。
本公开的一些实施方式中，所述投影矢量特征包括矢量地图中所述第一位姿的邻域范围内各个参考对象在所述第一图像的图像坐标系的投影矢量描述子，所述投影矢量描述子用于描述所述参考对象在所述第一图像的图像坐标系中的语义标签、形状点位置、中心点位置、邻域特征位置向量和方向向量。
本公开的一些实施方式中，所述获取矢量地图中所述第一位姿的邻域范围内的投影矢量特征，包括：
从矢量地图中解析得到第一位姿的邻域范围内各个参考对象的矢量特征；
将所述各个参考对象的矢量特征投影至第一图像的图像坐标系，以获得各个参考对象的投影矢量特征；
对各个参考对象的投影矢量特征进行矢量描述，以获得各个参考对象的投影矢量描述子。
本公开的一些实施方式中，所述将所述检测矢量特征与所述投影矢量特征匹配，以获得匹配结果，包括：
通过所述检测矢量特征与所述投影矢量特征之间的相似度过滤所述投影矢量特征，以使得所述投影矢量特征与所述检测矢量特征在矢量特征空间分布上具有近似的一致性；以及，
将所述检测矢量特征与过滤后的所述投影矢量特征进行图匹配，以获得所述匹配结果。
本公开的一些实施方式中，所述通过所述检测矢量特征与所述投影矢量特征之间的相似度过滤所述投影矢量特征，包括：
计算所述检测矢量特征中各个语义对象与所述投影矢量特征中各个参考对象在绝对距离上的相似性；
舍弃所述相似性小于或等于预定的相似性阈值的所述参考对象，保留所述相似性大于所述相似性阈值的参考对象，以得到过滤后的投影矢量特征。
本公开的一些实施方式中，所述将所述检测矢量特征与过滤后的所述投影矢量特征进行图匹配，以获得所述匹配结果，包括：
将所述检测矢量特征中各语义对象的矢量描述子与过滤后的所述投影矢量特征中各参考对象的投影矢量描述子进行图匹配，以获得匹配矩阵和置信度矩阵，所述匹配矩阵中的元素用于表示各个所述语义对象与各个所述参考对象之间的匹配关系，所述置信度矩阵中的元素用于表示各个所述语义对象与各个参考对象之间的匹配置信度。
本公开的一些实施方式中，所述重投影误差根据所述检测矢量特征中各个语义对象的语义标签、以及所述语义对象与过滤后的所述投影矢量特征中各个参考对象之间的形状点重投影误差、端点重投影误差和/或方向向量的重投影误差确定；其中，所述形状点重投影误差根据所述语义对象的形状角点位置、所述参考对象的形状角点位置、所述语义对象与所述参考对象的匹配关系与匹配置信度得到；和/或，所述端点重投影误差根据所述语义对象的端点位置、所述参考对象的端点位置、所述语义对象与所述参考对象的匹配关系与匹配置信度得到；和/或，所述方向向量的重投影误差根据所述语义对象的方向向量、所述参考对象的方向向量、所述语义对象与所述参考对象的匹配关系与匹配置信度得到。
本公开的一些实施方式中，所述重投影误差通过采用对数函数的预定核函数表示。
本公开的一些实施方式中，采用光束平差法最小化所述重投影误差，以优化所述第一位姿，获得所述第二位姿。
本公开的第二方面提供了一种位姿优化装置，包括：
获取模块，用于获取对应第一对象当前所处场景的第一图像和第一对象的第一位姿；
检测模块，用于获取所述第一图像的检测矢量特征；
投影模块，用于获取矢量地图中所述第一位姿的邻域范围内的投影矢量特征；
匹配模块，用于将所述检测矢量特征与所述投影矢量特征匹配，以获得匹配结果；
优化模块，用于最小化基于所述匹配结果确定的重投影误差，以优化所述第一位姿，获得所述第一对象的第二位姿。
本公开的一些实施方式中，所述第一图像的检测矢量特征包括所述第一图像中各个语义对象的矢量描述子，所述矢量描述子用于描述所述语义对象的语义标签、形状点位置、中心点位置、邻域特征位置向量和方向向量。
本公开的一些实施方式中，所述投影矢量特征包括矢量地图中所述第一位姿的邻域范围内各个参考对象在所述第一图像的图像坐标系的投影矢量描述子，所述投影矢量描述子用于描述所述参考对象在所述第一图像的图像坐标系中的语义标签、形状点位置、中心点位置、邻域特征位置向量和方向向量。
本公开的一些实施方式中，所述匹配模块，具体用于：通过所述检测矢量特征与所述投影矢量特征之间的相似度过滤所述投影矢量特征，以使得所述投影矢量特征与所述检测矢量特征在矢量特征空间分布上具有近似的一致性；以及，将所述检测矢量特征与过滤后的所述投影矢量特征进行图匹配，以获得所述匹配结果。
本公开的一些实施方式中，所述匹配模块，具体用于：将所述检测矢量特征中各语义对象的矢量描述子与过滤后的所述投影矢量特征中各参考对象的投影矢量描述子进行图匹配，以获得匹配矩阵和置信度矩阵，所述匹配矩阵中的元素用于表示各个所述语义对象与各个所述参考对象之间的匹配关系，所述置信度矩阵中的元素用于表示各个所述语义对象与各个参考对象之间的匹配置信度。
本公开的一些实施方式中，所述重投影误差根据所述检测矢量特征中各个语义对象的语义标签、以及所述语义对象与过滤后的所述投影矢量特征中各个参考对象之间的形状点重投影误差、端点重投影误差和/或方向向量的重投影误差确定；其中，所述形状点重投影误差根据所述语义对象的形状角点位置、所述参考对象的形状角点位置、所述语义对象与所述参考对象的匹配关系与匹配置信度得到；和/或，所述端点重投影误差根据所述语义对象的端点位置、所述参考对象的端点位置、所述语义对象与所述参考对象的匹配关系与匹配置信度得到；和/或，所述方向向量的重投影误差根据所述语义对象的方向向量、所述参考对象的方向向量、所述语义对象与所述参考对象的匹配关系与匹配置信度得到。
本公开的一些实施方式中，所述重投影误差通过采用对数函数的预定核函数表示。
本公开的一些实施方式中，所述优化模块，具体用于采用光束平差法最小化所述重投影误差，以优化所述第一位姿，获得所述第二位姿。
本公开的第三方面提供了一种电子设备，包括：
存储器，所述存储器存储执行指令；以及
处理器，所述处理器执行所述存储器存储的执行指令，使得所述处理器执行上述的位姿优化方法。
本公开的第四方面提供了一种可读存储介质，所述可读存储介质中存储有执行指令，所述执行指令被处理器执行时用于实现上述的位姿优化方法。
本公开具有如下有益效果：
1)结合语义矢量特征之间的绝对距离、相对距离、方向、语义、邻域特征空间分布信息，从多维度度量矢量特征的唯一性，提高矢量特征匹配的鲁棒性和精度。
2)利用语义矢量特征之间的结构相似性，包括特征的绝对距离，相对距离，方向，语义，邻域分布信息，实现语义矢量特征的匹配。相比于描述子信息，特征的结构化信息描述更鲁棒，对环境具有一定的抗干扰能力，能够提高特征匹配的准确率。同时，语义特征矢量化能够提高语义特征的鲁棒性，降低神经网络边缘检测不稳定对特征匹配的影响。
3)结合多维矢量描述子，实现轻量级语义矢量特征的空间全局一致性匹配，提高在复杂环境，以及长时环境下进行数据关联的准确性和鲁棒性，解决了复杂场景以及长时环境下数据关联失败或者错误的问题。
4)此外，本公开还利用基于矢量特征多粒度匹配和匹配置信度的重投影误差函数优化第一位姿，进一步提高了位姿优化的精度。
附图说明
附图示出了本公开的示例性实施方式，并与其说明一起用于解释本公开的原理，其中包括了这些附图以提供对本公开的进一步理解，并且附图包括在本说明书中并构成本说明书的一部分。
图1是本公开的一个实施方式的位姿优化方法的流程示意图。
图2是本公开的一个实施方式的获取检测矢量特征的流程示意图。
图3是本公开的一个实施方式的获取投影矢量特征的流程示意图。
图4是本公开的一个实施方式的检测矢量特征与投影矢量特征的匹配流程示意图。
图5是本公开的一个实施方式的最小化重投影误差的流程示意图。
图6是本公开的一个实施方式的采用处理系统的硬件实现方式的姿态优化装置的结构示意框图。
附图标记说明
600 姿态优化装置
602 获取模块
604 检测模块
606 投影模块
608 匹配模块
610 优化模块
700 总线
800 处理器
900 存储器
1000 其他电路。
具体实施方式
下面结合附图和实施方式对本公开作进一步的详细说明。可以理解的是，此处所描述的具体实施方式仅用于解释相关内容，而非对本公开的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本公开相关的部分。
需要说明的是，在不冲突的情况下，本公开中的实施方式及实施方式中的特征可以相互组合。下面将参考附图并结合实施方式来详细说明本公开的技术方案。
除非另有说明，否则示出的示例性实施方式/实施例将被理解为提供可以在实践中实施本公开的技术构思的一些方式的各种细节的示例性特征。因此，除非另有说明，否则在不脱离本公开的技术构思的情况下，各种实施方式/实施例的特征可以另外地组合、分离、互换和/或重新布置。
在附图中使用交叉影线和/或阴影通常用于使相邻部件之间的边界变得清晰。如此，除非说明，否则交叉影线或阴影的存在与否均不传达或表示对部件的具体材料、材料性质、尺寸、比例、示出的部件之间的共性和/或部件的任何其它特性、属性、性质等的任何偏好或者要求。此外，在附图中，为了清楚和/或描述性的目的，可以夸大部件的尺寸和相对尺寸。当可以不同地实施示例性实施例时，可以以不同于所描述的顺序来执行具体的工艺顺序。例如，可以基本同时执行或者以与所描述的顺序相反的顺序执行两个连续描述的工艺。此外，同样的附图标记表示同样的部件。
当一个部件被称作“在”另一部件“上”或“之上”、“连接到”或“结合到”另一部件时，该部件可以直接在所述另一部件上、直接连接到或直接结合到所述另一部件，或者可以存在中间部件。然而，当部件被称作“直接在”另一部件“上”、“直接连接到”或“直接结合到”另一部件时，不存在中间部件。为此，术语“连接”可以指物理连接、电气连接等，并且具有或不具有中间部件。
本文使用的术语是为了描述具体实施例的目的，而不意图是限制性的。如这里所使用的，除非上下文另外清楚地指出，否则单数形式“一个(种、者)”和“所述(该)”也意图包括复数形式。此外，当在本说明书中使用术语“包含”和/或“包括”以及它们的变型时，说明存在所陈述的特征、整体、步骤、操作、部件、组件和/或它们的组，但不排除存在或附加一个或更多个其它特征、整体、步骤、操作、部件、组件和/或它们的组。还要注意的是，如这里使用的，术语“基本上”、“大约”和其它类似的术语被用作近似术语而不用作程度术语，如此，它们被用来解释本领域普通技术人员将认识到的测量值、计算值和/或提供的值的固有偏差。
下面先对相关技术做简要分析说明。
相关技术1：公开号为CN111982133的中国专利公开了一种基于高精地图对车辆进行定位的方法、装置及电子设备，其中涉及一种基于高精地图的位姿优化方法，该方法通过预训练的深度学习模型预测采集图像序列的语义实例类别和边缘，然后基于当前传感器的估计位姿，构建当前场景的感兴趣语义元素的局部高精地图，并与之对应的云端高精地图进行匹配，再将匹配的语义元素边界点集使用奇异值分解(Singular ValueDecomposition,SVD)方法计算当前车辆在云端高精地图中的定位位姿。其中，基于高层语义信息的局部搜索匹配策略进行数据关联，虽然结合了高精地图的矢量信息，但是基于高精地图中车道线采样点进行匹配的方式，本质上还是基于特征点的匹配，特征的矢量化程度低，而且点匹配存在较多的噪声。此外，基于深度学习的语义边缘检测结果是不稳定的，基于边缘点集的匹配会受到边缘检测结果的影响。因此，该方案存在精度低、优化结果准确性差和鲁棒性欠佳等问题。
相关技术2：公开号为CN112085092A的中国专利公开了一种基于时空连续性约束的图匹配方法及装置，其提出了一种基于SURF(Speeded Up Robust Features)描述子的特征图匹配方法。该方案把时空连续的视频序列作为输入，通过帧间图像的单应性变换关系，得到初始的匹配特征点，再以初始的匹配关系建立关联图，将特征点的匹配优化问题转为图匹配优化问题来求解。该方案利用视频序列连续性的特点，用单应约束为图匹配的优化过程提供一个良好的初始状态，并且基于图的描述子匹配同时考虑到特征点之间的相似性和匹配点对的边构成的夹角的相似性，从而提高了图匹配的速度和准确率。虽然通过提供良好的初始值提高了图匹配的效率，但是基于描述子的特征点庞大而敏感，依然在很大程度上制约着图匹配的速度和精确度。因此，该方案存在精度低、优化结果准确性差、处理效率低、实时性差等缺陷。
相关技术3：名称为《Monocular Localization with Vector HD Map(MLVHM):ALow-Cost Method for Commercial》的论文提出了一种结合高精地图的位姿优化方法：先通过深度学习的方法获取当前场景中的语义点特征和线特征，然后利用改进随机抽样一致性算法(Random Sample Consensus,RANSAC)实现数据关联和位姿优化。该方案基于随机匹配的点对进行位姿优化，优化的好坏依赖于外点阈值和优化误差阈值的选择，而且仅依赖相同语义随机生成匹配子集，生成的子集中错误匹配比较多，会导致迭代求解的过程耗时长。因此，该方案存在精度低、结果准确性差和鲁棒性欠佳等问题。
基于高精地图的位姿优化方案中，由于高精地图的语义矢量特征没有对应的描述子信息，无法通过描述子匹配方法建立语义矢量特征之间的数据关联。因此，建立当前场景和高精地图之间的数据关联是位姿优化过程中的重要环节。对此，相关技术1中使用最近邻匹配，利用矢量特征的距离信息，将位置最相近的特征当作匹配点。而相关技术3则利用了矢量特征的语义信息，进行局部搜索匹配和RANSAC匹配。可见，相关技术1和相关技术3均未充分利用矢量语义地图所具有的属性和约束，而是仅仅基于距离或语义信息进行矢量特征匹配和数据关联，并且匹配的效果和精度依赖于先验的约束条件(例如，语义特征的鲁棒性、匹配子图的重叠率等)，因此，这两个方案存在精度低、优化结果准确性差等问题。此外，相关技术1和相关技术3都采用了深度学习的方法描述和提取语义特征，因神经网络对边缘的描述能力弱，在实际应用中容易受环境的影响，检测结果不稳定。因此，这两个方案基于深度学习的语义特征的鲁棒性也是有待改进的。相关技术2是一种基于图的特征匹配方法，旨在利用特征间的二阶相似性，提高特征匹配的准确率，但基于描述子的特征数量庞大，匹配速度缓慢，无法达到实时匹配的需求。
鉴于此，本公开提供了如下的位姿优化方法、装置、电子设备及存储介质，能够解决结合高精地图位姿优化方案中的跨时长场景的鲁棒的数据关联问题，实现了基于语义矢量特征的、多粒度匹配的位姿优化方案，定位精度高、适应性强。
下文结合图1至图6对本公开的示例性实施方式进行详细说明。
图1示出了本公开的一些实施方式中的位姿优化方法的流程示意图。参见图1所示，位姿优化方法S10可包括：
步骤S12，获取对应第一对象当前所处场景的第一图像和第一对象的第一位姿；
第一图像可以通过诸如摄像头等传感器采集得到。第一图像包含第一对象所处场景的内容，但可以不包含第一对象本身。这里，第一对象可以是诸如车辆、机器人等需要进行实时定位的物体。以车辆为例，第一图像可以是车辆的前视摄像头采集的前视图像，该前视摄像头可以是但不限于单目相机、彩色(RGB)相机、飞行时间(Time of flight，TOF)相机等。
需要说明的是，“车辆”可以是但不限于无人机、物流车、家用车、客车、地铁、高铁、飞行器、轮船等各种类型，可以是混合动力车辆、电动车辆、燃油车辆等。此外，本公开可适用于各类需要对物体进行实时定位的场景，可以适用于室内、室外和/或野外等各种环境。
需要说明的是，机器人可以是但不限于物流机器人、巡检机器人、飞行机器人、工业机器人等各种类型。
一些实施方式中，第一位姿可以是诸如里程计、全球定位系统(GlobalPositioning System，GPS)、惯性测量单元(Inertial Measurement Unit，IMU)等传感器直接得到的初始位姿，也可以是通过融合诸如里程计、GPS、惯性测量单元等多种传感器的数据而获得的位姿。此外，第一位姿可以是第一对象自身的位姿，也可以是固定安装于诸如车辆等第一对象上的传感器的位姿。
步骤S14，获取第一图像的检测矢量特征；
一些实施方式中，第一图像的检测矢量特征包括第一图像中各个语义对象的矢量描述子，矢量描述子用于描述语义对象的语义标签、形状点位置、中心点位置、邻域特征位置向量和方向向量。
一些实施方式中，步骤S14可以包括步骤S142和步骤S144：
步骤S142，对连续k帧的第一图像进行目标检测和多帧融合，以获得第一图像中各个语义对象的特征，每个语义对象的特征包括该语义对象的语义标签。
以道路场景为例，语义对象可以是但不限于树木，路桩，护栏，路灯，交通灯，电线杆，龙门架，例如指示牌、标识牌等交通标牌，例如车道线、导向指示线、停止线、人行横道等路面标识线等，语义对象的特征除包含语义标签之外，还可以包括但不限于语义对象的位置、形状、方向、邻域特征等信息中的一种或多种。
一些实施方式中，可以基于预训练的深度学习网络、目标检测网络、语义分割网络、基于视频序列的多任务网络等各种模型实现第一图像的目标检测。
一些实施方式中，步骤S142具体可以包括如下的步骤a1和步骤a2：
步骤a1，对当前场景的连续k帧第一图像进行目标检测，获得连续k帧第一图像的检测结果，连续k帧第一图像的检测结果可表示为I(I0，I1，...，Ik-1)，I0表示第0帧第一图像的目标检测结果，I1表示第1帧第一图像的目标检测结果，Ik-1表示第k-1帧第一图像的目标检测结果。这里，k为大于1的整数。
步骤a2，采用诸如DeepSORT等深度跟踪算法融合连续k帧第一图像的检测结果，获得第一图像的检测特征，第一图像的检测特征可以包括第一图像中各个语义对象的特征，第一图像的检测特征Vo可以表示为
Figure BDA0003636659770000091
n+1表示对连续k帧第一图像进行跟踪检测得到的语义对象的数量，
Figure BDA0003636659770000092
表示第0个语义对象的特征，
Figure BDA0003636659770000093
表示第1个语义对象的特征，……，
Figure BDA0003636659770000094
表示第n个语义对象的特征。
步骤S144，根据语义对象的语义标签对各个语义对象的特征进行矢量描述，以获得各个语义对象的矢量描述子，从而形成第一图像的检测矢量特征，第一图像的检测矢量特征可以包括连续k帧的第一图像目标检测和多帧融合后得到的所有语义对象的矢量描述子。
特征的矢量描述是指对特征进行位置、形状、邻域特征位置、语义等形式的描述。对于第一图像的检测特征Vo中的任意一个语义对象
Figure BDA0003636659770000095
Figure BDA00036366597700000929
它的矢量描述子
Figure BDA0003636659770000096
根据语义标签的不同描述成不同的向量。
一些实施方式中，对于例如路灯、杆、停止线等语义标签为直线类型的语义对象
Figure BDA0003636659770000097
其矢量描述子
Figure BDA0003636659770000098
表示为形状点位置向量
Figure BDA0003636659770000099
中心点位置
Figure BDA00036366597700000910
邻域特征位置向量
Figure BDA00036366597700000911
和语义标签
Figure BDA00036366597700000912
其中，
Figure BDA00036366597700000913
为原点，半径为r1的邻域范围内u个邻域特征位置，其中，
Figure BDA00036366597700000914
表示
Figure BDA00036366597700000915
的第u个邻域特征的中心点位置，
Figure BDA00036366597700000916
表示起始点的位置，
Figure BDA00036366597700000917
表示终点的位置。
一些实施方式中，对于例如引导标志线等语义标签为箭头类型的语义对象
Figure BDA00036366597700000918
其矢量描述子
Figure BDA00036366597700000919
表示为形状点位置向量
Figure BDA00036366597700000920
中心点位置
Figure BDA00036366597700000921
邻域特征位置向量
Figure BDA00036366597700000922
箭头的方向向量
Figure BDA00036366597700000923
和语义标签
Figure BDA00036366597700000924
其中，
Figure BDA00036366597700000925
表示
Figure BDA00036366597700000926
的p个角点位置，
Figure BDA00036366597700000927
表示
Figure BDA00036366597700000928
的第p个角点的位置。
一些实施方式中，对于例如标识牌、交通灯等语义标签为其他类型的语义对象
Figure BDA0003636659770000101
其矢量描述子
Figure BDA0003636659770000102
表示为形状点位置
Figure BDA0003636659770000103
中心点位置
Figure BDA0003636659770000104
邻域特征位置向量
Figure BDA0003636659770000105
和语义标签
Figure BDA0003636659770000106
换言之，可以根据下式(1)各个语义对象的矢量描述子，以获得第一图像的检测矢量特征：
Figure BDA0003636659770000107
其中，
Figure BDA0003636659770000108
表示语义对象
Figure BDA0003636659770000109
的矢量描述子，
Figure BDA00036366597700001010
表示语义对象
Figure BDA00036366597700001011
的起始点位置，
Figure BDA00036366597700001012
表示语义对象
Figure BDA00036366597700001013
的终点位置，
Figure BDA00036366597700001014
表示语义对象
Figure BDA00036366597700001015
的形状点位置向量，
Figure BDA00036366597700001016
表示
Figure BDA00036366597700001017
的第p个角点的位置，
Figure BDA00036366597700001018
表示语义对象
Figure BDA00036366597700001019
的中心点位置，
Figure BDA00036366597700001020
表示语义对象
Figure BDA00036366597700001021
的邻域特征位置向量，其中，
Figure BDA00036366597700001022
表示
Figure BDA00036366597700001023
为原点、半径为r1的邻域范围内第u个邻域特征的中心点位置，
Figure BDA00036366597700001024
表示语义对象
Figure BDA00036366597700001025
的箭头方向向量，
Figure BDA00036366597700001026
表示语义对象
Figure BDA00036366597700001027
的语义标签。
步骤S16，获取矢量地图中第一位姿的邻域范围内的投影矢量特征；
一些实施方式中，投影矢量特征包括矢量地图中第一位姿的邻域范围内各个参考对象在第一图像的图像坐标系的投影矢量描述子，投影矢量描述子用于描述参考对象在第一图像的图像坐标系中的语义标签、形状点位置、中心点位置、邻域特征位置向量和方向向量。
一些实施方式中，矢量地图中存储有场景中参考对象的空间位置、形状、方向、语义标签等信息。以道路场景为例，矢量地图中的参考对象可以包括但不限于树木，路桩，护栏，路灯，交通灯，电线杆，龙门架，例如指示牌和/或标识牌等交通标牌，例如车道线、导向标、停止线、人行横道等交通标识线。具体应用中，矢量地图可以是但不限于高精地图、人工或自动构建的轻量级矢量语义地图等。
一些实施方式中，步骤S16可以包括如下的步骤S162～步骤S166：
步骤S162，从矢量地图中解析得到第一位姿的邻域范围内(即，第一对象当前所处的场景)各个参考对象的矢量特征；
具体地，根据第一位姿，搜索矢量地图中该第一位姿邻域范围内参考对象的特征，得到与当前k帧第一图像对应的参考对象特征集合Xh，参考对象特征集合Xh表示为
Figure BDA00036366597700001028
m+1表示参考对象的数量，
Figure BDA00036366597700001029
表示第0个参考对象的特征，
Figure BDA00036366597700001030
表示第1个参考对象的特征，
Figure BDA00036366597700001031
表示第2个参考对象的特征，
Figure BDA00036366597700001032
表示第i个参考对象的特征，
Figure BDA00036366597700001033
表示第m个参考对象的特征，每个参考对象的特征可以包含该参考对象的空间位置、形状、方向和语义标签等信息。
这里，邻域范围可以根据应用场景的不同、实际应用的需求预先设置。例如，邻域范围可以是设置为以第一位姿为中心的固定大小的圆形区域或矩形区域。通常，邻域范围可以覆盖当前场景。以车辆为例，其当前场景可以是车辆前视场景，车辆的邻域范围可以包含车辆的前视区域。
步骤S164，将各个参考对象的特征投影至第一图像的图像坐标系，以获得各个参考对象的投影矢量特征；
具体地，对于参考对象特征集合Xh中的任意一个参考对象
Figure BDA0003636659770000111
Figure BDA0003636659770000112
可以根据对应第一图像的相机内外参将参考对象的位置特征(即，形状点位置、中心点位置)投影到第一图像的图像坐标系下，获得该参考对象
Figure BDA0003636659770000113
在第一图像的图像坐标系中的投影对象
Figure BDA0003636659770000114
的特征。
例如，任意参考对象
Figure BDA0003636659770000115
在高精地图上的中心点位置
Figure BDA0003636659770000116
可以通过下式(2)实现上述投影。形状点位置的投影与中心点投影类似。
Figure BDA0003636659770000117
其中，
Figure BDA0003636659770000118
表示
Figure BDA0003636659770000119
在图像上的投影点位置，Rj、tj表示参考对象
Figure BDA00036366597700001110
在世界坐标系下的位姿，Rext，text表示对应第一图像的相机外参矩阵(即，采集第一图像的传感器的外参矩阵，可通过相机标定获得)，也即世界坐标系到相机坐标系的转换矩阵。K表示对应第一图像的相机内参(即，采集第一图像的传感器的内参)。
由此，可以获得投影特征集合Vh，表示为
Figure BDA00036366597700001111
Figure BDA00036366597700001112
表示第0个参考对象的投影对象的特征，
Figure BDA00036366597700001113
表示第1个参考对象的投影对象的特征，
Figure BDA00036366597700001114
表示第m个参考对象的投影对象的特征。
步骤S166，对各个参考对象的投影矢量特征进行矢量描述，以获得各个参考对象的投影矢量描述子。
与步骤S144相似，可以按照下式(3)获得各个参考对象的投影矢量描述子：
Figure BDA00036366597700001115
其中，
Figure BDA00036366597700001116
表示参考对象
Figure BDA00036366597700001117
的投影对象
Figure BDA00036366597700001118
的投影矢量描述子，
Figure BDA00036366597700001119
表示参考对象
Figure BDA00036366597700001120
的投影对象
Figure BDA00036366597700001121
的起点位置，即参考对象
Figure BDA00036366597700001122
的起点重投影点位置，
Figure BDA00036366597700001123
表示参考对象
Figure BDA00036366597700001124
的投影对象
Figure BDA00036366597700001125
的终点位置，即参考对象
Figure BDA00036366597700001126
的终点重投影点位置，
Figure BDA00036366597700001127
表示参考对象
Figure BDA00036366597700001128
的投影对象
Figure BDA0003636659770000121
的形状点位置向量，即参考对象
Figure BDA0003636659770000122
的形状点重投影点位置，
Figure BDA0003636659770000123
表示
Figure BDA0003636659770000124
的第q个角点的位置，
Figure BDA0003636659770000125
表示参考对象
Figure BDA0003636659770000126
的投影对象
Figure BDA0003636659770000127
的中心点位置，即参考对象
Figure BDA0003636659770000128
的中心重投影点位置，
Figure BDA0003636659770000129
表示参考对象
Figure BDA00036366597700001210
的投影对象
Figure BDA00036366597700001211
的方向向量，即参考对象
Figure BDA00036366597700001212
的重投影方向向量，
Figure BDA00036366597700001213
表示参考对象
Figure BDA00036366597700001214
的投影对象
Figure BDA00036366597700001215
的邻域特征位置向量，其中，
Figure BDA00036366597700001216
表示
Figure BDA00036366597700001217
为原点、半径为r1的邻域范围内，第u个邻域特征的中心点位置，
Figure BDA00036366597700001218
表示参考对象
Figure BDA00036366597700001219
的投影对象
Figure BDA00036366597700001220
的语义标签。
步骤S18，将检测矢量特征与投影矢量特征匹配，以获得匹配结果；
一些实施方式中，步骤S18可以包括步骤S182和步骤S184：
步骤S182，粗粒度匹配：通过检测矢量特征与投影矢量特征之间的相似度过滤投影矢量特征。具体地，以检测矢量特征的中心点为原点，半径为
Figure BDA00036366597700001221
的邻域范围内，计算检测矢量特征与邻域内投影矢量特征的相似性，过滤投影矢量特征中的冗余部分，使得投影矢量特征与检测矢量特征在矢量特征空间分布上具有近似的一致性。
一些实施方式中，步骤S182可以包括如下的步骤b1和步骤b2：
步骤b1，计算检测矢量特征Vo中每个语义对象与r2邻域范围内参考对象在绝对距离上的相似性。
一些实施方式中，对于任意语义对象，即
Figure BDA00036366597700001222
可以按照下式(4)计算该语义对象
Figure BDA00036366597700001223
与r2邻域范围内参考对象的投影对象
Figure BDA00036366597700001224
的相似性
Figure BDA00036366597700001225
Figure BDA00036366597700001226
其中，
Figure BDA00036366597700001227
表示欧式空间下语义对象
Figure BDA00036366597700001228
和参考对象
Figure BDA00036366597700001229
的投影对象
Figure BDA00036366597700001230
的中心点距离差值。
步骤b2，根据步骤b1得到的相似性和预先设定的相似性阈值对投影矢量特征进行过滤，即，舍弃相似性小于或等于预定的相似性阈值的参考对象，保留相似性大于相似性阈值的参考对象，以得到过滤后的投影矢量特征。
具体地，对于任意的投影对象
Figure BDA00036366597700001231
如果能够检测到与该投影对象对应的语义对象，则邻域范围r2内一定存在语义对象
Figure BDA00036366597700001232
该语义对象
Figure BDA00036366597700001233
和投影对象
Figure BDA00036366597700001234
是匹配对，在空间位置上很接近，甚至可能重叠在一起。也即，语义对象
Figure BDA00036366597700001235
和投影对象
Figure BDA00036366597700001236
之间的相似性误差值应该是很小的。如果语义对象
Figure BDA00036366597700001237
和投影对象
Figure BDA00036366597700001238
的相似性误差值很大，则可以认为两者是不匹配的。因此，可以预先设置相似性阈值，通过相似性阈值过滤掉投影矢量特征中与语义对象不匹配的部分，由此，可以舍弃投影矢量特征中因遮挡、光照等因素导致错检或漏检的语义对象对应的参考对象，使检测矢量特征与投影矢量特征在空间分布上具有近似的一致性。
具体地，过滤后的投影矢量特征
Figure BDA0003636659770000131
可表示为下式(5)：
Figure BDA0003636659770000132
其中，m′表示过滤后投影矢量特征的数量，
Figure BDA0003636659770000133
表示语义对象
Figure BDA0003636659770000134
与邻域范围r2内参考对象
Figure BDA0003636659770000135
的投影对象
Figure BDA0003636659770000136
的相似性，τ表示相似性阈值，过滤后的Vh′和Vo在空间分布上具有近似的一致性。
步骤S184，细粒度匹配：利用矢量特征的矢量描述子，将检测矢量特征Vo与过滤后的投影矢量特征Vh′进行图匹配，获得匹配结果。由此，可结合矢量特征之间的结构化信息，即距离、方向、语义、邻域分布信息等进行细粒度特征匹配，得到矢量特征之间的一一对应的匹配对。
具体地，将检测矢量特征中各语义对象的矢量描述子与过滤后的投影矢量特征中各参考对象的投影矢量描述子进行图匹配，以获得匹配矩阵和置信度矩阵，匹配矩阵中的元素用于表示各个语义对象与各个参考对象之间的匹配关系，置信度矩阵中的元素用于表示各个语义对象与各个参考对象之间的匹配置信度。由于矢量描述子包含了矢量特征的语义标签、方向、位置、以及邻域特征等结构性信息，因此，通过矢量图匹配，可以得到检测矢量特征中语义对象与投影矢量特征中参考对象之间的一一对应关系。
一些实施方式中，步骤S184可以包括如下的步骤c1～步骤c4：
步骤c1，建立检测矢量特征Vo的有向图Go和投影矢量特征Vh′的有向图Gh′；
具体地，基于检测矢量特征Vo建立对应的有向图Go＝{Vo，Eo，Wo}，Vo表示图Go的节点信息，即把每个语义对象当作图Go的一个节点；Eo表示图Go的边信息，Eo包括图Go中任意两个节点之间的边信息
Figure BDA0003636659770000137
Wo表示图Go的权重信息，Wo包括图Go中任意两个节点之间边的权重
Figure BDA0003636659770000138
Figure BDA0003636659770000139
表示两个节点之间的亲密度(即，图的一阶相似性)。
对于
Figure BDA00036366597700001310
如果节点
Figure BDA00036366597700001311
和节点
Figure BDA00036366597700001312
之间存在边，则对应的
Figure BDA00036366597700001313
否则
Figure BDA00036366597700001314
对于
Figure BDA00036366597700001315
其对应的权重
Figure BDA00036366597700001316
表示节点
Figure BDA00036366597700001317
和
Figure BDA00036366597700001318
之间的亲密度。
Figure BDA00036366597700001319
可表示为一个多维向量，如下式(6)所示，其中，
Figure BDA00036366597700001320
分别表示节点
Figure BDA00036366597700001321
和
Figure BDA00036366597700001322
的语义标签，
Figure BDA00036366597700001323
分别表示节点
Figure BDA00036366597700001324
和
Figure BDA00036366597700001325
的中心点位置，
Figure BDA00036366597700001326
分别表示节点
Figure BDA00036366597700001327
和
Figure BDA00036366597700001328
的邻域特征位置向量。
Figure BDA00036366597700001329
按照上述原理，可以同样基于投影矢量特征Vh′建立对应的有向图Gh′＝{Vh′，Eh′，Wh′}，Vh′表示图Gh′的节点信息，Eh′表示图Gh′的边信息，Wh′表示图Gh′的权重信息。
步骤c2，根据有向图Go和有向图Gh′，建立对应的关联图Ga＝{Va，Ea，Wa}；
其中，Va表示图Ga的节点信息，
Figure BDA0003636659770000141
Figure BDA0003636659770000142
表示有向图Go的节点
Figure BDA0003636659770000143
和有向图Gh′的节点
Figure BDA0003636659770000144
共同构成的一个节点。
Ea表示图Ga的边信息，
Figure BDA0003636659770000145
对于任意的
Figure BDA0003636659770000146
Figure BDA0003636659770000147
与
Figure BDA0003636659770000148
之间的边
Figure BDA0003636659770000149
表示图Go的边
Figure BDA00036366597700001410
和图Gh′的边
Figure BDA00036366597700001411
之间二阶边的连接关系。
Wa表示图Ga的权重信息，
Figure BDA00036366597700001412
对于任意的
Figure BDA00036366597700001413
对应的权重
Figure BDA00036366597700001414
表示了图Ga的节点
Figure BDA00036366597700001415
和节点
Figure BDA00036366597700001416
的亲密度
Figure BDA00036366597700001417
(即，子图Go的边
Figure BDA00036366597700001418
和子图图Gh′的边
Figure BDA00036366597700001419
之间的相似性，也就是图的二阶相似性)。
其中，亲密度
Figure BDA00036366597700001420
可以通过下式(7)～(9)得到：
Figure BDA00036366597700001421
Figure BDA00036366597700001422
Figure BDA00036366597700001423
其中，⊙表示同或运算，当⊙左右的值相等时为1，反之为0。
Figure BDA00036366597700001424
表示了图Ga的节点
Figure BDA00036366597700001425
和节点
Figure BDA00036366597700001426
之间的亲密度，也就是子图Go的边
Figure BDA00036366597700001427
和子图Gh′的边
Figure BDA00036366597700001428
之间的相似性。ξij：pq表示亲密度的邻域权重参数，当边的邻域分布不相似的时候，ξij：pq很小，当边的邻域分布很接近的时候，ξij：pq很大。w1表示一阶相似性的权重参数，w2表示二阶相似性的权重参数，w1和w2可通过离线学习得到。其中，a,b∈{1,2,…,v}，p＝{0,1,2,…,m′}，q＝{0,1,2,…,m′}。
式(7)～(9)中，当ij＝pq时，
Figure BDA00036366597700001429
描述节点与节点的相似性(即，图的一阶相似性)；当ij≠pq时，
Figure BDA00036366597700001430
描述边与边的相似性(即，图的二阶相似性)。通过式(7)～(9)可见，本公开将图的一阶相似性置为零，并将一阶相似性的度量融合到图的二阶相似性，同时结合绝对距离、相对距离、方向、邻域特征空间分布关系来度量图的二阶相似度。
步骤c3，依据关联图Ga的权重信息Wa，构建一个加权亲密度矩阵Wnm‘×nm’。Wnm‘×nm’的行表示为图Ga的节点
Figure BDA00036366597700001431
列表示图Ga的节点
Figure BDA00036366597700001432
对应的元素为公式(7)计算得到的权重
Figure BDA00036366597700001433
接下来，将图Go和图Gh′的匹配问题转化为Wnm‘×nm’亲密度矩阵的二次指派问题(Quadratic Assignment Problem,QAP)，在约束条件下寻找最优指派矩阵的问题可以表述为式(10)～(11)，用指派矩阵Q∈{0,1}nm′×1表示该问题的解，对于qij∈Q，如果qij＝1，表示图Go的节点
Figure BDA0003636659770000151
和图Gh′的节点
Figure BDA0003636659770000152
是匹配对，也就是说，语义对象
Figure BDA0003636659770000153
和投影对象
Figure BDA0003636659770000154
是一对匹配对象。反之，qij＝0。
Q＝argmax(QTWnm′×nm′Q) (10)
Figure BDA0003636659770000155
步骤c4，采用加权随机游走图匹配算法(Reweighted Random Walks for GraphMatching,RRWM)实现问题的求解，该将寻求最优的指派矩阵的问题近似为一个连续的优化问题，优化方程如式(12)所示。
Q′＝argmax(Q′TWnm×nmQ′)，s.t.Q′∈[0，1]nm×1 (12)
由此，经过多次迭代，指派矩阵Q′中的元素都趋于一个稳定的概率值，然后基于公式(10)～(11)中的约束，对趋于稳定的指派矩阵Q′进行离散化映射，得到离散的指派矩阵Q，即为图Go和图Gh′的匹配结果。对于
Figure BDA0003636659770000156
Q′中的元素q′ij可用于描述节点
Figure BDA0003636659770000157
和节点
Figure BDA0003636659770000158
的匹配置信度。
这里，矩阵Q中包含指示图Go中各个节点和图Gh′中各个节点之间匹配关系的元素qij，如果图Go中第i个节点和图Gh′中第j个节点匹配，qij＝1，如果图Go中第i个节点和图Gh′中第j个节点不匹配，qij＝0。
在没有先验约束条件下，指派矩阵Q′初始化为均匀概率的稠密图，而本公开结合深度学习方法，能够获取Q′对应节点的语义信息，并以语义信息作为约束，将指派矩阵初始化为稀疏的概率图，提高了计算的效率，同时提高了匹配的准确度。
步骤S110，最小化基于匹配结果确定的重投影误差，以优化第一位姿，获得第一对象的第二位姿。
一些实施方式中，重投影误差可以根据检测矢量特征中各个语义对象的语义标签、以及语义对象与过滤后的投影矢量特征中各个参考对象之间的形状点重投影误差、端点重投影误差和/或方向向量的重投影误差确定。
一些实施方式中，形状点重投影误差可以根据语义对象的形状角点位置、参考对象的形状角点点位置、语义对象与参考对象的匹配关系与匹配置信度得到。
一些实施方式中，端点重投影误差可以根据语义对象的端点位置、所述参考对象的端点位置、所述语义对象与所述参考对象的匹配关系与匹配置信度得到。
一些实施方式中，方向向量的重投影误差可以根据语义对象的方向向量、参考对象的方向向量、语义对象与参考对象的匹配关系与匹配置信度得到。
一些实施方式中，步骤S110可以包括如下步骤S112和S114：
步骤S112，根据语义标签和匹配结果(即，匹配矩阵和置信度矩阵)，确定检测矢量特征与投影矢量特征在第一图像的图像坐标系下的重投影误差表达式E(Vo，Vh)。
设检测矢量特征表示为序列
Figure BDA0003636659770000161
投影矢量特征表示为序列
Figure BDA0003636659770000162
两者之间的匹配结果包括前文的匹配矩阵Q和对应的置信度矩阵Q′，一些实施方式中，Vo中的任意语义对象
Figure BDA0003636659770000163
与Vh′中的任意投影对象
Figure BDA0003636659770000164
在第一图像的图像坐标系下的形状角点投影误差eij可以通过下式(13)得到：
Figure BDA0003636659770000165
其中，qij表示语义对象
Figure BDA0003636659770000166
和投影对象
Figure BDA0003636659770000167
的匹配关系，q′ij表示语义对象
Figure BDA0003636659770000168
和投影对象
Figure BDA0003636659770000169
的匹配置信度，h(v,u)表示匹配的语义对象
Figure BDA00036366597700001610
和投影对象
Figure BDA00036366597700001611
的角点的对应关系，如果h(v,u)＝1表示
Figure BDA00036366597700001612
和
Figure BDA00036366597700001613
是对应的角点，如果h(v,u)＝0，表示
Figure BDA00036366597700001614
和
Figure BDA00036366597700001615
不是对应的角点。
Figure BDA00036366597700001616
和
Figure BDA00036366597700001617
之间的角点对应关系可以通过像素坐标系下角点坐标大小排序进行推理得到。
Figure BDA00036366597700001618
表示语义对象
Figure BDA00036366597700001619
和投影对象
Figure BDA00036366597700001620
的形状角点投影误差。
一些实施方式中，对于例如路灯、杆、停止线等语义标签为直线类型或线段类型的语义对象
Figure BDA00036366597700001621
考虑线段诸如起点和/或终点等端点的重投影误差
Figure BDA00036366597700001622
例如，可以根据下式(14)计算此类语义对象在第一图像的图像坐标系下的端点重投影误差
Figure BDA00036366597700001623
Figure BDA00036366597700001624
其中，
Figure BDA00036366597700001625
表示语义对象
Figure BDA00036366597700001626
和投影对象
Figure BDA00036366597700001627
的起点投影误差，
Figure BDA00036366597700001628
表示语义对象
Figure BDA00036366597700001629
和投影对象
Figure BDA00036366597700001630
的终点投影误差。
一些实施方式中，对于例如引导标志线等语义标签为箭头类型的语义对象
Figure BDA00036366597700001631
不仅考虑形状角点的重投影误差eij，同时还可以考虑箭头方向向量的重投影方向误差
Figure BDA00036366597700001632
作为位姿优化的变量。
例如，方向向量的重投影误差
Figure BDA00036366597700001633
可以通过下式(15)得到：
Figure BDA00036366597700001634
其中，
Figure BDA00036366597700001635
表示语义对象与参考对象的方向向量夹角，两个向量的误差越小，θ越小。
若语义对象
Figure BDA00036366597700001636
和投影对象
Figure BDA00036366597700001637
不匹配，对应的重投影误差的取值将会很大，因此，一些实施方式，可通过采用对数函数的预定核函数表示检测矢量特征与投影矢量特征之间的重投影误差，由此，可抑制误差的极大值，减少噪声干扰，提升计算效率。
一些实施方式中，检测矢量特征Vo与投影矢量特征Vh之间的重投影误差E(Vo，Vh)可表示为下式(16)：
Figure BDA0003636659770000171
其中，λ1、λ2和λ3分别表示当前的位姿优化对三种重投影误差，即形状角点重投影误差eij、端点重投影误差
Figure BDA0003636659770000172
方向向量的重投影误差
Figure BDA0003636659770000173
的依赖程度，可通过离线学习得到。
在漏检或者错检的情况下，检测矢量特征Vo和投影矢量特征Vh的空间分布不一致，因此，在尽可能地保证全局一致性匹配的过程中，会错误地产生一些不对等的匹配对。本公开的实施方式中，引入了鲁棒核函数，降低错误匹配对位姿优化带来的干扰和影响。
步骤S114，采用例如光束平差法或其他类似算法求解重投影误差表达式的最小值以优化第一位姿，从而获得第一对象的第二位姿。
根据式(2)可知，重投影误差的大小取决于第一位姿的准确性。理想情况下，即第一位姿与真实的位姿一致时，当前参考对象Xi投影到第一图像的图像坐标系的投影矢量特征和检测矢量特征应该是完全重叠的，即重投影误差E(Vo，Vh)为0。因此，可通过最小化重投影误差来优化第一位姿，获得更接近真实位姿的第二位姿。具体地，可以表示为下式(17)：
Figure BDA0003636659770000174
其中，
Figure BDA0003636659770000175
表示第二位姿，E(Vo，Vh)表示检测矢量特征Vo与投影矢量特征Vh之间的重投影误差，“argmin”表示最小化运算。
一些实施方式中，可以采用光束平差法(Bundle Adjustment)最小化重投影误差，以优化第一位姿，获得较为精确的第二位姿。
由式(15)～(16)可见，本公开可根据多粒度匹配生成的匹配对以及对应的匹配置信度，基于非线性优化的思想，通过融合不同语义对象的多层级重投影误差，联合优化位置误差和方向误差，以第一位姿为初始值，优化投影矢量特征和检测矢量特征的误差函数，从而计算得到第一对象的精确位姿，也即，第二位姿。
本公开的上述位姿优化方法具有如下有益效果：
1)结合语义矢量特征之间的绝对距离、相对距离、方向、语义、邻域特征空间分布信息，从多维度度量矢量特征的唯一性，提高矢量特征匹配的鲁棒性和精度。
2)利用语义矢量特征之间的结构相似性，包括特征的绝对距离，相对距离，方向，语义，邻域分布信息，实现语义矢量特征的匹配。相比于描述子信息，特征的结构化信息描述更鲁棒，对环境具有一定的抗干扰能力，能够提高特征匹配的准确率。同时，语义特征矢量化能够提高语义特征的鲁棒性，降低神经网络边缘检测不稳定对特征匹配的影响。
3)结合多维矢量描述子，实现轻量级语义矢量特征的空间全局一致性匹配，提高在复杂环境，以及长时环境下进行数据关联的准确性和鲁棒性，解决了复杂场景以及跨时长场景下数据关联失败或者错误的问题。
4)此外，本公开还利用基于矢量特征多粒度匹配和匹配置信度的重投影误差函数优化第一位姿，进一步提高了位姿优化的精度。
图6是本公开的一个实施方式的采用处理系统的硬件实现方式的姿态优化装置的结构示意框图。
参见图6所示，位姿优化装置600可以包括：
获取模块602，用于获取对应第一对象当前所处场景的第一图像和第一对象的第一位姿；
检测模块604，用于获取所述第一图像的检测矢量特征；
投影模块606，用于获取矢量地图中所述第一位姿的邻域范围内的投影矢量特征；
匹配模块608，用于将所述检测矢量特征与所述投影矢量特征匹配，以获得匹配结果；
优化模块610，用于最小化基于所述匹配结果确定的重投影误差，以优化所述第一位姿，获得所述第一对象的第二位姿。
一些实施方式中，所述第一图像的检测矢量特征包括所述第一图像中各个语义对象的矢量描述子，所述矢量描述子用于描述所述语义对象的语义标签、形状点位置、中心点位置、邻域特征位置向量和方向向量。
一些实施方式中，所述投影矢量特征包括矢量地图中所述第一位姿的邻域范围内各个参考对象在所述第一图像的图像坐标系的投影矢量描述子，所述投影矢量描述子用于描述所述参考对象在所述第一图像的图像坐标系中的语义标签、形状点位置、中心点位置、邻域特征位置向量和方向向量。
一些实施方式中，匹配模块608具体用于：通过所述检测矢量特征与所述投影矢量特征之间的相似度过滤所述投影矢量特征，以使得所述投影矢量特征与所述检测矢量特征在矢量特征空间分布上具有近似的一致性；以及，将所述检测矢量特征与过滤后的所述投影矢量特征进行图匹配，以获得所述匹配结果。
一些实施方式中，匹配模块608具体可用于：将所述检测矢量特征中各语义对象的矢量描述子与过滤后的所述投影矢量特征中各参考对象的投影矢量描述子进行图匹配，以获得匹配矩阵和置信度矩阵，所述匹配矩阵中的元素用于表示各个所述语义对象与各个所述参考对象之间的匹配关系，所述置信度矩阵中的元素用于表示各个所述语义对象与各个参考对象之间的匹配置信度。
一些实施方式中，重投影误差根据所述检测矢量特征中各个语义对象的语义标签、以及语义对象与过滤后的所述投影矢量特征中各个参考对象之间的形状点重投影误差、端点重投影误差和/或方向向量的重投影误差确定；其中，所述形状点重投影误差根据所述语义对象的形状角点位置、所述参考对象的形状角点位置、所述语义对象与所述参考对象的匹配关系与匹配置信度得到；和/或，所述端点重投影误差根据所述语义对象的端点位置、所述参考对象的端点位置、所述语义对象与所述参考对象的匹配关系与匹配置信度得到；和/或，所述方向向量的重投影误差根据所述语义对象的方向向量、所述参考对象的方向向量、所述语义对象与所述参考对象的匹配关系与匹配置信度得到。
一些实施方式中，重投影误差通过采用对数函数的预定核函数表示。
一些实施方式中，优化模块610具体可用于采用光束平差法最小化所述重投影误差，以优化所述第一位姿，获得所述第二位姿。
姿态优化装置600可以包括执行上述流程图中各个或几个步骤的相应模块。因此，可以由相应模块执行上述流程图中的每个步骤或几个步骤，并且该装置可以包括这些模块中的一个或多个模块。模块可以是专门被配置为执行相应步骤的一个或多个硬件模块、或者由被配置为执行相应步骤的处理器来实现、或者存储在计算机可读介质内用于由处理器来实现、或者通过某种组合来实现。
该硬件结构可以利用总线架构来实现。总线架构可以包括任何数量的互连总线和桥接器，这取决于硬件的特定应用和总体设计约束。总线700将包括一个或多个处理器800、存储器900和/或硬件模块的各种电路连接到一起。总线700还可以将诸如外围设备、电压调节器、功率管理电路、外部天线等的各种其他电路1000连接。
总线700可以是工业标准体系结构(ISA，Industry Standard Architecture)总线、外部设备互连(PCI，Peripheral Component)总线或扩展工业标准体系结构(EISA，Extended Industry Standard Component)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示，该图中仅用一条连接线表示，但并不表示仅有一根总线或一种类型的总线。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本公开的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本公开的实施方式所属技术领域的技术人员所理解。处理器执行上文所描述的各个方法和处理。例如，本公开中的方法实施方式可以被实现为软件程序，其被有形地包含于机器可读介质，例如存储器。在一些实施方式中，软件程序的部分或者全部可以经由存储器和/或通信接口而被载入和/或安装。当软件程序加载到存储器并由处理器执行时，可以执行上文描述的方法中的一个或多个步骤。备选地，在其他实施方式中，处理器可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行上述方法之一。
在流程图中表示或在此以其他方式描述的逻辑和/或步骤，可以具体实现在任何可读存储介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。
就本说明书而言，“可读存储介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。可读存储介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式只读存储器(CDROM)。另外，可读存储介质甚至可以是可在其上打印程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得程序，然后将其存储在存储器中。
应当理解，本公开的各部分可以用硬件、软件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。
本技术领域的普通技术人员可以理解实现上述实施方式方法的全部或部分步骤是可以通过程序来指令相关的硬件完成，程序可以存储于一种可读存储介质中，该程序在执行时，包括方法实施方式的步骤之一或其组合。
此外，在本公开各个实施方式中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个可读存储介质中。存储介质可以是只读存储器，磁盘或光盘等。
本公开还提供了一种电子设备，包括：存储器，存储器存储执行指令；以及处理器或其他硬件模块，处理器或其他硬件模块执行存储器存储的执行指令，使得处理器或其他硬件模块执行上述的姿态优化方法。
本公开还提供了一种可读存储介质，可读存储介质中存储有执行指令，所述执行指令被处理器执行时用于实现上述的姿态优化方法。
在本说明书的描述中，参考术语“一个实施方式/方式”、“一些实施方式/方式”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施方式/方式或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施方式/方式或示例中。在本说明书中，对上述术语的示意性表述不必须的是相同的实施方式/方式或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施方式/方式或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施方式/方式或示例以及不同实施方式/方式或示例的特征进行结合和组合。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
本领域的技术人员应当理解，上述实施方式仅仅是为了清楚地说明本公开，而并非是对本公开的范围进行限定。对于所属领域的技术人员而言，在上述公开的基础上还可以做出其它变化或变型，并且这些变化或变型仍处于本公开的范围内。

Claims (10)

1.一种位姿优化方法，其特征在于，包括：
获取对应第一对象当前所处场景的第一图像和第一对象的第一位姿；
获取所述第一图像的检测矢量特征；
获取矢量地图中所述第一位姿的邻域范围内的投影矢量特征；
将所述检测矢量特征与所述投影矢量特征匹配，以获得匹配结果；
最小化基于所述匹配结果确定的重投影误差，以优化所述第一位姿，获得所述第一对象的第二位姿；
优选地，所述第一图像的检测矢量特征包括所述第一图像中各个语义对象的矢量描述子，所述矢量描述子用于描述所述语义对象的语义标签、形状点位置、中心点位置、邻域特征位置向量和方向向量。
2.根据权利要求1所述的位姿优化方法，其特征在于，所述获取所述第一图像的检测矢量特征，包括：
对连续k帧的所述第一图像进行目标检测和多帧融合，以获得第一图像中各个语义对象的特征，所述语义对象的特征少包括所述语义对象的语义标签，k为大于1的整数；
根据所述语义对象的语义标签对各个所述语义对象的特征进行矢量描述，以获得各个所述语义对象的矢量描述子，从而形成所述第一图像的检测矢量特征；
优选地，所述投影矢量特征包括矢量地图中所述第一位姿的邻域范围内各个参考对象在所述第一图像的图像坐标系的投影矢量描述子，所述投影矢量描述子用于描述所述参考对象在所述第一图像的图像坐标系中的语义标签、形状点位置、中心点位置、邻域特征位置向量和方向向量；
优选地，所述获取矢量地图中所述第一位姿的邻域范围内的投影矢量特征，包括：从矢量地图中解析得到第一位姿的邻域范围内各个参考对象的矢量特征；将所述各个参考对象的矢量特征投影至第一图像的图像坐标系，以获得各个参考对象的投影矢量特征；对各个参考对象的投影矢量特征进行矢量描述，以获得各个参考对象的投影矢量描述子。
3.根据权利要求1或2所述的位姿优化方法，其特征在于，所述将所述检测矢量特征与所述投影矢量特征匹配，以获得匹配结果，包括：
通过所述检测矢量特征与所述投影矢量特征之间的相似度过滤所述投影矢量特征，以使得所述投影矢量特征与所述检测矢量特征在矢量特征空间分布上具有近似的一致性；以及，将所述检测矢量特征与过滤后的所述投影矢量特征进行图匹配，以获得所述匹配结果；
优选地，所述通过所述检测矢量特征与所述投影矢量特征之间的相似度过滤所述投影矢量特征，包括：计算所述检测矢量特征中各个语义对象与所述投影矢量特征中各个参考对象在绝对距离上的相似性；舍弃所述相似性小于或等于预定的相似性阈值的所述参考对象，保留所述相似性大于所述相似性阈值的参考对象，以得到过滤后的投影矢量特征；
优选地，所述将所述检测矢量特征与过滤后的所述投影矢量特征进行图匹配，以获得所述匹配结果，包括：将所述检测矢量特征中各语义对象的矢量描述子与过滤后的所述投影矢量特征中各参考对象的投影矢量描述子进行图匹配，以获得匹配矩阵和置信度矩阵，所述匹配矩阵中的元素用于表示各个所述语义对象与各个所述参考对象之间的匹配关系，所述置信度矩阵中的元素用于表示各个所述语义对象与各个参考对象之间的匹配置信度。
4.根据权利要求3所述的位姿优化方法，其特征在于，所述重投影误差根据所述检测矢量特征中各个语义对象的语义标签、以及所述语义对象与过滤后的所述投影矢量特征中各个参考对象之间的形状点重投影误差、端点重投影误差和/或方向向量的重投影误差确定；
其中，所述形状点重投影误差根据所述语义对象的形状角点位置、所述参考对象的形状角点位置、所述语义对象与所述参考对象的匹配关系与匹配置信度得到；和/或，所述端点重投影误差根据所述语义对象的端点位置、所述参考对象的端点位置、所述语义对象与所述参考对象的匹配关系与匹配置信度得到；和/或，所述方向向量的重投影误差根据所述语义对象的方向向量、所述参考对象的方向向量、所述语义对象与所述参考对象的匹配关系与匹配置信度得到；
优选地，所述重投影误差通过采用对数函数的预定核函数表示。
优选地，采用光束平差法最小化所述重投影误差，以优化所述第一位姿，获得所述第二位姿。
5.一种位姿优化装置，其特征在于，包括：
获取模块，用于获取对应第一对象当前所处场景的第一图像和第一对象的第一位姿；
检测模块，用于获取所述第一图像的检测矢量特征；
投影模块，用于获取矢量地图中所述第一位姿的邻域范围内的投影矢量特征；
匹配模块，用于将所述检测矢量特征与所述投影矢量特征匹配，以获得匹配结果；
优化模块，用于最小化基于所述匹配结果确定的重投影误差，以优化所述第一位姿，获得所述第一对象的第二位姿；
优选地，所述第一图像的检测矢量特征包括所述第一图像中各个语义对象的矢量描述子，所述矢量描述子用于描述所述语义对象的语义标签、形状点位置、中心点位置、邻域特征位置向量和方向向量。
6.根据权利要求5所述的位姿优化装置，其特征在于，
所述投影矢量特征包括矢量地图中所述第一位姿的邻域范围内各个参考对象在所述第一图像的图像坐标系的投影矢量描述子，所述投影矢量描述子用于描述所述参考对象在所述第一图像的图像坐标系中的语义标签、形状点位置、中心点位置、邻域特征位置向量和方向向量。
7.根据权利要求5所述的位姿优化装置，其特征在于，所述匹配模块，具体用于：通过所述检测矢量特征与所述投影矢量特征之间的相似度过滤所述投影矢量特征，以使得所述投影矢量特征与所述检测矢量特征在矢量特征空间分布上具有近似的一致性；以及，将所述检测矢量特征与过滤后的所述投影矢量特征进行图匹配，以获得所述匹配结果。
8.根据权利要求5所述的位姿优化装置，其特征在于，
所述匹配模块，具体用于：将所述检测矢量特征中各语义对象的矢量描述子与过滤后的所述投影矢量特征中各参考对象的投影矢量描述子进行图匹配，以获得匹配矩阵和置信度矩阵，所述匹配矩阵中的元素用于表示各个所述语义对象与各个所述参考对象之间的匹配关系，所述置信度矩阵中的元素用于表示各个所述语义对象与各个参考对象之间的匹配置信度；
优选地，所述重投影误差根据所述检测矢量特征中各个语义对象的语义标签、以及所述语义对象与过滤后的所述投影矢量特征中各个参考对象之间的形状点点重投影误差、端点重投影误差和/或方向向量的重投影误差确定；
其中，所述形状点点重投影误差根据所述语义对象的形状角点位置、所述参考对象的形状角点位置、所述语义对象与所述参考对象的匹配关系与匹配置信度得到；和/或，所述端点重投影误差根据所述语义对象的端点位置、所述参考对象的端点位置、所述语义对象与所述参考对象的匹配关系与匹配置信度得到；和/或，所述方向向量的重投影误差根据所述语义对象的方向向量、所述参考对象的方向向量、所述语义对象与所述参考对象的匹配关系与匹配置信度得到；
优选地，所述重投影误差通过采用对数函数的预定核函数表示。
优选地，所述优化模块，具体用于采用光束平差法最小化所述重投影误差，以优化所述第一位姿，获得所述第二位姿。
9.一种电子设备，其特征在于，包括：
存储器，所述存储器存储执行指令；以及
处理器，所述处理器执行所述存储器存储的执行指令，使得所述处理器执行权利要求1至4中任一项所述的位姿优化方法。
10.一种可读存储介质，其特征在于，所述可读存储介质中存储有执行指令，所述执行指令被处理器执行时用于实现权利要求1至4中任一项所述的位姿优化方法。
CN202210507633.6A 2022-05-10 2022-05-10 位姿优化方法、装置、电子设备及存储介质 Pending CN114842080A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210507633.6A CN114842080A (zh) 2022-05-10 2022-05-10 位姿优化方法、装置、电子设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210507633.6A CN114842080A (zh) 2022-05-10 2022-05-10 位姿优化方法、装置、电子设备及存储介质

Publications (1)

Publication Number Publication Date
CN114842080A true CN114842080A (zh) 2022-08-02

Family

ID=82570865

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210507633.6A Pending CN114842080A (zh) 2022-05-10 2022-05-10 位姿优化方法、装置、电子设备及存储介质

Country Status (1)

Country Link
CN (1) CN114842080A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115375772A (zh) * 2022-08-10 2022-11-22 北京英智数联科技有限公司 相机标定方法、装置、设备及存储介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115375772A (zh) * 2022-08-10 2022-11-22 北京英智数联科技有限公司 相机标定方法、装置、设备及存储介质
CN115375772B (zh) * 2022-08-10 2024-01-19 北京英智数联科技有限公司 相机标定方法、装置、设备及存储介质

Similar Documents

Publication Publication Date Title
Huang et al. Visual odometry and mapping for autonomous flight using an RGB-D camera
Siegemund et al. A temporal filter approach for detection and reconstruction of curbs and road surfaces based on conditional random fields
CN113506318B (zh) 一种车载边缘场景下的三维目标感知方法
Felisa et al. Robust monocular lane detection in urban environments
McManus et al. Learning place-dependant features for long-term vision-based localisation
Peršić et al. Online multi-sensor calibration based on moving object tracking
CN111967373B (zh) 一种基于摄像头和激光雷达的自适应强化融合实时实例分割方法
Ding et al. Persistent stereo visual localization on cross-modal invariant map
CN114088081B (zh) 一种基于多段联合优化的用于精确定位的地图构建方法
Zheng et al. Robust and accurate monocular visual navigation combining IMU for a quadrotor
Amorós et al. Trajectory estimation and optimization through loop closure detection, using omnidirectional imaging and global-appearance descriptors
Saleem et al. Neural network-based recent research developments in SLAM for autonomous ground vehicles: A review
CN114842080A (zh) 位姿优化方法、装置、电子设备及存储介质
Xu et al. Dynamic vehicle pose estimation and tracking based on motion feedback for LiDARs
Fu et al. Camera-based semantic enhanced vehicle segmentation for planar lidar
CN114898314A (zh) 驾驶场景的目标检测方法、装置、设备及存储介质
Park et al. Nonparametric background model-based LiDAR SLAM in highly dynamic urban environments
CN113971697A (zh) 一种空地协同车辆定位定向方法
CN117115414A (zh) 基于深度学习的无gps无人机定位方法及装置
CN114627365B (zh) 场景重识别方法、装置、电子设备及存储介质
CN112380933A (zh) 无人机识别目标的方法、装置及无人机
CN116385997A (zh) 一种车载障碍物精确感知方法、系统及存储介质
Patel et al. Tightly coupled semantic RGB-D inertial odometry for accurate long-term localization and mapping
CN115345944A (zh) 外参标定参数确定方法、装置、计算机设备和存储介质
Ge et al. An improved VSLAM for mobile robot localization in corridor environment

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