CN114332221A - 基于语义的回环检测方法、装置、电子设备及存储介质 - Google Patents
基于语义的回环检测方法、装置、电子设备及存储介质 Download PDFInfo
- Publication number
- CN114332221A CN114332221A CN202111621725.9A CN202111621725A CN114332221A CN 114332221 A CN114332221 A CN 114332221A CN 202111621725 A CN202111621725 A CN 202111621725A CN 114332221 A CN114332221 A CN 114332221A
- Authority
- CN
- China
- Prior art keywords
- semantic
- frame
- main object
- semantic category
- pose
- 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
Links
Images
Landscapes
- Traffic Control Systems (AREA)
Abstract
本公开提供了一种基于语义的回环检测方法,包括:基于获取的多帧激光点云数据获取相邻激光帧之间的位姿变换信息;基于相邻激光帧之间的位姿变换信息构建位姿图,对位姿图进行优化,基于优化后的位姿图构建全局地图;以及,基于语义信息从获取的多帧激光点云数据中获取与当前帧最相似的预设数量的候选帧,计算当前帧与各个候选帧之间的相似度,判断是否出现回环。本公开还提供了一种基于语义的回环检测装置、电子设备以及可读存储介质。
Description
技术领域
本公开涉及自动驾驶领域、机器人领域及人工智能领域,本公开尤其涉及一种基于语义的回环检测方法、装置、电子设备及存储介质。
背景技术
SLAM(Simultaneous Localization And Mapping,同步定位与建图)技术在自动驾驶、机器人等有自主定位需求的领域有着重要应用。
SLAM最核心的问题是移动设备(自动驾驶车辆、机器人等)的位姿估计,建图是在位姿估计的基础上依靠传感器(雷达、相机等)感知环境并描述记录的结果。
回环检测(Loop Closure Detection)主要是为了解决位姿估计随时间漂移的问题。在前端不断计算姿态的过程中,由于传感器误差等原因导致位姿出现漂移,当移动设备再次经过之前经过的位置时,此时的位置和之前记录的位置是有误差的,这个误差被找到后可以发送到后端优化用于去除两个回环检测之间的位姿漂移误差。回环检测是移动设备检测出到过的场景的方法,是后端优化消除累计误差的重要环节。
例如,现有技术1:《GOSMatch:Graph-of-Semantics Matching for DetectingLoop Closures in 3D LiDAR data》发表了一种基于语义图匹配的回环检测方法,该方法中只选取三种语义类别,并通过语义分割和聚类形成语义点云图。语义点云图采用无向完整图表示,每个顶点之间的边表示语义实例间的空间欧式距离,并生成基于直方图的图描述子。直方图的横坐标是顶点类别组合,纵轴是基于边长度的统计量;同理计算图中顶点描述子,顶点描述子也基于直方图,方法类似于图描述子。最后使用RANSAC-based的方法计算是否发生回环。该方法只统计了语义图中的距离信息,因为车辆也在语义类别中,并没有考虑车辆移动带来的场景差异,可能会造成回环召回率降低;同时回环的检测方法需要当前帧和历史帧不停使用RANSAC-based计算,时间消耗大。
现有技术2:中国专利申请CN112767485A公开了一种基于静态语义信息的点云地图创建与场景辨识方法,包括步骤1:激光点云与语义图像联合投影获得语义点云;步骤2:通过语义标签筛选鲁棒静态信息;步骤3:提取鲁棒静态信息的描述子并组合成当前场景的描述子;步骤4:语义点云与激光雷达的里程计相结合生成全局地图,并通过点云配准在全局地图上进行重定位。该技术提取鲁棒静态信息建立描述子,描述子直接匹配得出当前帧的位姿,但是描述子内容强依赖于激光雷达的位置,这容易造成描述子误匹配,并且直接匹配方式将消耗大量的时间,在实际使用时有一定局限性。
上述两项技术方案均无法直接计算出当前帧和回环帧的相对位姿关系;且现有技术1对于车辆等可移动障碍物未做处理,可移动障碍物导致的场景变化会造成回环失败;现有技术2计算效率不高并且容易出现错误匹配。
总之,常见的回环检测方案一般会出现如下几个问题:1)以激光雷达为中心,导致观测视角大大影响了回环的识别率;2)检测到回环无法直接计算相对位姿关系;
3)不考虑动态移动物体对场景造成的变化,对于有车辆的场景,车辆的移动会造成匹配失误;4)回环检测过程中直接匹配导致计算消耗过大。
发明内容
为了解决上述技术问题中的至少一个,本公开提供了一种基于语义的回环检测方法、装置、电子设备及存储介质。
根据本公开的一个方面,提供一种基于语义的回环检测方法,包括:
S110、基于获取的多帧激光点云数据获取相邻激光帧之间的位姿变换信息(即获取相邻激光帧之间的位姿转换关系);
S120、至少基于相邻激光帧之间的位姿变换信息构建位姿图(Pose Graph),对位姿图进行优化,基于优化后的位姿图构建全局地图;
S130、基于语义信息从所述获取的多帧激光点云数据中获取与当前帧最相似的预设数量的候选帧(TopN个候选帧),计算当前帧与各个候选帧之间的相似度,判断是否出现回环。
根据本公开的至少一个实施方式的基于语义的回环检测方法,还包括:S140、如果出现回环,获取出现回环的两帧之间的相对位姿误差,将所述相对位姿误差用于步骤S120中的对位姿图进行优化。
根据本公开的至少一个实施方式的基于语义的回环检测方法,S110、基于获取的多帧激光点云数据获取相邻激光帧之间的位姿变换信息,包括:
S111、对获取的包括激光雷达的SLAM相关传感器的传感数据(包括了上文描述的多帧激光点云数据)进行时间同步(优选地通过时间戳进行时间同步)以及对多帧激光点云数据进行点云畸变补偿;
S112、对各帧激光点云数据进行特征提取,以获取各帧激光点云数据的线面特征;
S113、对各帧激光点云数据进行特征匹配计算,以获得相邻两帧间的位姿变换信息。
根据本公开的至少一个实施方式的基于语义的回环检测方法,S120、至少基于相邻激光帧之间的位姿变换信息构建位姿图(Pose Graph),对位姿图进行优化,基于优化后的位姿图构建全局地图,包括:
S121、获取所述多帧激光点云数据中的至少一个关键帧(可以基于预设的关键帧获取标准获取),将关键帧位姿作为位姿图的节点并对关键帧位姿进行优化;
S122、基于优化后的关键帧位姿将关键帧的激光点云投影到地图上以获得全局地图。
根据本公开的至少一个实施方式的基于语义的回环检测方法,S130、基于语义信息从所述获取的多帧激光点云数据中获取与当前帧最相似的预设数量的候选帧(TopN个候选帧),计算当前帧与各个候选帧之间的相似度,判断是否出现回环,包括:
S131、以主物体为中心构建当前场景的全局描述子;
S132、基于全局描述子以及与主物体最近的语义物体的类别和距离,构建主物体的语义类别vector(语义类别vector);
S133、基于当前场景确定语义分割所需的语义类别,以主物体为中心将主物体周围的360度范围均分为NS等份区域,基于各个等份区域内的语义物体的语义类别以及各个语义类别的语义物体数量获取语义类别直方图;
S134(本步骤即基于语义类别vector进行粗匹配)、计算当前帧中主物体的语义类别vector与所述获取的多帧激光点云数据(即历史关键帧)中与该主物体具有相同语义类别的历史语义类别vector(历史语义类别vector)之间的语义类别vector相似度(对于当前帧中的所有主物体,均进行相同处理),以获取语义类别vector相似度大于或等于阈值相似度的历史关键帧作为候选帧;
S135、计算当前帧中主物体的语义类别直方图与各个候选帧中与该主物体具有相同语义类别的历史语义类别直方图的相似度,以获取语义类别直方图相似度最大的历史关键帧。
根据本公开的至少一个实施方式的基于语义的回环检测方法,还包括:S136、判断语义类别直方图相似度最大的历史关键帧的语义类别直方图与当前帧的语义类别直方图之间的距离是否小于或等于阈值距离,如果是,则将所述语义类别直方图相似度最大的历史关键帧作为回环帧(即回环关键帧)。
根据本公开的至少一个实施方式的基于语义的回环检测方法,步骤S134中,还包括:
判断当前帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断历史关键帧的历史语义类别vector中对应位置标签是否为车辆标签,如果是,则计算当前帧与该历史关键帧之间的语义类别vector相似度;和/或,
判断历史关键帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断当前帧的语义类别vector中对应位置标签是否为车辆标签,如果是,则计算当前帧与该历史关键帧之间的语义类别vector相似度。
根据本公开的至少一个实施方式的基于语义的回环检测方法,步骤S134中,还包括:
判断当前帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断历史关键帧的历史语义类别vector中对应位置标签是否为车辆标签,如果否,则判断当前帧中该主物体的语义类别vector中主物体与车辆标签的距离是否小于所述历史关键帧中该主物体的历史语义类别vector中主物体与对应位置标签的距离,如果小于,则计算当前帧与该历史关键帧之间的语义类别vector相似度时不考虑车辆标签,如果不小于,则考虑车辆标签;和/或,
判断历史关键帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断当前帧的语义类别vector中对应位置标签是否为车辆标签,如果否,则判断所述历史关键帧中该主物体的历史语义类别vector中主物体与车辆标签的距离是否小于当前帧中该主物体的语义类别vector中主物体与对应位置标签的距离,如果小于,则计算当前帧与该历史关键帧之间的语义类别vector相似度时不考虑车辆标签,如果不小于,则考虑车辆标签。
根据本公开的至少一个实施方式的基于语义的回环检测方法,S135、计算当前帧中主物体的语义类别直方图与各个候选帧中与该主物体具有相同语义类别的历史语义类别直方图的相似度,以获取语义类别直方图相似度最大的历史关键帧,包括:
判断当前帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断候选帧的历史语义类别vector中对应位置标签是否为车辆标签,如果否,则判断当前帧中该主物体的语义类别vector中主物体与车辆标签的距离是否小于所述候选帧中该主物体的历史语义类别vector中主物体与对应位置标签的距离,如果小于,则计算当前帧中主物体的语义类别直方图与候选帧中与该主物体具有相同语义类别的历史语义类别直方图的相似度时不将车辆关联区域计入相似度计算,如果不小于,则将车辆关联区域计入相似度计算;和/或,
判断候选帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断当前帧的语义类别vector中对应位置标签是否为车辆标签,如果否,则判断所述候选帧中该主物体的历史语义类别vector中主物体与车辆标签的距离是否小于当前帧中该主物体的语义类别vector中主物体与对应位置标签的距离,如果小于,则计算当前帧中主物体的语义类别直方图与候选帧中与该主物体具有相同语义类别的历史语义类别直方图的相似度时不将车辆关联区域计入相似度计算,如果不小于,则将车辆关联区域计入相似度计算。
根据本公开的至少一个实施方式的基于语义的回环检测方法,S140、如果出现回环,获取出现回环的两帧之间的相对位姿误差,包括:对于当前帧以及当前帧的回环帧,取出主物体在当前帧坐标系以及回环帧坐标系下的位姿,获取相对位姿误差。
根据本公开的另一个方面,提供一种基于语义的回环检测装置,包括:
位姿变换信息获取模块,所述位姿变换信息获取模块基于获取的多帧激光点云数据获取相邻激光帧之间的位姿变换信息;
位姿图/地图构建模块,所述位姿图/地图构建模块至少基于相邻激光帧之间的位姿变换信息构建位姿图,对位姿图进行优化,基于优化后的位姿图构建全局地图;
回环检测模块,所述回环检测模块基于语义信息从所述获取的多帧激光点云数据中获取与当前帧最相似的预设数量的候选帧,计算当前帧与各个候选帧之间的相似度,判断是否出现回环。
根据本公开的至少一个实施方式的基于语义的回环检测装置,还包括:位姿差获取模块,如果出现回环,所述位姿差获取模块获取出现回环的两帧之间的相对位姿误差。
根据本公开的至少一个实施方式的基于语义的回环检测装置,还包括:传感器装置,所述传感器装置至少用于获取激光点云数据。
根据本公开的又一个方面,提供一种电子设备,包括:存储器,所述存储器存储执行指令;以及,处理器,所述处理器执行所述存储器存储的执行指令,使得所述处理器执行上述任一项所述的回环检测方法。
根据本公开的再一个方面,提供一种可读存储介质,所述可读存储介质中存储有执行指令,所述执行指令被处理器执行时用于实现上述任一项所述的回环检测方法。
附图说明
附图示出了本公开的示例性实施方式,并与其说明一起用于解释本公开的原理,其中包括了这些附图以提供对本公开的进一步理解,并且附图包括在本说明书中并构成本说明书的一部分。
图1是本公开的一个实施方式的基于语义的回环检测方法的流程示意图。
图2是本公开的又一个实施方式的基于语义的回环检测方法的流程示意图。
图3是本公开的一个实施方式的基于语义的回环检测方法中的位姿变换信息的获取流程图。
图4是本公开的一个实施方式的基于语义的回环检测方法中的位姿图/地图的构建流程。
图5是本公开的一个实施方式的基于语义的回环检测方法中的判断回环帧的流程示意图。
图6是本公开的又一个实施方式的基于语义的回环检测方法中的判断回环帧的流程示意图。
图7是单个主物体及其周围全部扇形区域的语义物体示意图。
图8是图7中主物体的语义类别vector的构成元素示意图。
图9是主物体的语义类别直方图。
图10为本公开的一个实施方式的平移变化示意图。
图11是本公开的一个实施方式的两个回环帧之间的平移角度示意图。
图12是本公开的一个实施方式的采用硬件处理系统的基于语义的回环检测装置的结构示意框图。
附图标记说明
1000 回环检测装置
1002 位姿变换信息获取模块
1004 位姿图/地图构建模块
1006 回环检测模块
1008 位姿差获取模块
1100 总线
1200 处理器
1300 存储器
1400 其他电路。
具体实施方式
下面结合附图和实施方式对本公开作进一步的详细说明。可以理解的是,此处所描述的具体实施方式仅用于解释相关内容,而非对本公开的限定。另外还需要说明的是,为了便于描述,附图中仅示出了与本公开相关的部分。
需要说明的是,在不冲突的情况下,本公开中的实施方式及实施方式中的特征可以相互组合。下面将参考附图并结合实施方式来详细说明本公开的技术方案。
除非另有说明,否则示出的示例性实施方式/实施例将被理解为提供可以在实践中实施本公开的技术构思的一些方式的各种细节的示例性特征。因此,除非另有说明,否则在不脱离本公开的技术构思的情况下,各种实施方式/实施例的特征可以另外地组合、分离、互换和/或重新布置。
在附图中使用交叉影线和/或阴影通常用于使相邻部件之间的边界变得清晰。如此,除非说明,否则交叉影线或阴影的存在与否均不传达或表示对部件的具体材料、材料性质、尺寸、比例、示出的部件之间的共性和/或部件的任何其它特性、属性、性质等的任何偏好或者要求。此外,在附图中,为了清楚和/或描述性的目的,可以夸大部件的尺寸和相对尺寸。当可以不同地实施示例性实施例时,可以以不同于所描述的顺序来执行具体的工艺顺序。例如,可以基本同时执行或者以与所描述的顺序相反的顺序执行两个连续描述的工艺。此外,同样的附图标记表示同样的部件。
当一个部件被称作“在”另一部件“上”或“之上”、“连接到”或“结合到”另一部件时,该部件可以直接在所述另一部件上、直接连接到或直接结合到所述另一部件,或者可以存在中间部件。然而,当部件被称作“直接在”另一部件“上”、“直接连接到”或“直接结合到”另一部件时,不存在中间部件。为此,术语“连接”可以指物理连接、电气连接等,并且具有或不具有中间部件。
本文使用的术语是为了描述具体实施例的目的,而不意图是限制性的。如这里所使用的,除非上下文另外清楚地指出,否则单数形式“一个(种、者)”和“所述(该)”也意图包括复数形式。此外,当在本说明书中使用术语“包含”和/或“包括”以及它们的变型时,说明存在所陈述的特征、整体、步骤、操作、部件、组件和/或它们的组,但不排除存在或附加一个或更多个其它特征、整体、步骤、操作、部件、组件和/或它们的组。还要注意的是,如这里使用的,术语“基本上”、“大约”和其它类似的术语被用作近似术语而不用作程度术语,如此,它们被用来解释本领域普通技术人员将认识到的测量值、计算值和/或提供的值的固有偏差。
首先对本公开的部分技术术语进行解释说明:
主物体:场景中的语义物体在一定的限定条件下筛选出的比较有标志性的物体。
语义类别vector(语义类别vector):主物体周边按照角度分区,每个分区距离主物体最近的语义物体与主物体间的距离。
Submap(子图):局部连续多个激光帧构建的地图,每个submap有自己的位姿,多个子图可形成更大的局部地图。
语义类别直方图:以主物体为中心,周围环境中的语义物体的信息的一种统计形式。
类别标签:激光点云数据通过神经网络后对每个激光点分出的类别,每个点都属于对应神经网络的结果类别中的一个。
激光帧:传感器的数据以帧的形式存储和传输,每帧数据包括传感器在固定时间内发射激光束扫描环境后每个点的传输距离。
关键帧:激光帧中质量较优的帧,也用于后端根据关键帧构建位姿图。
坐标系:本公开优选地使用直角坐标系,直角坐标系使用机器人操作系统中通用的坐标系定义,为笛卡尔直角坐标系右手系(向前为X轴,向左为Y轴,向上为Z轴)。
位姿:本公开涉及的位姿为物体在2维平面的3自由度的位置和姿态,位置为x方向和y方向的坐标,姿态为以x轴为正方向的偏航角θ。
观测位置:以移动设备(自动驾驶车辆、机器人等)中心为坐标原点、移动设备正方向为X轴、向左为Y轴建立直角坐标系,主物体在坐标系中的位置为观测位置。
本公开中的传感器数据需要经过传感器相对移动设备中心的外参转换,将传感器数据转换到移动设备中心,本公开的技术方案中默认传感器数据已经过外参转换,本领域技术人员应当理解,在本公开的技术方案的某些步骤之前、之中或之后进行外参变换步骤,均落入本公开的保护范围。
下文结合图1至图12对本公开的基于语义的回环检测方法、回环检测装置进行详细说明。
图1是本公开的一个实施方式的基于语义的回环检测方法的流程示意图。
参考图1,基于语义的回环检测方法S100,包括:
S110、基于获取的多帧激光点云数据获取相邻激光帧之间的位姿变换信息(即获取相邻激光帧之间的位姿转换关系);
S120、至少基于相邻激光帧之间的位姿变换信息构建位姿图(Pose Graph),对位姿图进行优化,基于优化后的位姿图构建全局地图;
S130、基于语义信息从获取的多帧激光点云数据中获取与当前帧最相似的预设数量的候选帧(TopN个候选帧),计算当前帧与各个候选帧之间的相似度,判断是否出现回环。
移动设备例如车辆在行驶过程中,会不断地获取激光点云数据,其中,上文描述的多帧激光点云数据为移动设备(汽车等)在当前时刻之前的预设时间长度之内获取的激光点云数据。
其中,基于移动设备在预设时间长度之内的移动轨迹及相邻激光帧之间的位姿变换信息获取位姿图。
根据本公开的优选实施方式的于语义的回环检测方法S100,还包括:
S140、如果出现回环,获取出现回环的两帧之间的相对位姿误差,将相对位姿误差用于步骤S120中的对位姿图进行优化。
图3是本公开的一个实施方式的基于语义的回环检测方法中的位姿变换信息的获取流程图。
对于上述各个实施方式的基于语义的回环检测方法S100,参考图3,优选地,S110、基于获取的多帧激光点云数据获取相邻激光帧之间的位姿变换信息,包括:
S111、对获取的包括激光雷达的SLAM相关传感器的传感数据(包括了上文描述的多帧激光点云数据)进行时间同步(优选地通过时间戳进行时间同步)以及对多帧激光点云数据进行点云畸变补偿;
S112、对各帧激光点云数据进行特征提取,以获取各帧激光点云数据的线面特征;
S113、对各帧激光点云数据进行特征匹配计算,以获得相邻两帧间的位姿变换信息。
其中,SLAM相关传感器包括轮速编码器、惯性测量单元、相机、激光雷达、毫米波雷达、卫星定位系统中的一个或多个,激光雷达用于获取激光帧。
由于移动设备在采集激光数据的过程中大多数处于运动状态,并且因为激光频率有限,导致同一帧激光数据并不是在同一时刻采集到的,同一激光帧的起始激光束和结束激光束对应了移动设备的不同位置,因此不同激光点就对应了不同的坐标系。如果不考虑激光点云畸变,定位和建图会有较大的误差,因此需要对点云畸变进行补偿消除,点云畸变补偿可以有效提升建图精度。
根据本公开的优选实施方式,通过将同一激光帧的所有激光点推算到起始激光束的坐标系(可以按照运动模型进行推算)进行点云畸变补偿。优选地,基于移动设备的运动速度(可由SLAM相关传感器中的其他传感器提供)并积分得到各个激光点的相对运动位置,再依据相对运动位置计算转换矩阵,将所有激光点转换到初始点的坐标系下。
对于3D激光雷达,不同的激光雷达型号,单帧激光点云数据会有所不同。但是普遍的问题是点云数据量较大,实时计算压力大。
优选地,对各帧激光点云数据进行采样处理,以减轻计算量。
其中,点云数据的采样方法,可以采用基于点的曲率的滤波或者基于法向量方差的滤波等。在语义相关的方案中,语义物体的大小差距可能会比较大,如果按照统一的大小过滤可能会把有较多特征的小语义物体过滤掉,所以优选地考虑将体素滤波的窗口根据语义类别做自适应,提高滤波结果的有效性。
优选地,将3维空间分割成固定体积的多个立方体,在每个立方体中按照预设规则保留少量点,余下的激光点会被丢弃。上述采样处理在优化计算量的同时能够过滤掉部分点云噪声。
根据本公开的优选实施方式,各帧激光点云数据的线面特征基于各帧激光点云数据中每个点的平滑度获取,优选地,基于平滑度将点云划分为平面点和边缘点。平面点是在三维空间中处于平滑平面上的点,平滑度较高;边缘点是处于尖锐边缘上的点,平滑度较低。
上述位姿变换信息可以通过激光里程计获得,激光里程计是使用激光雷达推算移动设备的位姿变化的过程。对于接收到的每个激光帧,都会与其上一帧做点云特征匹配,通过特征匹配计算两帧间位姿变换信息。
其中,点云特征匹配的方法包括ICP、NDT等。一般情况下,点云特征匹配对位姿的初值敏感,因此,优选地使用其它传感器的输出结果作为初值,或者使用运动模型预测初值。计算帧与帧间的位姿变换并累积的过程就是激光里程计,激光里程计的计算属于增量式,因此,随着时间的推移会导致累计误差不断增大,需要后端优化消除累积误差。
图4是本公开的一个实施方式的基于语义的回环检测方法中的位姿图/地图的构建流程。
参考图4,优选地,基于语义的回环检测方法S100中的S120、至少基于相邻激光帧之间的位姿变换信息构建位姿图(Pose Graph),对位姿图进行优化,基于优化后的位姿图构建全局地图,包括:
S121、获取多帧激光点云数据中的至少一个关键帧(可以基于预设的关键帧获取标准获取),将关键帧位姿作为位姿图的节点并对关键帧位姿进行优化;
S122、基于优化后的关键帧位姿将关键帧的激光点云投影到地图上以获得全局地图。
本公开中,对于所有的关键帧,取关键帧位姿作为位姿图的节点,以空间约束为位姿图的边构建位姿图。相邻节点的边代表里程约束,其余边表示闭环约束。当前端(前端处理模块)不停发送关键帧给后端(后端处理模块)或者检测到回环时都会增加约束,优选地,位姿图通过最小化约束的最小二乘误差优化节点位姿。
本公开中,后端优化完关键帧位姿后,按照优化后的关键帧位姿将关键帧的激光点云投影到地图上以生成位姿图,将全部关键帧按照优化后的位姿做投影即得到全局地图。
图5为本公开的一个实施方式的基于语义的回环检测方法中的判断回环帧的流程示意图。
对于上述各个实施方式的基于语义的回环检测方法S100,优选地,参考图5,S130、基于语义信息从获取的多帧激光点云数据中获取与当前帧最相似的预设数量的候选帧(TopN个候选帧),计算当前帧与各个候选帧之间的相似度,判断是否出现回环,包括:
S131、以主物体为中心构建当前场景的全局描述子;
S132、基于全局描述子以及与主物体最近的语义物体的类别和距离,构建主物体的语义类别vector(语义类别向量);
S133、基于当前场景确定语义分割所需的语义类别,以主物体为中心将主物体周围的360度范围均分为NS等份区域,基于各个等份区域内的语义物体的语义类别以及各个语义类别的语义物体数量获取语义类别直方图;
S134、计算当前帧中主物体的语义类别vector与获取的多帧激光点云数据(即历史关键帧)中与该主物体具有相同语义类别的历史语义类别vector(历史语义类别vector)之间的语义类别vector相似度(对于当前帧中的所有主物体,均进行相同处理),以获取语义类别vector相似度大于或等于阈值相似度的历史关键帧作为候选帧;
S135、计算当前帧中主物体的语义类别直方图与各个候选帧中与该主物体具有相同语义类别的历史语义类别直方图的相似度,以获取语义类别直方图相似度最大的历史关键帧。
其中,步骤S134为基于语义类别vector的回环粗匹配,步骤S135为基于语义类别直方图的回环精匹配。
本公开以主物体为中心,而不是以激光雷达为中心,进行场景全局描述子的构建,可摆脱当前帧需要与回环帧非常相近位置的前提条件,只要检测到相同主物体及相似的场景全局描述子,即可找出回环并直接基于语义描述子(语义信息)计算相对位置关系,鲁棒性更强,回环召回率更高。
更详细地,在以主物体为中心的语义类别直方图构建的相同的区域,计算每个扇形块中距离主物体最近的语义物体的类别和距离,能形成一个主物体的语义类别vector,每个语义类别vector的维度等于NS,每个维度的值包括语义类别和主物体距离语义物体的距离。
语义类别vector对应的数学描述为:
其中,i代表第i个主物体,NS是主物体周围划分的扇形区个数,l是扇形区里面距离主物体最近的语义物体的距离;t是语义物体的语义类别标签,上式即公式#(1)中的加号不代表数值计算,只表示语义类别vector存储时要同时存储语义物体的类别和距离主物体的距离。计算出每个主物体的语义类别vector后,将语义类别vector连同该主物体的语义类别同时存入数据库。
根据使用场景(当前场景)确定语义分割所需要的语义类别,语义类别的选取的主要原则包括场景内静止的物体、场景内高区分度的物体,因此,人和动物等都将不作为语义类别,静止的车辆虽然具有潜在的移动性,但其本身区分度高,所以将其纳入语义类别。同时将车辆类别单独标记和存储,在后续的相似度计算时特殊处理。语义类别确定后,获取对应场景的数据集进行语义分割训练,可以基于RangeNet++语义分割网络进行语义分割训练,本领域技术人员也可以基于其他类型的语义分割网络(例如RandLA-Net、SalsaNet、KPConv等)进行语义分割训练,均落入本公开的保护范围。
移动设备例如汽车在行驶过程中,会不断的获取激光点云信息,多帧激光组成子图(submap)。对于每个子图的点云信息,使用训练好的语义分割网络对每个点做语义识别获得语义类别标签,然后做聚类分析(例如使用K-Means聚类算法)以获得多个主物体,并计算每个主物体的质心,作为主物体的x坐标和y坐标。
以主物体为中心,将主物体周围的360度范围均分为NS等份,优选地,以主物体和本车车体中心的连接线为起始,依次按照预设角度间隔进行划分,划分半径为L,这样可将主物体附近区域划分为NS个扇形块。每个扇形块内会分布有多个语义物体,按照语义类别分别统计个数以得到每个扇形块的语义类别直方图,可以以横轴为类别,纵轴为该类别的个数。语义类别个数为NC,得到NS×NC长度的主物体的语义类别直方图。语义类别直方图的数学表达为:
其中,label是主物体的语义类别,i是指第i个主物体的直方图,NC是语义类别总数,NS是主物体周围扇形块个数。
计算出每个主物体的语义类别直方图后,将语义类别直方图连同该主物体的语义类别同时存入数据库。至此,每个主物体都对应了一个语义类别vector和一个语义类别直方图。
从组成上可以看出,语义类别vector和语义类别直方图都具有旋转不变性,即在不同的视角观测,只会影响语义类别直方图和语义类别vector的平移顺序,而不会改变内容。这个特性保证了回环检测的可靠性。
其中,由于激光建图中存在大量的关键帧,本公开在进行回环检测时需要做计算优化,以加快回环检测速度。本公开使用语义类别vector做粗匹配就是为了快速的在大量的历史关键帧数据中找出与当前帧最相似的部分数据。
每个语义类别vector长度是NS,每个维度中同时存储了语义类别标签和距离值。当进行匹配时,从数据库中调取所有与当前主物体相同的类别的历史语义类别vector。以单个历史语义类别vector为例,其语义类别vector为q,当前观测中主物体的语义类别vector为p,先通过类别标签计算相似性,因为旋转yaw的存在以及语义类别vector的旋转不变性,计算相似性时需要做旋转,直至语义类别完全对应或把所有可能计算完毕。计算公式为:
其中,p是当前观测的主物体的语义类别vector,是数据库中已经存储的单个语义类别vector,上标rt代表前后平移变化,k是语义类别vector的下标值,n是将q平移后与p有最大相似度时的平移位数。平移变化可以参考图10。
对于上述实施方式的基于语义的回环检测方法S100,优选地,参考图6,步骤S130还包括:
S136、判断语义类别直方图相似度最大的历史关键帧的语义类别直方图与当前帧的语义类别直方图之间的距离是否小于或等于阈值距离,如果是,则将语义类别直方图相似度最大的历史关键帧作为回环帧(即回环关键帧)。
步骤S136为回环帧判定步骤。
上文描述的计算直方图相似度时优选地使用曼哈顿距离,也可以采取其它公式计算直方图距离,如欧式距离或者余弦距离等,但需要注意增加语义类别权重。
根据本公开的优选实施方式,步骤S134中,还包括:
判断当前帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断历史关键帧的历史语义类别vector中对应位置标签是否为车辆标签,如果是,则计算当前帧与该历史关键帧之间的语义类别vector相似度;和/或,
判断历史关键帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断当前帧的语义类别vector中对应位置标签是否为车辆标签,如果是,则计算当前帧与该历史关键帧之间的语义类别vector相似度。
本公开中,需要特殊处理的是,如果语义类别vector中的某个位置的类别是车辆,由于车辆的潜在移动性,可能导致在相同场景中出现类别不能完全一致的现象,因此,当语义类别vector中某些位置为车辆标签时,对比两个语义类别vector的标签,如果对应位置都为车辆则计算两个帧之间的语义类别相似度。
根据本公开的优选实施方式,步骤S134中,还包括:
判断当前帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断历史关键帧的历史语义类别vector中对应位置标签是否为车辆标签,如果否,则判断当前帧中该主物体的语义类别vector中主物体与车辆标签的距离是否小于历史关键帧中该主物体的历史语义类别vector中主物体与对应位置标签的距离,如果小于,则计算当前帧与该历史关键帧之间的语义类别vector相似度时不考虑车辆标签(即计算相似度时不计入车辆标签),如果不小于,则考虑车辆标签;和/或,
判断历史关键帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断当前帧的语义类别vector中对应位置标签是否为车辆标签,如果否,则判断历史关键帧中该主物体的历史语义类别vector中主物体与车辆标签的距离是否小于当前帧中该主物体的语义类别vector中主物体与对应位置标签的距离,如果小于,则计算当前帧与该历史关键帧之间的语义类别vector相似度时不考虑车辆标签(即计算相似度时不计入车辆标签),如果不小于,则考虑车辆标签。
本实施方式中,如果当前帧的某个主物体的语义类别vector与该主物体在历史关键帧的历史语义类别vector除车辆标签之外的其余位置均对应相同,但车辆的位置出现差异,需进一步计算语义类别vector的距离信息,使用对应位置的距离值。如果与车辆不相同的类别的距离小于车辆距离主物体的距离,则代表该位置之前没有车,类别差异不是由车辆的移动导致的,此时I(pk,qk)=0;如果与车辆不相同的类别的距离大于车辆距离主物体的距离,则代表很可能是车辆的移动导致的类别差异,类别差异不影响场景相同,此时I(pk,qk)=1。至此,在平移位数为n时的历史语义类别vector与当前主物体的语义类别vector有最大相似度,最大的相似度为:
根据本公开的优选实施方式,计算完毕全部的主物体和有相同主物体类别的历史关键帧的语义类别vector相似度,使用阈值ε作为评定是否被过滤的标准:
当关键帧的语义类别vector的相似度高于阈值时,保存旋转过的角度:
其中,以向左为正向,n是由式#(4)计算得到的。
在上文描述的语义类别vector匹配中,qrt是经过旋转的关键帧的语义类别vector,θrt是对应的旋转角度。对于所有中的关键帧,按照对应的θrt将语义类别直方图依次旋转位置,将旋转后的直方图和主物体的直方图做距离计算:
其中,hi和ki是直方图对应位置的值,ωj是类别权重,j∈{0,1,2...NC},通过限定ωj可以控制每个语义类别在计算中的重要程度。直方图距离是两个直方图相似度的表征,距离越小代表越相似。
对于上述各个实施方式的基于语义的回环检测方法S100,优选地,S135、计算当前帧中主物体的语义类别直方图与各个候选帧中与该主物体具有相同语义类别的历史语义类别直方图的相似度,以获取语义类别直方图相似度最大的历史关键帧,包括:
判断当前帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断候选帧的历史语义类别vector中对应位置标签是否为车辆标签,如果否,则判断当前帧中该主物体的语义类别vector中主物体与车辆标签的距离是否小于候选帧中该主物体的历史语义类别vector中主物体与对应位置标签的距离,如果小于,则计算当前帧中主物体的语义类别直方图与候选帧中与该主物体具有相同语义类别的历史语义类别直方图的相似度时不将车辆关联区域计入相似度计算,如果不小于,则将车辆关联区域计入相似度计算;和/或,
判断候选帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断当前帧的语义类别vector中对应位置标签是否为车辆标签,如果否,则判断候选帧中该主物体的历史语义类别vector中主物体与车辆标签的距离是否小于当前帧中该主物体的语义类别vector中主物体与对应位置标签的距离,如果小于,则计算当前帧中主物体的语义类别直方图与候选帧中与该主物体具有相同语义类别的历史语义类别直方图的相似度时不将车辆关联区域计入相似度计算,如果不小于,则将车辆关联区域计入相似度计算。
在语义地图的精匹配中依旧需要考虑车辆移动给匹配结果带来的影响。车辆移动带来的观测视角变化可以分为两种,一种是历史帧中有车辆,但是当前帧中没有;另一种是历史帧中没有车辆,而当前帧有车辆。在统计直方图时使用的是语义物体的质心距离,车辆的体积一般比较大,其引起的观测变化和其与主物体的距离成反比,距离越近则影响越大。语义类别vector中存储的数据可以判断车辆是否是距离主物体最近的语义物体,如果车辆距离主物体最近,则不将车辆对应的扇形块计入相似度计算,只计算没有车辆的扇形块,需要注意的是,计算时只考虑质心,但是实际车辆因为体积较大可能影响不止一个扇形块,因此需要根据车辆体积计算影响了多少个扇形块(即车辆关联区域),这些被影响的扇形块均不计入相似度计算;如果车辆不是距离最近的,则使用对应语义vector的结果作为直方图相似度结果。
对于上述各个实施方式的基于语义的回环检测方法S100,优选地,S140、如果出现回环,获取出现回环的两帧之间的相对位姿误差,包括:
对于当前帧以及当前帧的回环帧,取出主物体在当前帧坐标系以及回环帧坐标系下的位姿,获取相对位姿误差。
经过精匹配计算出所有候选回环与当前帧的相似度后,可以得到相似度最大即距离最小的结果。但是最相似的结果不一定就是回环结果,需要进一步判断最小距离是否小于一定阈值,即:
Dmin≤δ#(8)
满足上述阈值条件的最短距离对应的历史帧被认为是回环帧。
对于已经发生回环的两个关键帧,取出主物体在对应关键帧坐标系下的位置,分别为:
(X1,y1)和(x2,y2);
两个关键帧的位姿分别为:
推导计算出的当前关键帧位姿相对于回环关键帧的位姿差为:
其中,θ是rt的平移角度。对于确定是回环的两帧,计算出相对位姿误差后发给后端用于后端优化。图11是本公开的一个实施方式的两个回环帧之间的平移角度示意图。
本公开的基于语义的回环检测方法,采用分层递进的语义相似度匹配策略,先基于语义vector进行粗匹配(提高搜索效率),然后基于语义直方图进行精细匹配,匹配过程中支持对车辆等可移动障碍物的处理,消除可移动障碍物导致的场景变化,能够提高汽车移动较多的场景的回环检测的成功率。本公开的基于语义的回环检测方法能够直接计算出回环两帧的相对位置关系。
图7是单个主物体及其周围全部扇形区域的语义物体,也就是语义直方图的全部组成元素。
图8是图7中主物体的语义类别vector的构成元素,也就是每个扇形块中距离主物体最近的语义物体。
图9是主物体的语义类别直方图,其中每个扇形块均统计了所有语义类别的语义物体个数。
根据本公开的另一个方面,提供一种基于语义的回环检测装置。
根据本公开的一个实施方式的基于语义的回环检测装置1000,包括:
位姿变换信息获取模块1002,位姿变换信息获取模块1002基于获取的多帧激光点云数据获取相邻激光帧之间的位姿变换信息(即获取相邻激光帧之间的位姿转换关系);
位姿图/地图构建模块1004,位姿图/地图构建模块1004至少基于相邻激光帧之间的位姿变换信息构建位姿图(Pose Graph),对位姿图进行优化,基于优化后的位姿图构建全局地图;
回环检测模块1006,回环检测模块1006基于语义信息从获取的多帧激光点云数据中获取与当前帧最相似的预设数量的候选帧(TopN个候选帧),计算当前帧与各个候选帧之间的相似度,判断是否出现回环。
优选地,本公开的基于语义的回环检测装置1000还包括:位姿差获取模块1008,如果出现回环,位姿差获取模块1008获取出现回环的两帧之间的相对位姿误差。
上述各个实施方式的基于语义的回环检测装置可以基于计算机软件架构的形式实现。
对于上述各个实施方式的基于语义的回环检测装置1000,优选地,还包括:传感器装置,传感器装置至少用于获取激光点云数据。
本公开的技术方案使用移动设备(自动驾驶车辆、机器人等)上的传感器装置,在移动设备的运动过程中进行SLAM(同步定位与建图)过程,包括前端里程计、后端优化、回环检测和实时建图。
移动设备包括安装距离探测传感器的自动驾驶车辆、机器人等需要实时建图定位需求的自主移动设备。距离探测传感器包括激光雷达、深度相机、毫米波雷达等能够获取环境距离信息的传感器。
图12是本公开的一个实施方式的采用硬件处理系统的基于语义的回环检测装置的结构示意框图。
该回环检测装置可以包括执行上述流程图中各个或几个步骤的相应模块。因此,可以由相应模块执行上述流程图中的每个步骤或几个步骤,并且该装置可以包括这些模块中的一个或多个模块。模块可以是专门被配置为执行相应步骤的一个或多个硬件模块、或者由被配置为执行相应步骤的处理器来实现、或者存储在计算机可读介质内用于由处理器来实现、或者通过某种组合来实现。
该硬件结构可以利用总线架构来实现。总线架构可以包括任何数量的互连总线和桥接器,这取决于硬件的特定应用和总体设计约束。总线1100将包括一个或多个处理器1200、存储器1300和/或硬件模块的各种电路连接到一起。总线1100还可以将诸如外围设备、电压调节器、功率管理电路、外部天线等的各种其它电路1400连接。
总线1100可以是工业标准体系结构(ISA,Industry Standard Architecture)总线、外部设备互连(PCI,Peripheral Component)总线或扩展工业标准体系结构(EISA,Extended Industry Standard Component)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示,该图中仅用一条连接线表示,但并不表示仅有一根总线或一种类型的总线。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为,表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分,并且本公开的优选实施方式的范围包括另外的实现,其中可以不按所示出或讨论的顺序,包括根据所涉及的功能按基本同时的方式或按相反的顺序,来执行功能,这应被本公开的实施方式所属技术领域的技术人员所理解。处理器执行上文所描述的各个方法和处理。例如,本公开中的方法实施方式可以被实现为软件程序,其被有形地包含于机器可读介质,例如存储器。在一些实施方式中,软件程序的部分或者全部可以经由存储器和/或通信接口而被载入和/或安装。当软件程序加载到存储器并由处理器执行时,可以执行上文描述的方法中的一个或多个步骤。备选地,在其他实施方式中,处理器可以通过其他任何适当的方式(例如,借助于固件)而被配置为执行上述方法之一。
在流程图中表示或在此以其他方式描述的逻辑和/或步骤,可以具体实现在任何可读存储介质中,以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用,或结合这些指令执行系统、装置或设备而使用。
就本说明书而言,“可读存储介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。可读存储介质的更具体的示例(非穷尽性列表)包括以下:具有一个或多个布线的电连接部(电子装置),便携式计算机盘盒(磁装置),随机存取存储器(RAM),只读存储器(ROM),可擦除可编辑只读存储器(EPROM或闪速存储器),光纤装置,以及便携式只读存储器(CDROM)。另外,可读存储介质甚至可以是可在其上打印程序的纸或其他合适的介质,因为可以例如通过对纸或其他介质进行光学扫描,接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得程序,然后将其存储在存储器中。
应当理解,本公开的各部分可以用硬件、软件或它们的组合来实现。在上述实施方式中,多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件来实现。例如,如果用硬件来实现,和在另一实施方式中一样,可用本领域公知的下列技术中的任一项或他们的组合来实现:具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路,具有合适的组合逻辑门电路的专用集成电路,可编程门阵列(PGA),现场可编程门阵列(FPGA)等。
本技术领域的普通技术人员可以理解实现上述实施方式方法的全部或部分步骤是可以通过程序来指令相关的硬件完成,程序可以存储于一种可读存储介质中,该程序在执行时,包括方法实施方式的步骤之一或其组合。
此外,在本公开各个实施方式中的各功能单元可以集成在一个处理模块中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现,也可以采用软件功能模块的形式实现。集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时,也可以存储在一个可读存储介质中。存储介质可以是只读存储器,磁盘或光盘等。
本公开还提供了一种电子设备,包括:存储器,存储器存储执行指令;以及处理器或其他硬件模块,处理器或其他硬件模块执行存储器存储的执行指令,使得处理器或其他硬件模块执行上述的基于语义的回环检测方法。
本公开还提供了一种可读存储介质,可读存储介质中存储有执行指令,所述执行指令被处理器执行时用于实现上述的基于语义的回环检测方法。
在本说明书的描述中,参考术语“一个实施方式/方式”、“一些实施方式/方式”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施方式/方式或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施方式/方式或示例中。在本说明书中,对上述术语的示意性表述不必须的是相同的实施方式/方式或示例。而且,描述的具体特征、结构、材料或者特点可以在任一个或多个实施方式/方式或示例中以合适的方式结合。此外,在不相互矛盾的情况下,本领域的技术人员可以将本说明书中描述的不同实施方式/方式或示例以及不同实施方式/方式或示例的特征进行结合和组合。
此外,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中,“多个”的含义是至少两个,例如两个,三个等,除非另有明确具体的限定。
本领域的技术人员应当理解,上述实施方式仅仅是为了清楚地说明本公开,而并非是对本公开的范围进行限定。对于所属领域的技术人员而言,在上述公开的基础上还可以做出其它变化或变型,并且这些变化或变型仍处于本公开的范围内。
Claims (10)
1.一种基于语义的回环检测方法,其特征在于,包括:
S110、基于获取的多帧激光点云数据获取相邻激光帧之间的位姿变换信息;
S120、至少基于相邻激光帧之间的位姿变换信息构建位姿图,对位姿图进行优化,基于优化后的位姿图构建全局地图;以及
S130、基于语义信息从所述获取的多帧激光点云数据中获取与当前帧最相似的预设数量的候选帧,计算当前帧与各个候选帧之间的相似度,判断是否出现回环。
2.根据权利要求1所述的基于语义的回环检测方法,其特征在于,还包括:
S140、如果出现回环,获取出现回环的两帧之间的相对位姿误差,将所述相对位姿误差用于步骤S120中的对位姿图进行优化。
3.根据权利要求1所述的基于语义的回环检测方法,其特征在于,S110、基于获取的多帧激光点云数据获取相邻激光帧之间的位姿变换信息,包括:
S111、对获取的包括激光雷达的SLAM相关传感器的传感数据进行时间同步以及对多帧激光点云数据进行点云畸变补偿;
S112、对各帧激光点云数据进行特征提取,以获取各帧激光点云数据的线面特征;以及
S113、对各帧激光点云数据进行特征匹配计算,以获得相邻两帧间的位姿变换信息。
4.根据权利要求1或2所述的基于语义的回环检测方法,其特征在于,S120、至少基于相邻激光帧之间的位姿变换信息构建位姿图,对位姿图进行优化,基于优化后的位姿图构建全局地图,包括:
S121、获取所述多帧激光点云数据中的至少一个关键帧,将关键帧位姿作为位姿图的节点并对关键帧位姿进行优化;以及
S122、基于优化后的关键帧位姿将关键帧的激光点云投影到地图上以获得全局地图。
5.根据权利要求1所述的基于语义的回环检测方法,其特征在于,S130、基于语义信息从所述获取的多帧激光点云数据中获取与当前帧最相似的预设数量的候选帧,计算当前帧与各个候选帧之间的相似度,判断是否出现回环,包括:
S131、以主物体为中心构建当前场景的全局描述子;
S132、基于全局描述子以及与主物体最近的语义物体的类别和距离,构建主物体的语义类别vector;
S133、基于当前场景确定语义分割所需的语义类别,以主物体为中心将主物体周围的360度范围均分为NS等份区域,基于各个等份区域内的语义物体的语义类别以及各个语义类别的语义物体数量获取语义类别直方图;
S134、计算当前帧中主物体的语义类别vector与所述获取的多帧激光点云数据中与该主物体具有相同语义类别的历史语义类别vector之间的语义类别vector相似度,以获取语义类别vector相似度大于或等于阈值相似度的历史关键帧作为候选帧;以及
S135、计算当前帧中主物体的语义类别直方图与各个候选帧中与该主物体具有相同语义类别的历史语义类别直方图的相似度,以获取语义类别直方图相似度最大的历史关键帧;
优选地,还包括:
S136、判断语义类别直方图相似度最大的历史关键帧的语义类别直方图与当前帧的语义类别直方图之间的距离是否小于或等于阈值距离,如果是,则将所述语义类别直方图相似度最大的历史关键帧作为回环帧。
6.根据权利要求4所述的基于语义的回环检测方法,其特征在于,步骤S134中,还包括:
判断当前帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断历史关键帧的历史语义类别vector中对应位置标签是否为车辆标签,如果是,则计算当前帧与该历史关键帧之间的语义类别vector相似度;和/或
判断历史关键帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断当前帧的语义类别vector中对应位置标签是否为车辆标签,如果是,则计算当前帧与该历史关键帧之间的语义类别vector相似度。
7.根据权利要求6所述的基于语义的回环检测方法,其特征在于,步骤S134中,还包括:
判断当前帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断历史关键帧的历史语义类别vector中对应位置标签是否为车辆标签,如果否,则判断当前帧中该主物体的语义类别vector中主物体与车辆标签的距离是否小于所述历史关键帧中该主物体的历史语义类别vector中主物体与对应位置标签的距离,如果小于,则计算当前帧与该历史关键帧之间的语义类别vector相似度时不考虑车辆标签,如果不小于,则考虑车辆标签;和/或
判断历史关键帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断当前帧的语义类别vector中对应位置标签是否为车辆标签,如果否,则判断所述历史关键帧中该主物体的历史语义类别vector中主物体与车辆标签的距离是否小于当前帧中该主物体的语义类别vector中主物体与对应位置标签的距离,如果小于,则计算当前帧与该历史关键帧之间的语义类别vector相似度时不考虑车辆标签,如果不小于,则考虑车辆标签;
优选地,S135、计算当前帧中主物体的语义类别直方图与各个候选帧中与该主物体具有相同语义类别的历史语义类别直方图的相似度,以获取语义类别直方图相似度最大的历史关键帧,包括:
判断当前帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断候选帧的历史语义类别vector中对应位置标签是否为车辆标签,如果否,则判断当前帧中该主物体的语义类别vector中主物体与车辆标签的距离是否小于所述候选帧中该主物体的历史语义类别vector中主物体与对应位置标签的距离,如果小于,则计算当前帧中主物体的语义类别直方图与候选帧中与该主物体具有相同语义类别的历史语义类别直方图的相似度时不将车辆关联区域计入相似度计算,如果不小于,则将车辆关联区域计入相似度计算;和/或
判断候选帧的主物体的语义类别vector中是否存在车辆标签,如果存在,则判断当前帧的语义类别vector中对应位置标签是否为车辆标签,如果否,则判断所述候选帧中该主物体的历史语义类别vector中主物体与车辆标签的距离是否小于当前帧中该主物体的语义类别vector中主物体与对应位置标签的距离,如果小于,则计算当前帧中主物体的语义类别直方图与候选帧中与该主物体具有相同语义类别的历史语义类别直方图的相似度时不将车辆关联区域计入相似度计算,如果不小于,则将车辆关联区域计入相似度计算;
优选地,S140、如果出现回环,获取出现回环的两帧之间的相对位姿误差,包括:
对于当前帧以及当前帧的回环帧,取出主物体在当前帧坐标系以及回环帧坐标系下的位姿,获取相对位姿误差。
8.一种基于语义的回环检测装置,其特征在于,包括:
位姿变换信息获取模块,所述位姿变换信息获取模块基于获取的多帧激光点云数据获取相邻激光帧之间的位姿变换信息;
位姿图/地图构建模块,所述位姿图/地图构建模块至少基于相邻激光帧之间的位姿变换信息构建位姿图,对位姿图进行优化,基于优化后的位姿图构建全局地图;以及
回环检测模块,所述回环检测模块基于语义信息从所述获取的多帧激光点云数据中获取与当前帧最相似的预设数量的候选帧,计算当前帧与各个候选帧之间的相似度,判断是否出现回环;
优选地,还包括:
位姿差获取模块,如果出现回环,所述位姿差获取模块获取出现回环的两帧之间的相对位姿误差;
优选地,还包括:
传感器装置,所述传感器装置至少用于获取激光点云数据。
9.一种电子设备,其特征在于,包括:
存储器,所述存储器存储执行指令;以及
处理器,所述处理器执行所述存储器存储的执行指令,使得所述处理器执行权利要求1至7中任一项所述的方法。
10.一种可读存储介质,其特征在于,所述可读存储介质中存储有执行指令,所述执行指令被处理器执行时用于实现权利要求1至7中任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111621725.9A CN114332221A (zh) | 2021-12-28 | 2021-12-28 | 基于语义的回环检测方法、装置、电子设备及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111621725.9A CN114332221A (zh) | 2021-12-28 | 2021-12-28 | 基于语义的回环检测方法、装置、电子设备及存储介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114332221A true CN114332221A (zh) | 2022-04-12 |
Family
ID=81015568
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111621725.9A Pending CN114332221A (zh) | 2021-12-28 | 2021-12-28 | 基于语义的回环检测方法、装置、电子设备及存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114332221A (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115079202A (zh) * | 2022-06-16 | 2022-09-20 | 智道网联科技(北京)有限公司 | 激光雷达建图方法、装置及电子设备、存储介质 |
CN115240115A (zh) * | 2022-07-27 | 2022-10-25 | 河南工业大学 | 一种语义特征和词袋模型相结合的视觉slam回环检测方法 |
CN115375870A (zh) * | 2022-10-25 | 2022-11-22 | 杭州华橙软件技术有限公司 | 回环检测优化方法、电子设备及计算机可读存储装置 |
CN116105721A (zh) * | 2023-04-11 | 2023-05-12 | 深圳市其域创新科技有限公司 | 地图构建的回环优化方法、装置、设备及存储介质 |
CN117392347A (zh) * | 2023-10-13 | 2024-01-12 | 苏州煋海图科技有限公司 | 一种地图构建方法、装置、计算机设备及可读存储介质 |
-
2021
- 2021-12-28 CN CN202111621725.9A patent/CN114332221A/zh active Pending
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115079202A (zh) * | 2022-06-16 | 2022-09-20 | 智道网联科技(北京)有限公司 | 激光雷达建图方法、装置及电子设备、存储介质 |
CN115240115A (zh) * | 2022-07-27 | 2022-10-25 | 河南工业大学 | 一种语义特征和词袋模型相结合的视觉slam回环检测方法 |
CN115375870A (zh) * | 2022-10-25 | 2022-11-22 | 杭州华橙软件技术有限公司 | 回环检测优化方法、电子设备及计算机可读存储装置 |
CN116105721A (zh) * | 2023-04-11 | 2023-05-12 | 深圳市其域创新科技有限公司 | 地图构建的回环优化方法、装置、设备及存储介质 |
CN116105721B (zh) * | 2023-04-11 | 2023-06-09 | 深圳市其域创新科技有限公司 | 地图构建的回环优化方法、装置、设备及存储介质 |
CN117392347A (zh) * | 2023-10-13 | 2024-01-12 | 苏州煋海图科技有限公司 | 一种地图构建方法、装置、计算机设备及可读存储介质 |
CN117392347B (zh) * | 2023-10-13 | 2024-04-30 | 苏州煋海图科技有限公司 | 一种地图构建方法、装置、计算机设备及可读存储介质 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN114332221A (zh) | 基于语义的回环检测方法、装置、电子设备及存储介质 | |
CN112396650B (zh) | 一种基于图像和激光雷达融合的目标测距系统及方法 | |
CN108445480B (zh) | 基于激光雷达的移动平台自适应扩展目标跟踪系统及方法 | |
Erbs et al. | Moving vehicle detection by optimal segmentation of the dynamic stixel world | |
WO2012086821A1 (ja) | 測位装置、測位方法 | |
CN111340855A (zh) | 一种基于轨迹预测的道路移动目标检测方法 | |
CN111611853A (zh) | 一种传感信息融合方法、装置及存储介质 | |
Andreasson et al. | Mini-SLAM: Minimalistic visual SLAM in large-scale environments based on a new interpretation of image similarity | |
CN112037268B (zh) | 一种动态场景下的基于概率传递模型的环境感知方法 | |
CN115620261A (zh) | 基于多传感器的车辆环境感知方法、系统、设备及介质 | |
CN115908539A (zh) | 一种目标体积自动测量方法和装置、存储介质 | |
Fu et al. | Camera-based semantic enhanced vehicle segmentation for planar lidar | |
Tian et al. | DL-SLOT: Tightly-Coupled Dynamic LiDAR SLAM and 3D Object Tracking Based on Collaborative Graph Optimization | |
Tsintotas et al. | Visual place recognition for simultaneous localization and mapping | |
Meuter et al. | 3D traffic sign tracking using a particle filter | |
CN116978009A (zh) | 基于4d毫米波雷达的动态物体滤除方法 | |
Chen et al. | Appearance-based topological Bayesian inference for loop-closing detection in a cross-country environment | |
CN116385997A (zh) | 一种车载障碍物精确感知方法、系统及存储介质 | |
CN111476828B (zh) | 多视点动物群体跟踪方法及装置 | |
CN114973195A (zh) | 基于多信息融合的车辆追踪方法、装置及系统 | |
CN114926536A (zh) | 一种基于语义的定位与建图方法、系统及智能机器人 | |
Brink | Using probabilistic graphical models to detect dynamic objects for mobile robots | |
CN117593650B (zh) | 基于4d毫米波雷达与sam图像分割的动点滤除视觉slam方法 | |
CN113409268B (zh) | 基于单目相机的可通行区域检测方法、装置及存储介质 | |
CN110544266B (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 |