[发明专利]同时定位建图方法及装置

说明书

本发明实施例提供一种同时定位建图方法及装置。其中，方法包括：对双目图像序列进行特征点提取、匹配和三角化，获取每帧双目图像的三维路标点云；根据各帧双目图像的三维路标点云，获取亲和度矩阵；根据所述亲和度矩阵进行层次聚类，确定每一路标所属的刚体；根据所述各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图；其中，亲和度，为两个路标属于同一刚体的对数概率。本发明实施例提供的同时定位建图方法及装置，根据亲和度进行层次聚类，能更准确地检测出场景中的运动物体并对运动进行建模和位姿估计，并且计算量更小，从而能提高同时定位建图的精度和速度。

技术领域

本发明涉及计算机技术领域，更具体地，涉及一种同时定位建图方法及装置。

背景技术

对周围环境的感知和建模是自主化物件(AuT)导航的基础，目前最常用的方式是同时定位建图。同时定位建图(Simultaneous Localization and Mapping，SLAM)，又称即时定位与建图、同步定位与建图、同时定位与建图，是指载有特定传感器的主体在没有先验信息的情况下，通过运动估计自身的轨迹，并建立周围环境的地图。随着服务机器人和自动驾驶的需求增加，动态场景下的SLAM变得更加重要。

目前动态场景下的SLAM，主要包括剔除动态特征和直接对动态物体的轨迹进行追踪和建模两种技术路线。使用了深度学习的目标检测为SLAM带来了两个问题：第一，深度学习仅能检测物体的语义类别，对于运动不敏感，只能在预定义好的类别中寻找可能的动态物体，通用性不强；第二，在嵌入式设备上其所需的高计算量很难使SLAM系统达到实时需求。而通过运动分析直接得到画面的几何运动信息的方法，虽然通用性更强，但其中有的方法基于实际中并不常见的平行投影假设，有的需要较复杂的变分计算，有的容易陷入数值求解的局部最优导致定位建图失效。

综上，现有同时定位建图方法的精度和速度都存在不足。

发明内容

本发明实施例提供一种同时定位建图方法及装置，用以解决或者至少部分地解决现有技术精度不足的缺陷。

第一方面，本发明实施例提供一种同时定位建图方法，包括：

对双目图像序列进行特征点提取、匹配和三角化，获取每帧双目图像的三维路标点云；

根据各帧双目图像的三维路标点云，获取亲和度矩阵；

根据所述亲和度矩阵进行层次聚类，确定每一路标所属的刚体；

根据所述各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图；

其中，亲和度用于反映两个路标属于同一刚体的对数概率。

优选地，所述根据各帧双目图像的三维路标点云，获取亲和度矩阵的具体步骤包括：

根据各帧双目图像的三维路标点云，获取每两个路标间的亲和度；

根据所述每两个路标间的亲和度，获取亲和度矩阵。

优选地，所述根据所述各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图的具体步骤包括：

对每帧双目图像的三维路标点云进行鲁棒初始化，获取所述每帧双目图像的三维路标点云中各路标的初始化位置；

对于所述每帧双目图像的三维路标点云中每一刚体包括的路标，利用因子图对所述刚体包括的路标的位置进行更新，获取所述每帧双目图像新的三维路标点云。

优选地，所述根据所述各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图之后，还包括：

根据各帧双目图像新的三维路标点云，获取新的亲和度矩阵；

对所述新的亲和度矩阵进行层次聚类，确定各刚体包括的路标；