CN110751722B - 同时定位建图方法及装置 - Google Patents
同时定位建图方法及装置 Download PDFInfo
- Publication number
- CN110751722B CN110751722B CN201910849671.8A CN201910849671A CN110751722B CN 110751722 B CN110751722 B CN 110751722B CN 201910849671 A CN201910849671 A CN 201910849671A CN 110751722 B CN110751722 B CN 110751722B
- Authority
- CN
- China
- Prior art keywords
- frame
- binocular image
- point cloud
- dimensional
- rigid body
- 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
Links
Images
Classifications
-
- 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
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/23—Clustering techniques
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T15/00—3D [Three Dimensional] image rendering
- G06T15/005—General purpose rendering architectures
Abstract
本发明实施例提供一种同时定位建图方法及装置。其中，方法包括：对双目图像序列进行特征点提取、匹配和三角化，获取每帧双目图像的三维路标点云；根据各帧双目图像的三维路标点云，获取亲和度矩阵；根据所述亲和度矩阵进行层次聚类，确定每一路标所属的刚体；根据所述各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图；其中，亲和度，为两个路标属于同一刚体的对数概率。本发明实施例提供的同时定位建图方法及装置，根据亲和度进行层次聚类，能更准确地检测出场景中的运动物体并对运动进行建模和位姿估计，并且计算量更小，从而能提高同时定位建图的精度和速度。
Description
技术领域
本发明涉及计算机技术领域，更具体地，涉及一种同时定位建图方法及装置。
背景技术
对周围环境的感知和建模是自主化物件(AuT)导航的基础，目前最常用的方式是同时定位建图。同时定位建图(Simultaneous Localization and Mapping，SLAM)，又称即时定位与建图、同步定位与建图、同时定位与建图，是指载有特定传感器的主体在没有先验信息的情况下，通过运动估计自身的轨迹，并建立周围环境的地图。随着服务机器人和自动驾驶的需求增加，动态场景下的SLAM变得更加重要。
目前动态场景下的SLAM，主要包括剔除动态特征和直接对动态物体的轨迹进行追踪和建模两种技术路线。使用了深度学习的目标检测为SLAM带来了两个问题：第一，深度学习仅能检测物体的语义类别，对于运动不敏感，只能在预定义好的类别中寻找可能的动态物体，通用性不强；第二，在嵌入式设备上其所需的高计算量很难使SLAM系统达到实时需求。而通过运动分析直接得到画面的几何运动信息的方法，虽然通用性更强，但其中有的方法基于实际中并不常见的平行投影假设，有的需要较复杂的变分计算，有的容易陷入数值求解的局部最优导致定位建图失效。
综上，现有同时定位建图方法的精度和速度都存在不足。
发明内容
本发明实施例提供一种同时定位建图方法及装置，用以解决或者至少部分地解决现有技术精度不足的缺陷。
第一方面，本发明实施例提供一种同时定位建图方法，包括：
对双目图像序列进行特征点提取、匹配和三角化，获取每帧双目图像的三维路标点云；
根据各帧双目图像的三维路标点云，获取亲和度矩阵；
根据所述亲和度矩阵进行层次聚类，确定每一路标所属的刚体；
根据所述各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图；
其中，亲和度用于反映两个路标属于同一刚体的对数概率。
优选地，所述根据各帧双目图像的三维路标点云，获取亲和度矩阵的具体步骤包括：
根据各帧双目图像的三维路标点云，获取每两个路标间的亲和度；
根据所述每两个路标间的亲和度，获取亲和度矩阵。
优选地，所述根据所述各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图的具体步骤包括：
对每帧双目图像的三维路标点云进行鲁棒初始化，获取所述每帧双目图像的三维路标点云中各路标的初始化位置；
对于所述每帧双目图像的三维路标点云中每一刚体包括的路标，利用因子图对所述刚体包括的路标的位置进行更新，获取所述每帧双目图像新的三维路标点云。
优选地，所述根据所述各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图之后，还包括：
根据各帧双目图像新的三维路标点云，获取新的亲和度矩阵；
对所述新的亲和度矩阵进行层次聚类，确定各刚体包括的路标；
根据鲁棒初始化方法和因子图对新的每帧的三维路标点云进行更新。
优选地，对每帧双目图像的三维路标点云进行鲁棒初始化，获取所述每帧双目图像的三维路标点云中各路标的初始化位置的具体步骤包括：
对于当前帧双目图像的三维路标点云中的任一路标，根据所述任一路标的高斯混合模型，确定使得所述任一路标的注册残差最小的两帧变换；
其中，所述两帧变换指所述当前帧双目图像与上一帧双目图像之间的变换。
优选地，对于所述每帧双目图像的三维路标点云中每一刚体包括的路标，利用因子图对所述刚体包括的路标的位置进行更新的具体步骤包括：
根据所述上一帧双目图像的三维路标点云中所述任一路标的位置更新结果和所述两帧变换，获取所述当前帧双目图像的三维路标点云中所述任一路标的初始化位置；
根据所述当前帧双目图像的三维路标点云中各路标的初始化位置，构建因子图；
对所述因子图的能量函数进行解耦式求解，获取所述当前帧双目图像的三维路标点云中每一刚体包括的路标更新后的位置。
优选地，对所述亲和度矩阵进行层次聚类，确定每一路标所属的刚体的具体步骤包括：
根据最长距离法进行层次聚类，根据聚类结果确定各刚体，并确定每一路标所属的刚体。
第二方面，本发明实施例提供一种同时定位建图装置，包括：
提取模块，用于对双目图像序列进行特征点提取、匹配和三角化，获取每帧双目图像的三维路标点云；
获取模块，用于根据各帧双目图像的三维路标点云，获取亲和度矩阵；
聚类模块，用于根据所述亲和度矩阵进行层次聚类，确定每一路标所属的刚体；
优化模块，用于根据所述各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图；
其中，亲和度用于反映两个路标属于同一刚体的对数概率。
第三方面，本发明实施例提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，执行所述程序时实现如第一方面的各种可能的实现方式中任一种可能的实现方式所提供的同时定位建图方法的步骤。
第四方面，本发明实施例提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如第一方面的各种可能的实现方式中任一种可能的实现方式所提供的同时定位建图方法的步骤。
本发明实施例提供的同时定位建图方法及装置，根据亲和度进行层次聚类，能更准确地检测出场景中的运动物体并对运动进行建模和位姿估计，并且计算量更小，从而能提高同时定位建图的精度和速度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为根据本发明实施例提供的同时定位建图方法的流程示意图；
图2为根据本发明实施例提供的同时定位建图系统的结构示意图；
图3为根据本发明实施例提供的电子设备的实体结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为了克服现有技术的上述问题，本发明实施例提供一种同时定位建图方法及装置，其发明构思是，根据两个路标属于同一个刚体的对数概率，对各路标进行层次聚类，确定图像序列中的动态物体和静态背景，实现目标检测和追踪，从而根据目标检测和追踪结果进行同时定位建图，步骤简单，同时定位建图方法的速度和精度更高。
图1为根据本发明实施例提供的同时定位建图方法的流程示意图。如图1所示，该方法包括：步骤S101、对双目图像序列进行特征点提取、匹配和三角化，获取每帧双目图像的三维路标点云。
需要说明的是，本发明实施例提供的同时定位建图方法适用于动态场景，能自动根据双目视频恢复出相机和运动物体的轨迹，在自动驾驶和场景理解方面有重要应用。
具体地，对于双目图像序列中的每一帧，可以先利用通常的特征点提取方法，对该帧中的两幅图像进行特征点提取，获取每幅图像中的特征点；获取每幅图像中的特征点之后，对上述两幅图像中的特征点进行匹配，获取该帧双目图像的各特征点。
可以理解的是，任一特征点为二维特征点。
对双目图像序列中每相邻的两帧进行特征点匹配，获取双目图像序列的各特征点。
例如，可以通过SuperPoint特征提取器对每幅图像中的特征点位置进行计算，并解码出每个特征点的特征描述算子；对于每一帧中的两幅图像，可以按照特征描述算子和位置特征进行最近邻搜索，获取特征点匹配的结果，将相匹配的特征点用相同的编号；对于任意相邻的两帧双目图像，可以按照特征描述算子和位置特征进行最近邻搜索，获取特征点匹配的结果。
对于每一帧双目图像，利用通常的双目相机的三角化公式对该帧双目图像中的每一个二维特征点进行三维化，将二维空间中的特征点转换为三维空间中的路标，得到该帧双目图像的三维路标点云。
双目图像序列中第i个路标，若从第t帧双目图像中提取出该路标，记为
若从第t+1帧双目图像中提取出该路标，记为
上述三角化过程包含了从二维图像特征点噪声到三维噪声的传播，将第t帧双目图像的三维路标点云中三维噪声的协方差矩阵记为
其中，i为上标，表示从双目图像序列中提取出的第i个路标，不表示指数含义；下标t表示第t帧双目图像。
步骤S102、根据各帧双目图像的三维路标点云，获取亲和度矩阵。
其中，亲和度用于反映两个路标属于同一刚体的对数概率。
具体地，亲和度矩阵D的第i行、第j列的元素dij，反映了双目图像序列中的第i个路标和第j个路标属于同一个刚体的对数概率。其中，上标ij表示第i行、第j列，不表示指数。
亲和度矩阵中的每个元素(即亲和度)，为噪声不敏感的马氏距离，用于度量长序列中两个路标在三维空间中的距离的变化量，同时也考虑了在二维图像空间中特征点之间的二维距离。
对于双目图像序列中的任意两个路标，可以根据各帧双目图像的三维路标点云，获取该两个路标在多帧双目图像的三维路标点云中的马氏距离变化，作为该两个路标之间的亲和度，从而可以根据每两个路标之间的亲和度，构建亲和度矩阵。
步骤S103、根据亲和度矩阵进行层次聚类，确定每一路标所属的刚体。
具体地，进行自底向上的层次聚类的过程中，初始时将每一个路标分别作为单独一类；每次迭代时，根据亲和度矩阵计算各个类之间的距离，将最近的两个类聚合成一个新的类，并更新该新的类与其他类的距离；通过不断重复上述过程，直至获得多个类，作为聚类结果。
通过层次聚类获得的每一个类，均作为一个刚体，该刚体为该类中的各路标所属的刚体，即该刚体包括的每一路标均为属于该刚体的路标。聚类结果，就是每一刚体的路标分配。刚体，为双目图像序列中出现的刚体。
步骤S104、根据各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图。
具体地，基于每一刚体包括的路标，根据不同帧双目图像的三维路标点云中同一刚体包括的路标，可以实现帧间的运动目标(刚体)跟踪；还可以利用通常的特征点法中的姿态估计，结合各帧双目图像的三维路标点云，对相机和各刚体的位姿进行估计，回复；根据相机和各刚体的位姿，可以获取跟踪地图(即稀疏地图)，实现同时定位建图。
本发明实施例根据亲和度进行层次聚类，能更准确地检测出场景中的运动物体并对运动进行建模和位姿估计，并且计算量更小，从而能提高同时定位建图的精度和速度。
基于上述各实施例的内容，根据各帧双目图像的三维路标点云，获取亲和度矩阵的具体步骤包括：根据各帧双目图像的三维路标点云，获取每两个路标间的亲和度；根据每两个路标间的亲和度，获取亲和度矩阵。
具体地，亲和度矩阵D的第i行、第j列的元素dij的计算公式为
其中，
表示双目图像序列中的第i个路标和第j个路标在第t帧双目图像的三维路标点云中的欧氏距离；
表示第i个路标在第t帧双目图像的三维路标点云中的坐标；
表示第j个路标在第t帧双目图像的三维路标点云中的坐标；
和
为中间变量；
表示
的高斯方差。
其中，上标×表示该向量相对的局部坐标系为×，高斯方差
是一个标量。
从上述公式可以看出，在计算dij时考虑到了特征点提取的误差，因此，亲和度对噪声不敏感，从而能减少噪声对定位建图结果的影响。
通过上述步骤确定每一个dij(1≤i≤n，1≤j≤n，n表示路标的个数)之后，即可获得亲和度矩阵D。
本发明实施例通过构建噪声不敏感的路标亲和性矩阵，能减少噪声对定位建图结果的影响，获得精度更高的定位建图结果。
基于上述各实施例的内容，根据各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图的具体步骤包括：对每帧双目图像的三维路标点云进行鲁棒初始化，获取每帧双目图像的三维路标点云中各路标的初始化位置。
具体地，注册步骤，可以利用噪声敏感的鲁棒初始化方法对相机与刚体位姿进行初始化，获取每帧双目图像的三维路标点云中各路标的初始化位置。
对于每帧双目图像的三维路标点云中每一刚体包括的路标，利用因子图对刚体包括的路标的位置进行更新，获取每帧双目图像新的三维路标点云。
具体地，更新步骤，可以利用因子图方法对每帧双目图像的三维路标点云中各路标的初始化位置进行更新，获取更准确的各路标的位置，从而获得每帧双目图像新的三维路标点云。
本发明实施例通过鲁棒初始化与因子图优化，能更准确地检测出场景中的运动物体并对运动进行建模和位姿估计，从而能提高同时定位建图的精度。
基于上述各实施例的内容，根据各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图之后，还包括：根据各帧双目图像新的三维路标点云，获取新的亲和度矩阵；对新的亲和度矩阵进行层次聚类，确定各刚体包括的路标；根据鲁棒初始化方法和因子图对新的每帧的三维路标点云进行更新。
具体地，获得各帧双目图像新的三维路标点云之后，可以基于各帧双目图像新的三维路标点云，重复步骤S102至步骤S104，进行迭代，直至达到预设的迭代次数或者迭代收敛，使得聚类结果和位姿估计结果能相互促进，实现对路标的聚类结果，及相机和各刚体的位姿进行进一步精化。
由于初始时特征点检测和三角化存在较多噪声，可能导致聚类不准确，而经过了因子图优化之后，噪声减小，就能重新计算聚类，更准确的聚类也使得优化求解的结果更好，二者相互促进、相互增强。
本发明实施例通过迭代的方法不断精化聚类结果和位姿估计结果，能进一步提高同时定位建图的精度。
基于上述各实施例的内容，对每帧双目图像的三维路标点云进行鲁棒初始化，获取每帧双目图像的三维路标点云中各路标的初始化位置的具体步骤包括：对于当前帧双目图像的三维路标点云中的任一路标，根据任一路标的高斯混合模型，确定使得任一路标的注册残差最小的两帧变换。
其中，两帧变换，当前帧双目图像与上一帧双目图像之间的变换。
需要说明的是，鲁棒初始化和因子图优化都是逐帧进行。
具体地，步骤S104的注册步骤，对第i个路标(1≤i≤n，n表示路标的个数)维护一个高斯混合模型
其中，上标i不表示指数；下标t表示第t帧。
对于高斯混合模型
在全部两帧变换T中确定一个最合适的两帧变换Tt，使得在最大后验的意义下路标注册残差最小。T和Tt为均变换矩阵。
其中，g表示高斯混合模型
中的元素；Σg表示元素g对应的方差；
为中间变量；方差
R是变换矩阵T的旋转部分，
中的上标T表示转置；
表示对第t-1帧双目图像的三维路标点云进行更新得到的第i个路标的位置；
表示对第t-1帧双目图像的三维路标点云进行更新前第i个路标的位置。
本发明实施例通过使用噪声敏感的鲁棒初始化方法对相机与各刚体位姿进行初始化，能基于初始化结果获取更准确的各路标的位置，从而获得每帧双目图像新的三维路标点云。
基于上述各实施例的内容，对于每帧双目图像的三维路标点云中每一刚体包括的路标，利用因子图对刚体包括的路标的位置进行更新的具体步骤包括：根据上一帧双目图像的三维路标点云中任一路标的位置更新结果和两帧变换，获取当前帧双目图像的三维路标点云中任一路标的初始化位置。
具体地，在步骤S104的更新步骤中，每一路标点的位置会被上一步计算出的位姿进行更新。
对第t帧双目图像的三维路标点云进行更新得到的第i个路标的初始化位置
其中，方差
Rt是变换矩阵Tt的旋转部分；
中的上标T表示转置。
若某一路标在观测上有缺失(即上一帧中存在，而当前帧中不存在)，则不对其进行更新，也不将其加入能量函数进行运算。
根据当前帧双目图像的三维路标点云中各路标的初始化位置，构建因子图。
具体地，根据第t帧双目图像的三维路标点云进行更新得到的各路标的初始化位置，构建因子图。
对因子图的能量函数进行解耦式求解，获取当前帧双目图像的三维路标点云中每一刚体包括的路标更新后的位置。
具体地，通过因子图进行位姿优化。
因子图优化目标能量函数的表达式为
其中，ρ(·)为鲁棒核函数；π(·)为双目投影函数；q表示刚体(包括静态背景)；
表示相机在t时刻的位姿；
表示刚体q在t时刻的位姿；
表示第i个路标在第t帧双目图像的三维路标点云中的坐标；
表示刚体q应用上述鲁棒初始化技术之后所得到的
其参考坐标系需要转换为刚体q的局部坐标系；t时刻指相机拍摄第t帧双目图像的时刻。
完全耦合式的因子图求解方法运算量较大，求解较为复杂，因此，本发明实施例采用解耦式因子图求解方法，即单独对每个能量函数的组成部分Eq进行求解，将相机位姿解算和动态刚体位姿计算分成两步计算，可在不损失精度的情况下加快计算速度，大大提升了效率。
本发明实施例通过解耦式因子图优化，能更准确地检测出场景中的运动物体并对运动进行建模和位姿估计，并且计算更加简单、计算量更小，从而能提高同时定位建图的精度和速度。
基于上述各实施例的内容，对亲和度矩阵进行层次聚类，确定每一路标所属的刚体的具体步骤包括：根据最长距离法进行层次聚类，根据聚类结果确定各刚体，并确定每一路标所属的刚体。
具体地，根据最长距离法进行层次聚类，即合并亲和性矩阵中亲和度最大的两个类，并将新类到其他类的距离更新为合并前的两个类到上述其他类的距离的最大值。
通过不断迭代合并，直至各类间的亲和性分数均大于预设的阈值∈，获得聚类结果。该算法的计算复杂度为O(n2logn)，其中，n表示路标的个数。
通过层次聚类获得的每一个类，均作为一个刚体，该刚体为该类中的各路标所属的刚体，即该刚体包括的每一路标均为属于该刚体的路标。
本发明实施例通过最长距离法进行层次聚类，能更简单、方便、准确地检测出场景中的运动物体，从而能提高同时定位建图的精度和速度。
图2为根据本发明实施例提供的同时定位建图装置的结构示意图。基于上述各实施例的内容，如图2所示，该装置包括提取模块201、获取模块202、聚类模块203和优化模块204，其中：
提取模块201，用于对双目图像序列进行特征点提取、匹配和三角化，获取每帧双目图像的三维路标点云；
获取模块202，用于根据各帧双目图像的三维路标点云，获取亲和度矩阵；
聚类模块203，用于根据亲和度矩阵进行层次聚类，确定每一路标所属的刚体；
优化模块204，用于根据各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图；
其中，亲和度用于反映两个路标属于同一刚体的对数概率。
具体地，提取模块201对于双目图像序列中的每一帧，可以先利用通常的特征点提取方法，对该帧中的两幅图像进行特征点提取，获取每幅图像中的特征点；获取每幅图像中的特征点之后，对上述两幅图像中的特征点进行匹配，获取该帧双目图像的各特征点；对双目图像序列中每相邻的两帧进行特征点匹配，获取双目图像序列的各特征点；对于每一帧双目图像，利用通常的双目相机的三角化公式对该帧双目图像中的每一个二维特征点进行三维化，将二维空间中的特征点转换为三维空间中的路标，得到该帧双目图像的三维路标点云。
对于双目图像序列中的任意两个路标，获取模块202可以根据各帧双目图像的三维路标点云，获取该两个路标在多帧双目图像的三维路标点云中的马氏距离变化，作为该两个路标之间的亲和度，从而可以根据每两个路标之间的亲和度，构建亲和度矩阵。
聚类模块203根据亲和度矩阵进行层次聚类，将获得的每一个类，均作为一个刚体，获得每一刚体的路标分配。
优化模块204可以根据不同同帧双目图像的三维路标点云中同一刚体包括的路标，可以实现帧间的运动目标(刚体)跟踪；还可以利用通常的特征点法中的姿态估计，结合各帧双目图像的三维路标点云，对相机和各刚体的位姿进行估计；根据相机和各刚体的位姿，可以获取跟踪地图(即稀疏地图)，实现同时定位建图。
本发明实施例提供的同时定位建图装置，用于执行本发明上述各实施例提供的同时定位建图方法，该同时定位建图装置包括的各模块实现相应功能的具体方法和流程详见上述同时定位建图方法的实施例，此处不再赘述。
该同时定位建图装置用于前述各实施例的同时定位建图方法。因此，在前述各实施例中的同时定位建图方法中的描述和定义，可以用于本发明实施例中各执行模块的理解。
本发明实施例根据亲和度进行层次聚类，能更准确地检测出场景中的运动物体并对运动进行建模和位姿估计，并且计算量更小，从而能提高同时定位建图的精度和速度。
图3为根据本发明实施例提供的电子设备的实体结构示意图。基于上述实施例的内容，如图3所示，该电子设备可以包括：处理器(processor)301、存储器(memory)302和总线303；其中，处理器301和存储器302通过总线303完成相互间的通信；处理器301用于调用存储在存储器302中并可在处理器301上运行的计算机程序指令，以执行上述各方法实施例所提供的同时定位建图方法，例如包括：对双目图像序列进行特征点提取、匹配和三角化，获取每帧双目图像的三维路标点云；根据各帧双目图像的三维路标点云，获取亲和度矩阵；根据亲和度矩阵进行层次聚类，确定每一路标所属的刚体；根据各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图；其中，亲和度用于反映两个路标属于同一刚体的对数概率。
本发明另一实施例公开一种计算机程序产品，计算机程序产品包括存储在非暂态计算机可读存储介质上的计算机程序，计算机程序包括程序指令，当程序指令被计算机执行时，计算机能够执行上述各方法实施例所提供的同时定位建图方法，例如包括：对双目图像序列进行特征点提取、匹配和三角化，获取每帧双目图像的三维路标点云；根据各帧双目图像的三维路标点云，获取亲和度矩阵；根据亲和度矩阵进行层次聚类，确定每一路标所属的刚体；根据各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图；其中，亲和度用于反映两个路标属于同一刚体的对数概率。
此外，上述的存储器302中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实施例的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
本发明另一实施例提供一种非暂态计算机可读存储介质，非暂态计算机可读存储介质存储计算机指令，计算机指令使计算机执行上述各方法实施例所提供的同时定位建图方法，例如包括：对双目图像序列进行特征点提取、匹配和三角化，获取每帧双目图像的三维路标点云；根据各帧双目图像的三维路标点云，获取亲和度矩阵；根据亲和度矩阵进行层次聚类，确定每一路标所属的刚体；根据各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图；其中，亲和度用于反映两个路标属于同一刚体的对数概率。
以上所描述的装置实施例仅仅是示意性的，其中作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行上述各个实施例或者实施例的某些部分的方法。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (8)
1.一种同时定位建图方法，其特征在于，包括：
对双目图像序列进行特征点提取、匹配和三角化，获取每帧双目图像的三维路标点云；
根据各帧双目图像的三维路标点云，获取亲和度矩阵；
根据所述亲和度矩阵进行层次聚类，确定每一路标所属的刚体；
根据所述各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图；
其中，亲和度用于反映两个路标属于同一刚体的对数概率；
所述根据所述各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图的具体步骤包括：
对每帧双目图像的三维路标点云进行鲁棒初始化，获取所述每帧双目图像的三维路标点云中各路标的初始化位置；
对于所述每帧双目图像的三维路标点云中每一刚体包括的路标，利用因子图对所述刚体包括的路标的位置进行更新，获取所述每帧双目图像新的三维路标点云；
对每帧双目图像的三维路标点云进行鲁棒初始化，获取所述每帧双目图像的三维路标点云中各路标的初始化位置的具体步骤包括：
对于当前帧双目图像的三维路标点云中的任一路标，根据所述任一路标的高斯混合模型，确定使得所述任一路标的注册残差最小的两帧变换；
其中，所述两帧变换指所述当前帧双目图像与上一帧双目图像之间的变换。
2.根据权利要求1所述的同时定位建图方法，其特征在于，所述根据各帧双目图像的三维路标点云，获取亲和度矩阵的具体步骤包括：
根据各帧双目图像的三维路标点云，获取每两个路标间的亲和度；
根据所述每两个路标间的亲和度，获取亲和度矩阵。
3.根据权利要求1所述的同时定位建图方法，其特征在于，所述根据所述各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图之后，还包括：
根据各帧双目图像新的三维路标点云，获取新的亲和度矩阵；
对所述新的亲和度矩阵进行层次聚类，确定各刚体包括的路标；
根据鲁棒初始化方法和因子图对新的每帧的三维路标点云进行更新。
4.根据权利要求1所述的同时定位建图方法，其特征在于，对于所述每帧双目图像的三维路标点云中每一刚体包括的路标，利用因子图对所述刚体包括的路标的位置进行更新的具体步骤包括：
根据所述上一帧双目图像的三维路标点云中所述任一路标的位置更新结果和所述两帧变换，获取所述当前帧双目图像的三维路标点云中所述任一路标的初始化位置；
根据所述当前帧双目图像的三维路标点云中各路标的初始化位置，构建因子图；
对所述因子图的能量函数进行解耦式求解，获取所述当前帧双目图像的三维路标点云中每一刚体包括的路标更新后的位置。
5.根据权利要求1至4任一项所述的同时定位建图方法，其特征在于，对所述亲和度矩阵进行层次聚类，确定每一路标所属的刚体的具体步骤包括：
根据最长距离法进行层次聚类，根据聚类结果确定各刚体，并确定每一路标所属的刚体。
6.一种同时定位建图装置，其特征在于，包括：
提取模块，用于对双目图像序列进行特征点提取、匹配和三角化，获取每帧双目图像的三维路标点云；
获取模块，用于根据各帧双目图像的三维路标点云，获取亲和度矩阵；
聚类模块，用于根据所述亲和度矩阵进行层次聚类，确定每一路标所属的刚体；
优化模块，用于根据所述各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图；
其中，亲和度用于反映两个路标属于同一刚体的对数概率；
所述优化模块，具体用于对每帧双目图像的三维路标点云进行鲁棒初始化，获取所述每帧双目图像的三维路标点云中各路标的初始化位置；对于所述每帧双目图像的三维路标点云中每一刚体包括的路标，利用因子图对所述刚体包括的路标的位置进行更新，获取所述每帧双目图像新的三维路标点云；
对每帧双目图像的三维路标点云进行鲁棒初始化，获取所述每帧双目图像的三维路标点云中各路标的初始化位置的具体步骤包括：
对于当前帧双目图像的三维路标点云中的任一路标，根据所述任一路标的高斯混合模型，确定使得所述任一路标的注册残差最小的两帧变换；
其中，所述两帧变换指所述当前帧双目图像与上一帧双目图像之间的变换。
7.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1至5任一项所述的同时定位建图方法的步骤。
8.一种非暂态计算机可读存储介质，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时实现如权利要求1至5任一项所述的同时定位建图方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910849671.8A CN110751722B (zh) | 2019-09-09 | 2019-09-09 | 同时定位建图方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910849671.8A CN110751722B (zh) | 2019-09-09 | 2019-09-09 | 同时定位建图方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110751722A CN110751722A (zh) | 2020-02-04 |
| CN110751722B true CN110751722B (zh) | 2021-06-15 |
Family
ID=69276223
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910849671.8A Active CN110751722B (zh) | 2019-09-09 | 2019-09-09 | 同时定位建图方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110751722B (zh) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2021164688A1 (en) * | 2020-02-19 | 2021-08-26 | Guangdong Oppo Mobile Telecommunications Corp., Ltd. | Methods for localization, electronic device and storage medium |
| CN112258575B (zh) * | 2020-10-13 | 2022-12-02 | 浙江大学 | 一种同步定位和地图构建中物体的快速识别方法 |
| CN115183778A (zh) * | 2022-07-01 | 2022-10-14 | 北京斯年智驾科技有限公司 | 一种基于码头石墩的建图方法、装置、设备以及介质 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108460779A (zh) * | 2018-02-12 | 2018-08-28 | 浙江大学 | 一种动态环境下的移动机器人图像视觉定位方法 |
| EP3265845A4 (en) * | 2015-03-05 | 2019-01-09 | Commonwealth Scientific and Industrial Research Organisation | STRUCTURE MODELING |
| CN110084272A (zh) * | 2019-03-26 | 2019-08-02 | 哈尔滨工业大学（深圳） | 一种聚类地图创建方法及基于聚类地图和位置描述子匹配的重定位方法 |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP3086196B1 (en) * | 2015-04-21 | 2017-04-05 | Hexagon Technology Center GmbH | Method and control system for surveying and mapping a terrain while operating a bulldozer |
| US10582121B2 (en) * | 2016-01-12 | 2020-03-03 | Mitsubishi Electric Research Laboratories, Inc. | System and method for fusing outputs of sensors having different resolutions |
| CN107480603B (zh) * | 2017-07-27 | 2020-09-18 | 和创懒人(大连)科技有限公司 | 基于slam和深度摄像头的同步建图与物体分割方法 |
| CN110110763B (zh) * | 2019-04-19 | 2023-06-23 | 苏州大学 | 一种基于最大公共子图的栅格地图融合方法 |
-
2019
- 2019-09-09 CN CN201910849671.8A patent/CN110751722B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP3265845A4 (en) * | 2015-03-05 | 2019-01-09 | Commonwealth Scientific and Industrial Research Organisation | STRUCTURE MODELING |
| CN108460779A (zh) * | 2018-02-12 | 2018-08-28 | 浙江大学 | 一种动态环境下的移动机器人图像视觉定位方法 |
| CN110084272A (zh) * | 2019-03-26 | 2019-08-02 | 哈尔滨工业大学（深圳） | 一种聚类地图创建方法及基于聚类地图和位置描述子匹配的重定位方法 |
Non-Patent Citations (5)
| Title |
|---|
| A non-rigid map fusion-based direct SLAM method for endoscopic capsule robots;Mehmet Turan et al;《International Journal of Intelligent Robotics and Applications》;20171231;第399–409页 * |
| Efficient EKF-SLAM Algorithm Based on Measurement Clustering and Real Data Simulations;Hyukjung Lee et al;《2018 IEEE 88th Vehicular Technology Conference (VTC-Fall)》;20180830;第1-5页 * |
| Probabilistic Projective Association and Semantic Guided Relocalization;Sheng Yang et al;《2019 International Conference on Robotics and Automation (ICRA)》;20190524;第7130-7136页 * |
| 基于光流拓扑稀疏加权的同步运动去混叠;郑思凡 等;《光学 精密工程》;20190531;第27卷(第5期);第1188-1195页 * |
| 基于视觉的同时定位与建图优化方法研究;曹雄;《中国优秀硕士学位论文全文数据库 信息科技辑》;20190115;第I138-2606页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110751722A (zh) | 2020-02-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US8199977B2 (en) | System and method for extraction of features from a 3-D point cloud | |
| JP6095018B2 (ja) | 移動オブジェクトの検出及び追跡 | |
| US8238612B2 (en) | Method and apparatus for vision based motion determination | |
| CN109425348B (zh) | 一种同时定位与建图的方法和装置 | |
| CN110751722B (zh) | 同时定位建图方法及装置 | |
| CN108229416B (zh) | 基于语义分割技术的机器人slam方法 | |
| CN110675307A (zh) | 基于vslam的3d稀疏点云到2d栅格图的实现方法 | |
| Jellal et al. | LS-ELAS: Line segment based efficient large scale stereo matching | |
| CN112785705B (zh) | 一种位姿获取方法、装置及移动设备 | |
| US20200226392A1 (en) | Computer vision-based thin object detection | |
| GB2566443A (en) | Cross-source point cloud registration | |
| CN114782499A (zh) | 一种基于光流和视图几何约束的图像静态区域提取方法及装置 | |
| EP3440628A1 (en) | An image processing device, an image processing method, and computer-readable recording medium | |
| CN112634333A (zh) | 一种基于eco算法和卡尔曼滤波的跟踪装置方法及装置 | |
| US10242453B2 (en) | Simultaneous localization and mapping initialization | |
| CN113822996B (zh) | 机器人的位姿估计方法及装置、电子设备、存储介质 | |
| CN111695480B (zh) | 基于单帧图像的实时目标检测与3d定位方法 | |
| CN117132649A (zh) | 人工智能融合北斗卫星导航的船舶视频定位方法及装置 | |
| CN115239776B (zh) | 点云的配准方法、装置、设备和介质 | |
| CN109816726B (zh) | 一种基于深度滤波器的视觉里程计地图更新方法和系统 | |
| CN113763468B (zh) | 一种定位方法、装置、系统及存储介质 | |
| CN111583331B (zh) | 用于同时定位和地图构建的方法及装置 | |
| Miyama | Fast stereo matching with super-pixels using one-way check and score filter | |
| JP7207479B2 (ja) | 建築限界判定方法 | |
| Drulea et al. | An omnidirectional stereo system for logistic plants. Part 2: stereo reconstruction and obstacle detection using digital elevation maps |
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 |