CN111563442B - 基于激光雷达的点云和相机图像数据融合的slam方法及系统 - Google Patents
基于激光雷达的点云和相机图像数据融合的slam方法及系统 Download PDFInfo
- Publication number
- CN111563442B CN111563442B CN202010357986.3A CN202010357986A CN111563442B CN 111563442 B CN111563442 B CN 111563442B CN 202010357986 A CN202010357986 A CN 202010357986A CN 111563442 B CN111563442 B CN 111563442B
- Authority
- CN
- China
- Prior art keywords
- point cloud
- image
- segmentation
- frame
- objects
- 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
- 238000000034 method Methods 0.000 title claims abstract description 54
- 230000011218 segmentation Effects 0.000 claims abstract description 67
- 238000005457 optimization Methods 0.000 claims abstract description 20
- 239000013598 vector Substances 0.000 claims description 43
- 230000004927 fusion Effects 0.000 claims description 25
- 238000001514 detection method Methods 0.000 claims description 19
- 238000003709 image segmentation Methods 0.000 claims description 17
- 238000004422 calculation algorithm Methods 0.000 claims description 11
- 238000012549 training Methods 0.000 claims description 11
- 239000011159 matrix material Substances 0.000 claims description 8
- 238000013135 deep learning Methods 0.000 claims description 5
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000013519 translation Methods 0.000 claims description 3
- 238000000605 extraction Methods 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 5
- 238000013527 convolutional neural network Methods 0.000 description 4
- AYFVYJQAPQTCCC-GBXIJSLDSA-N L-threonine Chemical compound C[C@@H](O)[C@H](N)C(O)=O AYFVYJQAPQTCCC-GBXIJSLDSA-N 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 229910000831 Steel Inorganic materials 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 239000010959 steel Substances 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/10—Terrestrial scenes
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/23—Clustering techniques
- G06F18/232—Non-hierarchical techniques
- G06F18/2321—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions
- G06F18/23213—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions with fixed number of clusters, e.g. K-means clustering
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/24—Classification techniques
- G06F18/241—Classification techniques relating to the classification model, e.g. parametric or non-parametric approaches
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/10—Segmentation; Edge detection
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/50—Depth or shape recovery
- G06T7/55—Depth or shape recovery from multiple images
-
- 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/20—Image preprocessing
- G06V10/26—Segmentation of patterns in the image field; Cutting or merging of image elements to establish the pattern region, e.g. clustering-based techniques; Detection of occlusion
- G06V10/267—Segmentation of patterns in the image field; Cutting or merging of image elements to establish the pattern region, e.g. clustering-based techniques; Detection of occlusion by performing operations on regions, e.g. growing, shrinking or watersheds
-
- 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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/02—Neural networks
- G06N3/04—Architecture, e.g. interconnection topology
- G06N3/045—Combinations of networks
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/02—Neural networks
- G06N3/08—Learning methods
-
- 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/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- 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/20—Special algorithmic details
- G06T2207/20081—Training; Learning
-
- 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/20—Special algorithmic details
- G06T2207/20084—Artificial neural networks [ANN]
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Data Mining & Analysis (AREA)
- Multimedia (AREA)
- Evolutionary Computation (AREA)
- Artificial Intelligence (AREA)
- Remote Sensing (AREA)
- Evolutionary Biology (AREA)
- General Engineering & Computer Science (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Radar, Positioning & Navigation (AREA)
- Bioinformatics & Computational Biology (AREA)
- Probability & Statistics with Applications (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Health & Medical Sciences (AREA)
- Computing Systems (AREA)
- Databases & Information Systems (AREA)
- General Health & Medical Sciences (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Image Analysis (AREA)
Abstract
本发明提供了一种基于激光雷达的点云和相机图像数据融合的slam方法及系统,包括:提取关键帧,并对关键帧图像进行物体实例分割,得到图像中的物体实例;对关键帧的点云进行物体分割,得到点云空间中的物体对象;对图像中的物体实例和点云空间中的物体对象进行融合统一,得到物体集合;根据物体集合,对前后帧的物体进行匹配;根据前后帧的点云匹配的误差、图像的重投影误差以及特征点在前后帧中的物体类别误差,求解得到相机的位姿;根据相机的位姿将携带物体实例信息的图像注册到点云地图中,得到具有图像语义信息的点云地图。本发明提升了物体实例分割的鲁棒性,将语义约束增加到优化方程中,使得求解的位姿的精度更高。
Description
技术领域
本发明涉及移动机器人定位导航技术领域,具体地,涉及一种基于激光雷达的点云和相机图像数据融合的slam方法。尤其地,涉及一种移动机器人基于多传感器融合的slam方法。
背景技术
在移动机器人定位导航领域,同步定位与建图(Simultaneous Localization andMapping,SLAM)是指机器人在工作环境中确定自身的位姿,同时构建环境的地图。SLAM问题的本质是在环境未知的情况下机器人对环境的建模和对自身位姿的估计,为了建立周围环境的模型,需要确定自身的位姿,同时为了确定自身的位姿,又要对环境进行建模。
SLAM问题常常可以分为前端的数据关联和后端的回环优化,现有基于激光雷达和视觉的SLAM方法中,均围绕这两个部分来解决。基于视觉的方法分为直接法和特征点法,直接法将所有像素点均加入投影误差计算,而特征点法仅提取特征点来进行匹配和位姿计算,最终得到特征点地图;基于激光雷达的方法通过点云的匹配来求解位姿并将点云注册到点云地图中。传统的slam方法依赖于点、线等低层次的特征信息来估计位姿,在复杂的室外环境中,尤其是特征稀疏的场景中,其鲁棒性较低。近年来也有语义slam方法,通过语义分割来完成位姿的估计或回环检测,将slam的问题解决方法提升到语义层面。
例如专利文献CN110781262A公开的基于视觉SLAM的语义地图的构建方法,该方法包括:通过RGB-D相机采集目标环境图像信息;根据所述目标环境图像信息选取关键帧并确定关键帧的相机位姿;对所述关键帧进行语义分割,预测图像像素的类别;利用所述关键帧的相机位姿以及所述预测的关键帧像素的类别,采用八叉树结构构建三维语义地图。
专利文献CN110533720A(申请号:201910768052.6)公开了一种基于联合约束的语义SLAM系统与方法,旨在解决解决在像素深度值不稳定情况下相机位姿计算不准确和当动态目标占据相机视野大部分空间时无法计算相机位姿的问题,通过深度约束方法提高了相机位姿估计的准确性,采用极线约束方法提高了相机轨迹的完整性。实现方法为:数据采集模块获取图像序列;神经网络模块获取检测图像和实例分割图像;联合约束模块获取不同的特征点类别集合;数据融合模块获取静态目标实例分割图像和动态目标实例分割图像;视觉前端模块获取深度相机的位姿和三维空间中的路标点集合;后端优化模块获取全局最优的深度相机位姿和路标点;语义地图模块获取语义点云地图。
发明内容
针对现有技术中的缺陷,本发明的目的是提供一种基于激光雷达的点云和相机图像数据融合的slam方法及系统。
根据本发明提供的基于激光雷达的点云和相机图像数据融合的slam方法,包括:
图像分割步骤:提取关键帧,并对关键帧图像进行物体实例分割,得到图像中的物体实例;
点云分割步骤:对关键帧的点云进行物体分割,得到点云空间中的物体对象;
物体分割融合步骤:对图像中的物体实例和点云空间中的物体对象进行融合统一,得到物体集合;
前后帧物体关联步骤:根据物体集合,对前后帧的物体进行匹配;
多约束位姿估计步骤:根据前后帧的点云匹配的误差、图像的重投影误差以及特征点在前后帧中的物体类别误差,建立多约束的非线性最小二乘模型,求解得到相机的位姿;
闭环检测与优化步骤:根据相机的位姿将携带物体实例信息的图像注册到点云地图中,得到具有图像语义信息的点云地图。
优选的,所述图像分割步骤包括:基于深度学习的图像实例分割网络Mask R-CNN来对关键帧的图像进行分割,获得图像中所有的物体实例信息;
选择在室外环境中用双目相机采集图片来作为Mask R-CNN算法的训练集,利用深度学习图像标注工具VGG Image Annotator进行标注,通过模型训练,得到训练参数;
用训练得到的权重模型对每帧关键帧进行预测,对关键帧中的物体进行目标框检测,得到剔除背景后的实例像素级掩码。
优选的,所述点云分割步骤包括:利用欧式聚类方法完成点云的分割,通过判断两点的欧式距离是否小于聚类半径,实现激光点的分类,最终得到物体的分割结果。
优选的,所述物体分割融合步骤包括:根据图像分割的结果和点云分割的结果确定出当前关键帧中存在的物体,剔除误检测对象。
优选的,所述物体分割融合步骤包括:
坐标转化步骤:将图像分割得到的物体的像素点通过以下公式转化到点云空间中:
P′=(x′,y′,z′)=RP+T
其中,R为从相机坐标系向激光坐标系转化的旋转矩阵,T为从相机坐标系向激光坐标系转化的平移矩阵,P=(x,y,z)为相机坐标系中的物体的点的坐标;P′=(x′,y′,z′)为激光坐标系中点的坐标,x,y,x分别为三维坐标系的坐标点;
分割结果融合步骤:对于转化得到的P′点,在点云空间中分割得到的所有物体点云中寻找近邻点Pi,并对每个物体转化后的点到近邻点的距离di求和,用D表示,若D小于预设阈值,则判断图像分割和点云分割均得到了这一物体,并加入最终的分割结果中,否则不加入分割结果中。
优选的,所述前后帧物体关联步骤包括:通过对物体点云提取特征并计算物体之间的特征关联距离,将前一帧中分割得到的物体和当前帧分割得到的物体进行匹配,确定对应关系;
在点云空间中得到物体包含的点,对每个物体通过特征向量进行表示,基于包围框的位置、尺寸、点的数量、点分布直方图四个特征的提取得到特征向量;
将上一帧中的每个物体与当前帧中的每个物体计算特征向量的差,作为关联距离,评估两个物体之间的相似程度,将前后帧的物体根据特征向量进行匹配,将前后帧中特征向量距离最小的物体视为同一物体,并为所有找到匹配的物体分配唯一的ID。
优选的,所述闭环检测与优化步骤包括:根据关键帧的点云特征与图像特征组成特征向量,并将当前帧的特征向量与历史帧的特征向量进行匹配完成闭环检测,构成闭环后利用点云和图像信息来优化位姿序列,并将图像信息添加到点云中构建地图。
优选的,将图像的特征点转化到点云空间中,得到其深度值和激光的强度值;
基于VLAD算法,对图像进行网格划分,对每个网格中心提取稠密sift特征、RGB颜色值,结合获取的深度值和强度值,组成特征向量;
利用k-means算法将特征描述向量进行聚类,得到预设数量的类别;
利用每个网格的特征描述向量与其聚类中心的差值,所有维度的差值组成该图像的F-VLAD向量Fusion-VLAD,将当前关键帧的F-VLAD向量与历史关键帧的F-VLAD向量进行匹配,匹配距离小于预设阈值则认为构成闭环。
优选的,计算所有关键帧的点云匹配误差和图像重投影误差,相加得到联合误差的表示,并进行优化,得到优化后的位姿序列。
根据本发明提供的基于激光雷达的点云和相机图像数据融合的slam系统,包括:
图像分割模块:提取关键帧,并对关键帧图像进行物体实例分割,得到图像中的物体实例;
点云分割模块:对关键帧的点云进行物体分割,得到点云空间中的物体对象;
物体分割融合模块:对图像中的物体实例和点云空间中的物体对象进行融合统一,得到物体集合;
前后帧物体关联模块:根据物体集合,对前后帧的物体进行匹配;
多约束位姿估计模块:根据前后帧的点云匹配的误差、图像的重投影误差以及特征点在前后帧中的物体类别误差,建立多约束的非线性最小二乘模型,求解得到相机的位姿;
闭环检测与优化模块:根据相机的位姿将携带物体实例信息的图像注册到点云地图中,得到具有图像语义信息的点云地图。
与现有技术相比,本发明具有如下的有益效果:
1、本发明将激光点云与视觉的物体分割信息进行融合,提升了物体实例分割的鲁棒性,同时将点云的匹配误差和图像的投影误差统一到同一个优化问题中,并考虑物体在前后帧的对应关系,将语义约束增加到优化方程中,使得求解的位姿的精度更高;
2、本发明在闭环检测与优化中融合了点云与图像的信息,进一步优化了全局的位姿序列;
3、本发明作为一种多传感器融合的slam方法,可以得到更好的位姿估计效果,以及具有图像信息的点云地图。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述,本发明的其它特征、目的和优点将会变得更明显:
图1为本发明的一种点云和图像融合的slam方法各模块示意图;
图2为本发明的一种点云和图像融合的slam方法实施例流程示意图;
图3为本发明的前后帧物体关联模块示意图;
图4为本发明位姿估计方法和现有的方法对比的示意图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明,但不以任何形式限制本发明。应当指出的是,对本领域的普通技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变化和改进。这些都属于本发明的保护范围。
如图1所示,本发明提供一种点云和图像融合的slam方法,分别涉及图像分割模块、点云分割模块、物体分割融合模块、前后帧物体关联模块、多约束位姿估计模块、闭环检测与优化模块。如图2所示为本发明的实施例流程示意图。其中,步骤1涉及两个模块,图像分割模块用于获取每帧图像中的物体实例;点云分割模块用于通过聚类方法同步检测每帧中的物体点云;步骤2涉及一个模块,即物体分割融合模块,用于融合图像分割和点云分割的结果,得到统一的物体描述;步骤3涉及一个模块,即前后帧物体关联模块,同于将前后帧的物体进行匹配,并为每个物体赋予唯一的ID(如图3所示);步骤4涉及一个模块,即多约束位姿估计模块,用于将点云的匹配误差、图像特征点的重投影误差以及特征点的物体类别误差同时进行最小化,求解出相机的精确位姿(如图3所示);步骤5涉及一个模块,即闭环检测与优化模块,用于将关键帧的图像与点云添加到地图中,构建得到具有图像语义信息的点云地图。如图4,为本发明位姿估计方法和现有的方法对比的示意图。
图像分割模块用于获取每帧图像中的物体实例,包含以下步骤:
步骤1:选择在室外环境中用双目相机采集图片来作为Mask R-CNN算法的训练集,利用深度学习图像标注工具VGG Image Annotator(VIA)进行标注,通过模型训练,得到参数。
其中,采用的Mask R-CNN是一种开源的目标检测算法,其特点在于可以对目标的轮廓进行像素级分割,相比于其他的网络可以更准确地完成物体实例分割。
步骤2:用训练得到的权重模型对每帧关键帧进行预测,对关键帧中的物体进行目标框检测,得到剔除背景后的实例像素级掩码。
其中,关键帧的选择主要是考虑当前帧与上一帧的相对运动距离d,只有距离大于一定范围才视为关键帧,防止大量的重复帧出现,具体规则如下:
1)若d<dthre,则当前帧不作为关键帧;
2)若d≥dthre,则当前帧作为关键帧;
其中,dthre是允许的帧间最大运动距离。
点云分割模块用于分割出每帧点云中的物体对象。具体实施方法如下:
采用欧式聚类的方法对点云进行聚类分割,欧式聚类是一种基于欧式距离对点云中的点进行聚类的方法,聚类半径的设置与激光点到激光雷达的距离有关。采用这种可变聚类半径阈值的方法,在基于扫描线的三维点云聚类中能够取得更佳的效果。设置规则如下:聚类半径r的设置方法和激光点的距离d之间的关系为:
其中,r为聚类半径,d为激光扫到的点到激光中心的距离。
物体分割融合模块用于将图像分割与点云分割的结果进行融合,得到最终的物体分割结果。包含以下步骤:
步骤1:将图像分割得到的物体的像素点通过以下公式转化到点云空间中:
P′=(x′,y′,z′)=RP+T
其中,R为从相机坐标系向激光坐标系转化的旋转矩阵,T为从相机坐标系向激光坐标系转化的平移矩阵,P=(x,y,z)为相机坐标系中的物体的点的坐标;P′=(x′,y′,z′)为激光坐标系中点的坐标;
步骤2:将分割结果进行融合。对于转化得到的P′点,在点云空间的物体点云集合中寻找近邻点Pi,并对每个物体其中的点到近邻点的距离di求和,用D表示,假设D小于一定阈值,则认为图像分割和点云分割均得到了该物体,将其加入最终的分割结果中,否则不将其加入分割结果中。
前后帧物体关联模块用于将前后帧的物体进行匹配,并为每个物体赋予唯一的ID,包含以下步骤:
步骤1:物体特征提取。根据物体包含的点,对每个物体通过特征向量进行表示,基于包围框的位置、尺寸、点的数量、点分布直方图四个特征的提取得到其特征向量。
其中,物体点云的包围框位置、尺寸、点的数量都可以简单地获得,点分布直方图是通过统计所有点在x,y,z三个方向的每个单位长度上的分布数量得到的。基于上述四种特征,可分别计算两个物体每种特征的距离,用于衡量该两个物体该特征的相似程度,具体如下所示:
步骤2:物体关联步骤:将上一帧中的每个物体与当前帧中的每个物体计算特征向量的差,作为“关联距离”,如下式所示:
其中,dij表示上一帧的物体i和当前帧的物体j之间的关联距离,dk表示每种特征计算的距离,wk表示特征的权重,由于经过仿真发现位置属性和直方图属性更能反映物体之间的相似性,而其它两条属性容易受到点云分割结果的影响,尤其在点云存在局部遮挡等情况下物体的尺寸和扫描到的点数量可能出现波动,因此在实际使用中本文给予了d1与d4的更大的权重。
基于计算的关联距离,对上一帧中的每个物体,将当前帧中的与其关联距离最小的物体视为同一物体,并为所有找到匹配的物体分配唯一的ID。
多约束位姿估计模块用于将点云的匹配误差、图像特征点的重投影误差以及特征点的物体语义约束误差同时进行最小化,求解出相机的精确位姿。具体地,
步骤1:计算点云的匹配误差和物体语义约束的误差
传统的ICP算法仅仅依赖点与点之间的距离最小来作为约束条件,假设M,P是两个点集,其中P为待配准点集,而M为目标点集,取P中的每个点Pi,在M中找出距离Pi最近的Mi,两个点集之间的位姿变换关系为(R,T),则其最小化的目标函数为:
与语义slam中的语义误差项一致,用下式表示物体类别的误差项:
E1=Ebase+λElabel
其中,λ为权重因子,是用同一物体在前后帧的关联距离通过高斯函数计算得到,即关联距离越大,表示相似程度越低,后面的语义项权重越小。
步骤2:计算图像特征点的重投影误差,对于两帧之间n组投影点,其重投影误差为:
步骤3:构建多约束优化方程,求解相机的位姿:
{Ti},{di}=arg min(βE1+(1-β)αE2)
其中,参数β为权重因子,与环境有关,在图像特征比较丰富的场景下β较小,参数α是从像素误差到空间距离误差的转换因子,和相机的分辨率以及激光投射的范围有关。
闭环检测与优化模块用于执行闭环检测、位姿优化以及将关键帧的图像与点云添加到地图中,构建得到具有图像语义信息的点云地图。具体地:
步骤1:将图像的sift特征点转化到点云空间中,得到其深度值和激光的强度值,基于VLAD算法,首先对图像进行网格划分,对每个网格中心提取稠密sift特征、RGB颜色值,结合获取的深度值和强度值,组成特征向量。
步骤2:利用k-means算法将特征描述向量进行聚类,得到一定数量的类别,即一定数量的聚类中心(假设为k);利用每个网格的特征描述向量与其聚类中心的差值,所有维度的差值组成该图像的F-VLAD向量(Fusion-VLAD),表示为
其中,xj是该图像的一个特征点,xj维度为d,N(x)是离xj最近的聚类中心,ψi是以c为聚类中心的特征点xj的每一维的值与c的每一维的值的差的和。由于这种方法使得该向量的维度较高(k×d),使用PCA降维法对F-VLAD向量进行处理,得到最终的向量表示。
步骤3:将当前关键帧的F-VLAD向量与历史关键帧的F-VLAD向量计算欧式距离,距离小于一定的阈值则认为构成闭环。该阈值在离线环境下通过计算不同相似程度的图像的F-VLAD向量差可以获得。
步骤4:检测到闭环后,利用所有关键帧点云的iCP匹配和图像的重投影误差进行联合优化,假设一共有n个关键帧,点云的匹配误差和图像的重投影误差分别是:
其中,Ei,Ej分别是在每个关键帧的误差,则优化后的位姿序列及其协方差为:
步骤5:构建地图,需要将关键帧的图像映射到三维点云中,关键帧的像素点用[uj,vj]表示,其中,uj,vj分别是横纵坐标,将关键帧映射到三维点云中,如下式所示:
根据本发明提供的基于激光雷达的点云和相机图像数据融合的slam系统,包括:
图像分割模块:提取关键帧,并对关键帧图像进行物体实例分割,得到图像中的物体实例;
点云分割模块:对关键帧的点云进行物体分割,得到点云空间中的物体对象;
物体分割融合模块:对图像中的物体实例和点云空间中的物体对象进行融合统一,得到物体集合;
前后帧物体关联模块:根据物体集合,对前后帧的物体进行匹配;
多约束位姿估计模块:根据前后帧的点云匹配的误差、图像的重投影误差以及特征点在前后帧中的物体类别误差,建立多约束的非线性最小二乘模型,求解得到相机的位姿;
闭环检测与优化模块:根据相机的位姿将携带物体实例信息的图像注册到点云地图中,得到具有图像语义信息的点云地图。
本领域技术人员知道,除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外,完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以,本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件,而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构;也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是,本发明并不局限于上述特定实施方式,本领域技术人员可以在权利要求的范围内做出各种变化或修改,这并不影响本发明的实质内容。在不冲突的情况下,本申请的实施例和实施例中的特征可以任意相互组合。
Claims (10)
1.一种基于激光雷达的点云和相机图像数据融合的slam方法,其特征在于,包括:
图像分割步骤:提取关键帧,并对关键帧图像进行物体实例分割,得到图像中的物体实例;
点云分割步骤:对关键帧的点云进行物体分割,得到点云空间中的物体对象;
物体分割融合步骤:对图像中的物体实例和点云空间中的物体对象进行融合统一,得到物体集合;
前后帧物体关联步骤:根据物体集合,对前后帧的物体进行匹配;
多约束位姿估计步骤:根据前后帧的点云匹配的误差、图像的重投影误差以及特征点在前后帧中的物体类别误差,建立多约束的非线性最小二乘模型,求解得到相机的位姿;
闭环检测与优化步骤:根据相机的位姿将携带物体实例信息的图像注册到点云地图中,得到具有图像语义信息的点云地图。
2.根据权利要求1所述的基于激光雷达的点云和相机图像数据融合的slam方法,其特征在于,所述图像分割步骤包括:基于深度学习的图像实例分割网络Mask R-CNN来对关键帧的图像进行分割,获得图像中所有的物体实例信息;
选择在室外环境中用双目相机采集图片来作为Mask R-CNN算法的训练集,利用深度学习图像标注工具VGG Image Annotator进行标注,通过模型训练,得到训练参数;
用训练得到的权重模型对每帧关键帧进行预测,对关键帧中的物体进行目标框检测,得到剔除背景后的实例像素级掩码。
3.根据权利要求1所述的基于激光雷达的点云和相机图像数据融合的slam方法,其特征在于,所述点云分割步骤包括:利用欧式聚类方法完成点云的分割,通过判断两点的欧式距离是否小于聚类半径,实现激光点的分类,最终得到物体的分割结果。
4.根据权利要求1所述的基于激光雷达的点云和相机图像数据融合的slam方法,其特征在于,所述物体分割融合步骤包括:根据图像分割的结果和点云分割的结果确定出当前关键帧中存在的物体,剔除误检测对象。
5.根据权利要求1所述的基于激光雷达的点云和相机图像数据融合的slam方法,其特征在于,所述物体分割融合步骤包括:
坐标转化步骤:将图像分割得到的物体的像素点通过以下公式转化到点云空间中:
P′=(x′吠y′吠z′)=RP+T
其中,R为从相机坐标系向激光坐标系转化的旋转矩阵,T为从相机坐标系向激光坐标系转化的平移矩阵,P=(x,y,z)为相机坐标系中的物体的点的坐标;P′=(x′,y′,z′)为激光坐标系中点的坐标,x,y,x分别为三维坐标系的坐标点;
6.根据权利要求1所述的基于激光雷达的点云和相机图像数据融合的slam方法,其特征在于,所述前后帧物体关联步骤包括:通过对物体点云提取特征并计算物体之间的特征关联距离,将前一帧中分割得到的物体和当前帧分割得到的物体进行匹配,确定对应关系;
在点云空间中得到物体包含的点,对每个物体通过特征向量进行表示,基于包围框的位置、尺寸、点的数量、点分布直方图四个特征的提取得到特征向量;
将上一帧中的每个物体与当前帧中的每个物体计算特征向量的差,作为关联距离,评估两个物体之间的相似程度,将前后帧的物体根据特征向量进行匹配,将前后帧中特征向量距离最小的物体视为同一物体,并为所有找到匹配的物体分配唯一的ID。
7.根据权利要求1所述的基于激光雷达的点云和相机图像数据融合的slam方法,其特征在于,所述闭环检测与优化步骤包括:根据关键帧的点云特征与图像特征组成特征向量,并将当前帧的特征向量与历史帧的特征向量进行匹配完成闭环检测,构成闭环后利用点云和图像信息来优化位姿序列,并将图像信息添加到点云中构建地图。
8.根据权利要求7所述的基于激光雷达的点云和相机图像数据融合的slam方法,其特征在于,将图像的特征点转化到点云空间中,得到其深度值和激光的强度值;
基于VLAD算法,对图像进行网格划分,对每个网格中心提取稠密sift特征、RGB颜色值,结合获取的深度值和强度值,组成特征向量;
利用k-means算法将特征描述向量进行聚类,得到预设数量的类别;
利用每个网格的特征描述向量与其聚类中心的差值,所有维度的差值组成该图像的F-VLAD向量Fusion-VLAD,将当前关键帧的F-VLAD向量与历史关键帧的F-VLAD向量进行匹配,匹配距离小于预设阈值则认为构成闭环。
9.根据权利要求8所述的基于激光雷达的点云和相机图像数据融合的slam方法,其特征在于,计算所有关键帧的点云匹配误差和图像重投影误差,相加得到联合误差的表示,并进行优化,得到优化后的位姿序列。
10.一种基于激光雷达的点云和相机图像数据融合的slam系统,其特征在于,包括:
图像分割模块:提取关键帧,并对关键帧图像进行物体实例分割,得到图像中的物体实例;
点云分割模块:对关键帧的点云进行物体分割,得到点云空间中的物体对象;
物体分割融合模块:对图像中的物体实例和点云空间中的物体对象进行融合统一,得到物体集合;
前后帧物体关联模块:根据物体集合,对前后帧的物体进行匹配;
多约束位姿估计模块:根据前后帧的点云匹配的误差、图像的重投影误差以及特征点在前后帧中的物体类别误差,建立多约束的非线性最小二乘模型,求解得到相机的位姿;
闭环检测与优化模块:根据相机的位姿将携带物体实例信息的图像注册到点云地图中,得到具有图像语义信息的点云地图。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010357986.3A CN111563442B (zh) | 2020-04-29 | 2020-04-29 | 基于激光雷达的点云和相机图像数据融合的slam方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010357986.3A CN111563442B (zh) | 2020-04-29 | 2020-04-29 | 基于激光雷达的点云和相机图像数据融合的slam方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111563442A CN111563442A (zh) | 2020-08-21 |
CN111563442B true CN111563442B (zh) | 2023-05-02 |
Family
ID=72067960
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010357986.3A Active CN111563442B (zh) | 2020-04-29 | 2020-04-29 | 基于激光雷达的点云和相机图像数据融合的slam方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111563442B (zh) |
Families Citing this family (39)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112102929A (zh) * | 2020-09-11 | 2020-12-18 | 沈阳东软智能医疗科技研究院有限公司 | 医学图像标注方法、装置、存储介质及电子设备 |
CN112631271A (zh) * | 2020-10-09 | 2021-04-09 | 南京凌华微电子科技有限公司 | 一种基于机器人感知融合的地图生成方法 |
CN112070068A (zh) * | 2020-10-13 | 2020-12-11 | 上海美迪索科电子科技有限公司 | 一种地图构建方法、装置、介质及设备 |
CN112419512B (zh) * | 2020-10-13 | 2022-09-13 | 南昌大学 | 一种基于语义信息的空中三维模型修复系统及方法 |
CN114529603B (zh) * | 2020-11-23 | 2023-05-19 | 新疆大学 | 一种基于激光slam与单目视觉slam融合的里程计方法 |
CN112347550B (zh) * | 2020-12-07 | 2022-07-15 | 厦门大学 | 耦合式室内三维语义建图及建模方法 |
CN112233163B (zh) * | 2020-12-14 | 2021-03-30 | 中山大学 | 一种激光雷达立体相机融合的深度估计方法、装置及其介质 |
CN112902966A (zh) * | 2021-01-28 | 2021-06-04 | 开放智能机器(上海)有限公司 | 一种融合定位系统及方法 |
CN112819840B (zh) * | 2021-02-24 | 2022-08-02 | 北京航空航天大学 | 一种融合深度学习与传统处理的高精度图像实例分割方法 |
CN112837371B (zh) * | 2021-02-26 | 2024-05-24 | 梅卡曼德(北京)机器人科技有限公司 | 基于3d匹配的物体抓取方法、装置及计算设备 |
TWI760128B (zh) * | 2021-03-05 | 2022-04-01 | 國立陽明交通大學 | 深度圖像之生成方法、系統以及應用該方法之定位系統 |
CN113168717B (zh) * | 2021-03-11 | 2023-03-03 | 华为技术有限公司 | 一种点云匹配方法及装置、导航方法及设备、定位方法、激光雷达 |
CN112990229A (zh) * | 2021-03-11 | 2021-06-18 | 上海交通大学 | 多模态3d目标检测方法、系统、终端及介质 |
CN113238554A (zh) * | 2021-05-08 | 2021-08-10 | 武汉科技大学 | 一种基于激光与视觉融合slam技术的室内导航方法及系统 |
CN113269803B (zh) * | 2021-06-09 | 2023-01-13 | 中国科学院自动化研究所 | 基于2d激光与深度图像融合的扫描定位方法、系统及设备 |
CN113313803B (zh) * | 2021-06-11 | 2024-04-19 | 梅卡曼德(北京)机器人科技有限公司 | 垛型分析方法、装置、计算设备及计算机存储介质 |
CN113362247B (zh) * | 2021-06-11 | 2023-08-15 | 山东大学 | 一种激光融合多目相机的语义实景三维重建方法及系统 |
CN113436270B (zh) * | 2021-06-18 | 2023-04-25 | 上海商汤临港智能科技有限公司 | 传感器标定方法及装置、电子设备和存储介质 |
CN113379841B (zh) * | 2021-06-21 | 2024-04-30 | 上海仙工智能科技有限公司 | 一种基于相位相关法与因子图的激光slam方法及其可读存储介质 |
CN113671522B (zh) * | 2021-07-07 | 2023-06-27 | 中国人民解放军战略支援部队信息工程大学 | 基于语义约束的动态环境激光slam方法 |
CN113516682B (zh) * | 2021-07-08 | 2023-08-11 | 福州大学 | 一种激光slam的回环检测方法 |
CN113763423B (zh) * | 2021-08-03 | 2024-08-06 | 中国北方车辆研究所 | 一种基于多模态数据的系统性目标识别与跟踪方法 |
CN113609985B (zh) * | 2021-08-05 | 2024-02-23 | 诺亚机器人科技(上海)有限公司 | 物体位姿检测方法、检测设备、机器人及可存储介质 |
CN113724387A (zh) * | 2021-08-10 | 2021-11-30 | 浙江工业大学 | 一种激光与相机融合的地图构建方法 |
CN115967823A (zh) * | 2021-10-09 | 2023-04-14 | 北京字节跳动网络技术有限公司 | 视频封面生成方法、装置、电子设备及可读介质 |
CN113916245B (zh) * | 2021-10-09 | 2024-07-19 | 上海大学 | 一种基于实例分割和vslam的语义地图构建方法 |
CN113935368B (zh) * | 2021-10-13 | 2024-06-07 | 福州大学 | 针对散乱堆叠状态且具有可抓取平面物体的识别、定位及抓取方法 |
CN114721001A (zh) * | 2021-11-17 | 2022-07-08 | 长春理工大学 | 一种基于多传感器融合的移动机器人定位方法 |
CN114398937B (zh) * | 2021-12-01 | 2022-12-27 | 北京航空航天大学 | 一种基于混合注意力机制的图像-激光雷达数据融合方法 |
CN114331966B (zh) * | 2021-12-02 | 2024-02-13 | 北京斯年智驾科技有限公司 | 基于高斯过程占位图估计辅助的港口锁站方法及其系统 |
CN114359493B (zh) * | 2021-12-20 | 2023-01-03 | 中国船舶重工集团公司第七0九研究所 | 一种用于无人船生成三维语义地图的方法和系统 |
CN114413881B (zh) * | 2022-01-07 | 2023-09-01 | 中国第一汽车股份有限公司 | 高精矢量地图的构建方法、装置及存储介质 |
CN114332233B (zh) * | 2022-03-17 | 2022-05-31 | 北京理工大学 | 一种激光slam回环检测方法和系统 |
CN114742893B (zh) * | 2022-06-09 | 2022-10-21 | 浙江大学湖州研究院 | 一种基于深度学习的3d激光数据训练与快速定位方法 |
CN115372972A (zh) * | 2022-08-05 | 2022-11-22 | 西安电子科技大学 | 一种基于交通毫米波雷达点云数据的聚类方法 |
CN117761704B (zh) * | 2023-12-07 | 2024-08-13 | 上海交通大学 | 多机器人相对位置的估计方法及系统 |
CN117635888B (zh) * | 2023-12-07 | 2024-04-26 | 腾讯科技(深圳)有限公司 | 一种数据处理方法及相关装置 |
CN117475397B (zh) * | 2023-12-26 | 2024-03-22 | 安徽蔚来智驾科技有限公司 | 基于多模态传感器的目标标注数据获取方法、介质及设备 |
CN118365888B (zh) * | 2024-06-19 | 2024-09-10 | 广汽埃安新能源汽车股份有限公司 | 去除图像中动态物体的方法、装置、电子设备及存储介质 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109816686A (zh) * | 2019-01-15 | 2019-05-28 | 山东大学 | 基于物体实例匹配的机器人语义slam方法、处理器及机器人 |
CN110097553A (zh) * | 2019-04-10 | 2019-08-06 | 东南大学 | 基于即时定位建图与三维语义分割的语义建图系统 |
WO2019169540A1 (zh) * | 2018-03-06 | 2019-09-12 | 斯坦德机器人(深圳)有限公司 | 紧耦合视觉slam的方法、终端及计算机可读存储介质 |
CN110243370A (zh) * | 2019-05-16 | 2019-09-17 | 西安理工大学 | 一种基于深度学习的室内环境三维语义地图构建方法 |
CN110738673A (zh) * | 2019-10-21 | 2020-01-31 | 哈尔滨理工大学 | 基于实例分割的视觉slam方法 |
CN110827395A (zh) * | 2019-09-09 | 2020-02-21 | 广东工业大学 | 一种适用于动态环境的即时定位与地图构建方法 |
CN111080659A (zh) * | 2019-12-19 | 2020-04-28 | 哈尔滨工业大学 | 一种基于视觉信息的环境语义感知方法 |
-
2020
- 2020-04-29 CN CN202010357986.3A patent/CN111563442B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2019169540A1 (zh) * | 2018-03-06 | 2019-09-12 | 斯坦德机器人(深圳)有限公司 | 紧耦合视觉slam的方法、终端及计算机可读存储介质 |
CN109816686A (zh) * | 2019-01-15 | 2019-05-28 | 山东大学 | 基于物体实例匹配的机器人语义slam方法、处理器及机器人 |
CN110097553A (zh) * | 2019-04-10 | 2019-08-06 | 东南大学 | 基于即时定位建图与三维语义分割的语义建图系统 |
CN110243370A (zh) * | 2019-05-16 | 2019-09-17 | 西安理工大学 | 一种基于深度学习的室内环境三维语义地图构建方法 |
CN110827395A (zh) * | 2019-09-09 | 2020-02-21 | 广东工业大学 | 一种适用于动态环境的即时定位与地图构建方法 |
CN110738673A (zh) * | 2019-10-21 | 2020-01-31 | 哈尔滨理工大学 | 基于实例分割的视觉slam方法 |
CN111080659A (zh) * | 2019-12-19 | 2020-04-28 | 哈尔滨工业大学 | 一种基于视觉信息的环境语义感知方法 |
Non-Patent Citations (1)
Title |
---|
张国良 等.融合直接法与特征法的快速双目SLAM算法.《机器人》.2017,第39卷(第06期),879-888. * |
Also Published As
Publication number | Publication date |
---|---|
CN111563442A (zh) | 2020-08-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111563442B (zh) | 基于激光雷达的点云和相机图像数据融合的slam方法及系统 | |
CN111798475B (zh) | 一种基于点云深度学习的室内环境3d语义地图构建方法 | |
US11816907B2 (en) | Systems and methods for extracting information about objects from scene information | |
Xia et al. | Geometric primitives in LiDAR point clouds: A review | |
CN109544677B (zh) | 基于深度图像关键帧的室内场景主结构重建方法及系统 | |
CN111665842B (zh) | 一种基于语义信息融合的室内slam建图方法及系统 | |
Brilakis et al. | Toward automated generation of parametric BIMs based on hybrid video and laser scanning data | |
CN112785643A (zh) | 一种基于机器人平台的室内墙角二维语义地图构建方法 | |
Berrio et al. | Camera-LIDAR integration: Probabilistic sensor fusion for semantic mapping | |
Huang et al. | A fast point cloud ground segmentation approach based on coarse-to-fine Markov random field | |
Taneja et al. | Geometric change detection in urban environments using images | |
CN111862201A (zh) | 一种基于深度学习的空间非合作目标相对位姿估计方法 | |
CN109815847A (zh) | 一种基于语义约束的视觉slam方法 | |
Ji et al. | RGB-D SLAM using vanishing point and door plate information in corridor environment | |
Zhang et al. | Lidar-guided stereo matching with a spatial consistency constraint | |
CN112634368A (zh) | 场景目标的空间与或图模型生成方法、装置及电子设备 | |
CN115222884A (zh) | 一种基于人工智能的空间对象分析及建模优化方法 | |
Tao et al. | Indoor 3D semantic robot VSLAM based on mask regional convolutional neural network | |
CN113487631A (zh) | 基于lego-loam的可调式大角度探测感知及控制方法 | |
Zhu et al. | A review of 6d object pose estimation | |
CN116643291A (zh) | 一种视觉与激光雷达联合剔除动态目标的slam方法 | |
CN117949942B (zh) | 基于雷达数据和视频数据融合的目标跟踪方法及系统 | |
CN115457130A (zh) | 一种基于深度关键点回归的电动汽车充电口检测定位方法 | |
CN113689459B (zh) | 动态环境下基于gmm结合yolo实时跟踪与建图方法 | |
CN113658195A (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 |