CN110533716A - 一种基于3d约束的语义slam系统及方法 - Google Patents

一种基于3d约束的语义slam系统及方法 Download PDF

Info

Publication number
CN110533716A
CN110533716A CN201910768405.2A CN201910768405A CN110533716A CN 110533716 A CN110533716 A CN 110533716A CN 201910768405 A CN201910768405 A CN 201910768405A CN 110533716 A CN110533716 A CN 110533716A
Authority
CN
China
Prior art keywords
depth
matching
module
classified
point
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.)
Granted
Application number
CN201910768405.2A
Other languages
English (en)
Other versions
CN110533716B (zh
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.)
Xian University of Electronic Science and Technology
Original Assignee
Xian University of Electronic Science and Technology
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 Xian University of Electronic Science and Technology filed Critical Xian University of Electronic Science and Technology
Priority to CN201910768405.2A priority Critical patent/CN110533716B/zh
Publication of CN110533716A publication Critical patent/CN110533716A/zh
Application granted granted Critical
Publication of CN110533716B publication Critical patent/CN110533716B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/0002—Inspection of images, e.g. flaw detection
    • 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
    • G06T7/593—Depth or shape recovery from multiple images from stereo images
    • G06T7/596—Depth or shape recovery from multiple images from stereo images from three or more stereo images
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • 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/10016—Video; Image sequence
    • G06T2207/10021—Stereoscopic video; Stereoscopic image sequence
    • 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
    • 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)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Quality & Reliability (AREA)
  • Image Analysis (AREA)

Abstract

本发明提出了一种基于3D约束的语义SLAM系统与方法，旨在解决在像素深度值不稳定情况下相机位姿计算不准确和当动态目标占据相机视野大部分空间时无法计算相机位姿的问题，通过深度稳定性检测方法提高了相机位姿估计的准确性，采用3D约束方法提高了相机轨迹的完整性。实现方法为：数据采集模块获取图像序列；神经网络模块获取目标检测图像和实例分割图像；3D约束模块获取不同类别的匹配对集合；目标分类模块获取静态目标实例分割图像和动态目标实例分割图像；视觉前端模块获取深度相机位姿和路标点集合；后端优化模块获取全局最优的深度相机位姿和路标点；语义地图模块获取语义点云地图。

Description

一种基于3D约束的语义SLAM系统及方法
技术领域
本发明属于计算机视觉技术领域，更进一步涉及一种基于3D约束的语义SLAM系统与方法，可用于复杂高动态环境中相机的位姿估计与语义地图的构建。
背景技术
同时定位与地图构建系统SLAM，在无人系统的自主导航避障上扮演着重要的角色，过去的三十年中，SLAM系统发展迅速，其主要目标是无人系统在对未知环境进行自主探索的过程中进行自身精确定位的同时能够构建环境地图。但是传统的SLAM系统建出的地图只包含环境中的点、线、面等低等级几何特征，而对于未来的无人系统，只包含简单空间信息的地图难以满足其发展需求。语义地图的独到之处在于包含了环境中物体的语义信息，三维空间的语义地图能够使无人系统正确感知周围情况，通过对环境的认知理解，能让SLAM系统提升一定的定位精度，弥补现有无人系统在环境感知，及理解的不足。语义SLAM系统在构建地图的过程中不仅获得环境中物体的几何结构信息，识别环境中物体，同时可以获取其位置、姿态和功能属性等语义信息，从而能有效的应对复杂场景及完成更加复杂的任务。
2018年10月，西班牙萨拉戈萨大学的Berta Bescos等人在IEEE Robotics andAutomation Letters第3卷第4期发表名称为“DynaSLAM:Tracking,Mapping,andInpainting in Dynamic Scenes”的文章，提出了一种基于实例分割的SLAM系统及方法，在ORB-SLAM2的基础上，增加了动态目标检测功能，将RGB-D图像数据输入到Mask R-CNN网络中对所有具备先验动态性质的目标进行逐像素的分割，得到动态目标实例，并采用多视图几何方法检测不包含于CNN网络输出类别中的真实移动物体，通过不属于这些动态目标实例和真实移动物体的特征点匹配对计算相机位姿，解决了ORB-SLAM2在环境中有动态目标情况下相机位姿估计不准确的问题。同时，在实例分割中将所有具备先验动态性质的目标实例分割出去，得到仅含有静态场景的图像，并使用静态场景图像构建点云地图。
然而，DynaSLAM将所有具有先验动态性质的目标去除，当这些目标在环境中是静态时，建立的静态场景地图将缺失这些物体的信息，从而使得地图构建不够准确，而且由于实例分割的精确度不够高，导致动态目标实例的边缘信息映射到地图中，从而在地图中出现动态目标边缘及边缘重影。另一方面，在深度值不稳定的情况下，利用深度值缺失或者突变的特征匹配对计算相机位姿将导致位姿估计误差较大，而且当动态目标占据相机视野中的大部分空间时，因为环境中的匹配点不足，会导致DynaSLAM无法计算相机位姿，从而出现丢帧现象，相机的轨迹将会不完整。
发明内容
本发明的目的在于克服上述已有技术的不足，提出了一种基于3D约束的语义SLAM系统及方法，用于解决在像素深度值不稳定情况下深度相机位姿计算不准确和当动态目标占据相机视野大部分空间时无法计算相机位姿的问题，以提高相机位姿的准确性和相机轨迹的完整性，同时解决动态目标在静止时无法在点云地图中构建和地图中出现动态目标边缘信息与边缘重影的问题，从而获取更准确的点云地图。
为实现上述目的，本发明采取的技术方案为：
一种基于3D约束的语义SLAM系统，包括数据采集模块、神经网络模块、3D约束模块、目标分类模块、视觉前端模块、后端优化模块和语义地图模块，其中：
数据采集模块，采用深度相机，用于采集室内环境的多帧彩色图像和深度图像，以获取彩色图像序列和深度图像序列；
神经网络模块，用于通过训练好的BlitzNet网络模型，对彩色图像序列逐帧进行前向传播处理，以获取带有潜在动态目标框的目标检测图像和带有潜在动态目标实例的实例分割图像；
3D约束模块，用于对相邻两帧彩色图像进行特征匹配，并对获取的匹配对进行3D约束，将3D约束得到的多个匹配对进行归类，以获取各类别匹配对集合；
目标分类模块，用于通过目标检测图像和实例分割图像的内容信息与各类别匹配对集合的联系对潜在动态目标实例进行分类，以获取静态目标实例分割图像和动态目标实例分割图像；
视觉前端模块，用于通过各类别匹配对集合中的稳定匹配对集合计算深度相机位姿，并利用深度相机位姿和深度相机内参计算三维空间中的路标点坐标，以获取深度相机位姿和路标点集合；
后端优化模块，用于通过深度相机位姿和路标点集合构建代价函数，并对代价函数进行非线性优化，以获取全局最优的深度相机位姿和路标点集合；
语义地图模块，用于根据全局最优的深度相机位姿建立点云地图，并将静态目标实例分割图像中静态目标实例像素点的语义信息映射到点云地图上，以获取语义点云地图。
一种基于3D约束的语义SLAM的实现方法，包括如下步骤：
(1)数据采集模块获取图像序列：
数据采集模块采用深度相机，对室内环境进行持续拍摄，得到N帧彩色图像和N帧深度图像，并按照拍摄时间由前到后顺序分别对N帧彩色图像和N帧深度图像进行排序，得到彩色图像序列C1,C2,...,Ci,...,CN和深度图像序列D1,D2,...,Di,...,DN，i＝1,2,...,N，N≥100；
(2)神经网络模块获取目标检测图像和实例分割图像：
神经网络模块采用通过COCO数据集训练的BlitzNet网络模型，对C1,C2,...,Ci,...,CN中的N帧彩色图像逐帧进行前向传播处理，对具有运动性质的物体进行检测和分割，得到带有潜在动态目标框的检测图像CD1,CD2,...,CDi,...,CDN，以及带有潜在动态目标实例的实例分割图像CS1,CS2,...,CSi,...,CSN；
(3)3D约束模块获取不同类别的匹配对集合SEMP1、SSMP1、SMP1和DMP1：
(3a)3D约束模块对C1和C2分别提取ORB特征，得到C1的特征点集合FP1和C2的特征点集合FP1′，并采用深度稳定性检测方法对FP1和FP1′进行检测，得到深度稳定特征点集合DSP1和DSP1′；
(3b)3D约束模块对DSP1和DSP1′进行特征匹配，将得到的多个匹配对归为匹配对集合DSMP1，DSMP1中每一个匹配对dsmp1包含属于C1的特征点dsp1和属于C2的特征点dsp1′，并将像素坐标位于CD2潜在动态目标框内的dsp1′对应的dsmp1归为潜在动态匹配对集合PMP1，将其余的dsmp1归为环境匹配对集合EMP1；
(3c)3D约束模块采用深度约束方法计算EMP1中每一个匹配对emp1的深度变化，将深度变化稳定的emp1归为深度稳定环境匹配对集合DSEMP1，并通过DSEMP1深度变化的上界和下界对PMP1进行深度约束，得到深度稳定静态匹配对集合DSSMP1和深度动态匹配对集合DDMP1；
(3d)3D约束模块采用距离约束方法计算DSEMP1中每一个匹配对dsemp1的距离变化，将距离变化稳定的dsemp1归为距离稳定环境匹配对集合LSEMP1，并通过LSEMP1距离变化的上界和下界对DSSMP1进行距离约束，得到距离稳定静态匹配对集合LSSMP1和距离动态匹配对集合LDMP1；
(3e)3D约束模块采用方向约束方法计算LSEMP1中每一个匹配对lsemp1的方向向量，将方向一致的lsemp1归为稳定环境匹配对集合SEMP1，并通过SEMP1方向向量的方向角对LSSMP1进行方向约束，得到稳定静态匹配对集合SSMP1和方向动态匹配对集合MDMP1；
(3f)3D约束模块将SEMP1和SSMP1合并，得到稳定匹配对集合SMP1，并对DDMP1、LDMP1和MDMP1进行合并，得到动态匹配对集合DMP1；
(4)目标分类模块获取静态目标实例分割图像CSS2和动态目标实例分割图像CDS2：
(4a)目标分类模块对实例分割图像CS2分割出来的潜在动态目标实例进行分类，将标签为人和动物的潜在动态目标实例归为柔性目标实例，其余的潜在动态目标实例归为刚性目标实例；
(4b)目标分类模块计算动态匹配对比率τd和潜在动态匹配对比率τp，并将CS2中τd和τp均小于预设阈值的刚性目标实例归为静态目标实例，得到静态目标实例分割图像CSS2，其余的实例归为动态目标实例，同时对动态目标实例进行膨胀处理，得到动态目标实例分割图像CDS2；
(5)视觉前端模块获取深度相机位姿和路标点集合：
(5a)视觉前端模块将C1的相机坐标系设定为世界坐标系，并采用迭代最近点ICP方法通过SMP1中的匹配对smp1计算C2的深度相机位姿T2；
(5b)视觉前端模块通过深度相机内参和T2，将匹配对smp1中C2特征点sp1′的像素坐标转化为世界坐标，得到三维空间中的路标点集合L2；
(5c)视觉前端模块按照获取T2和L2的方法获取C3,C4,...,Ci,...,CN的深度相机位姿T3,T4,...,Ti,...,TN和路标点集合L3,L4,...,Li,...,LN；
(6)后端优化模块获取全局最优的深度相机位姿和路标点集合：
后端优化模块将L2,L3,...,Li,...,LN合并为路标点集合L，L中含有路标点l1,l2,...,lj,...,lM，构建以深度相机位姿T2,T3,...,Ti,...,TN和路标点l1,l2,...,lj,...,lM为变量的代价函数Err，并利用列文伯格-马夸尔特法对Err进行非线性优化，得到全局最优深度相机位姿T2′,T3′,...,Ti′,...,TN′和路标点l1′,l2′,...,lj′,...,lM′；
(7)语义地图模块获取语义点云地图PL：
(7a)语义地图模块对彩色图像序列C2,C3,...,Ci,...,CN进行逐帧处理，采用深度稳定性检测方法将第i帧彩色图像Ci中深度稳定的像素点归为深度稳定像素点集合YPi，并利用CDSi中的动态目标实例信息将YPi中不属于动态目标实例的像素点归为静态场景像素点集合CPi；
(7b)语义地图模块通过深度相机内参和Ti计算CPi的世界坐标，利用点云库PCL生成CPi对应的三维空间点，并将所有三维空间点合并成点云PLi；
(7c)语义地图模块利用CSSi中的静态目标实例信息，对静态目标实例像素点对应的三维空间点进行语义标注，得到语义点云PLi′；
(7d)语义地图模块对语义点云PL2′,PL3′,...,PLi′,...,PLN′进行拼接，得到全局语义点云地图PL。
本发明与现有的技术相比，具有以下优点：
第一，本发明采用深度稳定性检测方法剔除深度值缺失和深度值不稳定的特征点，通过深度稳定特征点进行特征匹配，并利用深度稳定匹配对构建深度约束，与现有技术中通过环境中所有的特征匹配对计算相机位姿相比，不仅减少了计算的数据量，而且提高了相机位姿估计的准确性；
第二，本发明采用3D约束方法实现对特征匹配对深度变化的深度约束、距离变化的距离约束和方向向量的方向约束，得到稳定静态匹配对集合和动态匹配对集合，并通过稳定静态匹配对集合和稳定环境匹配对集合共同计算相机位姿，与现有技术中仅通过环境匹配对集合计算相机位姿相比，解决了动态目标占据相机视野中大部分空间时无法计算相机位姿的问题，从而绘制更完整的相机轨迹，同时提高了相机位姿估计的准确性；
第三，本发明计算具有运动性质目标的动态匹配对比率和潜在动态匹配对比率，将潜在动态目标实例分为静态目标实例和动态目标实例，并对动态目标实例进行膨胀处理，同时将静态目标实例映射到点云地图中，与现有技术中将潜在动态目标实例均归为动态目标实例并在构建地图时没有利用动态目标实例相比，得到内容更丰富的语义点云地图，解决了地图中出现动态目标边缘信息与边缘重影的问题。
附图说明
图1是本发明语义SLAM系统的结构示意图；
图2是本发明语义SLAM方法的实现流程图；
图3是本发明方法中3D约束模块的实现流程图。
具体实施方式
以下结合附图和具体实施例，对本发明作进一步的详细说明：
参照图1，本发明基于3D约束的语义SLAM系统，包括数据采集模块、神经网络模块、3D约束模块、目标分类模块、视觉前端模块、后端优化模块和语义地图模块，其中：
数据采集模块，采用深度相机，用于采集室内环境的多帧彩色图像和深度图像，以获取彩色图像序列和深度图像序列；
神经网络模块，用于通过训练好的BlitzNet网络模型，对彩色图像序列逐帧进行前向传播处理，以获取带有潜在动态目标框的目标检测图像和带有潜在动态目标实例的实例分割图像；
3D约束模块，用于对相邻两帧彩色图像进行特征匹配，并对获取的匹配对进行3D约束，将3D约束得到的多个匹配对进行归类，以获取各类别匹配对集合；
目标分类模块，用于通过目标检测图像和实例分割图像的内容信息与各类别匹配对集合的联系对潜在动态目标实例进行分类，以获取静态目标实例分割图像和动态目标实例分割图像；
视觉前端模块，用于通过各类别匹配对集合中的稳定匹配对集合计算深度相机位姿，并利用深度相机位姿和深度相机内参计算三维空间中的路标点坐标，以获取深度相机位姿和路标点集合；
后端优化模块，用于通过深度相机位姿和路标点集合构建代价函数，并对代价函数进行非线性优化，以获取全局最优的深度相机位姿和路标点集合；
语义地图模块，用于根据全局最优的深度相机位姿建立点云地图，并将静态目标实例分割图像中静态目标实例像素点的语义信息映射到点云地图上，以获取语义点云地图。
参照图2，本发明基于3D约束的语义SLAM方法，包括如下步骤：
步骤(1)数据采集模块获取图像序列：
数据采集模块采用深度相机，对室内环境进行持续拍摄，得到N帧彩色图像和N帧深度图像，并按照拍摄时间由前到后顺序分别对N帧彩色图像和N帧深度图像进行排序，得到彩色图像序列C1,C2,...,Ci,...,CN和深度图像序列D1,D2,...,Di,...,DN，i＝1,2,...,N，N≥100；
步骤(2)神经网络模块获取目标检测图像和实例分割图像：
神经网络模块采用通过COCO数据集训练的BlitzNet网络模型，对C1,C2,...,Ci,...,CN中的N帧彩色图像逐帧进行前向传播处理，对具有运动性质的物体进行检测和分割，得到带有潜在动态目标框的检测图像CD1,CD2,...,CDi,...,CDN，以及带有潜在动态目标实例的实例分割图像CS1,CS2,...,CSi,...,CSN；
步骤(3)3D约束模块获取不同类别的匹配对集合SEMP1、SSMP1、SMP1和DMP1：
步骤(3a)3D约束模块对C1和C2分别提取ORB特征，得到C1的特征点集合FP1和C2的特征点集合FP1′，并采用深度稳定性检测方法对FP1和FP1′进行检测，得到深度稳定特征点集合DSP1和DSP1′，深度值存在突变的区域主要集中在物体的边缘，同时深度相机获得的处于较远距离的物体表面的深度值也会不精确，求删除这些深度值不稳定的特征点既减少特征匹配数据量，在往后求解ICP问题时也会降低对求解精度的影响；
采用深度稳定性检测方法对FP1和FP1′进行检测的实现步骤为：
步骤(3a1)3D约束模块以FP1中每一个特征点fp1的像素坐标为中心构建大小为3×3的图像块，检测图像块中是否存在深度值为0的像素，将存在零深度值的图像块对应的特征点fp1删除，保留其余的fp1；
步骤(3a2)3D约束模块计算保留下来所有特征点fp1对应图像块内9个深度值的标准差，得到一组数量与特征点fp1数量相同的标准差值SD，SD＝{SD1,SD2,...,SDi,...,SDn}，计算SD的比例中位数绝对偏差SMADSD：
SMADSD＝c×median(|SDi-median(SD)|) (1)
其中，c为常数值1.4826，median(·)为一组数值的中位数；
步骤(3a3)3D约束模块将与median(SD)相差超过三倍SMADSD的标准差值SDi定义为异常标准差值，并删除异常标准差值对应的特征点fp1，将其余的fp1归为深度稳定特征点集合DSP1；
步骤(3a4)3D约束模块按照步骤(3a1)-(3a3)的方法对FP1′进行相同的处理，得到深度稳定特征点集合DSP1′。
参照图3，步骤(3b)3D约束模块对DSP1和DSP1′进行特征匹配，将得到的多个匹配对归为匹配对集合DSMP1，DSMP1中每一个匹配对dsmp1包含属于C1的特征点dsp1和属于C2的特征点dsp1′，并将像素坐标位于CD2潜在动态目标框内的dsp1′对应的dsmp1归为潜在动态匹配对集合PMP1，将其余的dsmp1归为环境匹配对集合EMP1；
参照图3，步骤(3c)3D约束模块采用深度约束方法计算EMP1中每一个匹配对emp1的深度变化，将深度变化稳定的emp1归为深度稳定环境匹配对集合DSEMP1，并通过DSEMP1深度变化的上界和下界对PMP1进行深度约束，得到深度稳定静态匹配对集合DSSMP1和深度动态匹配对集合DDMP1，相机运动的过程中，相邻两帧图像中环境匹配对的深度变化理论上等于相机沿光轴方向的移动距离，考虑到噪声干扰等因素的影响，环境匹配对的深度变化值会集中在一个区间内，剔除异常值即可用来对潜在动态匹配对进行深度约束；
深度稳定静态匹配对集合DSSMP1和深度动态匹配对集合DDMP1的获取方法为：
步骤(3c1)3D约束模块计算EMP1中每一个匹配对emp1的深度变化Δdepe：
Δdepe＝|depth(ep1)-depth(ep1')| (2)
其中，ep1为emp1中属于C1的特征点，ep1′为emp1中属于C2的特征点，depth(·)为特征点的深度值，得到一组数量与匹配对emp1数量相同的深度变化值Δdep，Δdep＝{Δdepe1,Δdepe2,...,Δdepei,...,Δdepen′}；
步骤(3c2)3D约束模块计算Δdep的比例中位数绝对偏差SMADΔdep，将与median(Δdep)相差超过三倍SMADΔdep的深度变化值Δdepe定义为异常深度变化值，剔除Δdep中的异常深度变化值得到Δdep′，并删除异常深度变化值对应的匹配对emp1，将其余的emp1归为深度稳定环境匹配对集合DSEMP1；
步骤(3c3)3D约束模块将Δdep'中的最大值作为深度变化上界Udep，Δdep′中的最小值作为深度变化下界Ddep，并计算PMP1中每一个匹配对pmp1的深度变化Δdepp，将Δdepp大于Ddep且小于Udep的pmp1归为深度稳定静态匹配对集合DSSMP1，其余的pmp1归为深度动态匹配对集合DDMP1。
参照图3，步骤(3d)3D约束模块采用距离约束方法计算DSEMP1中每一个匹配对dsemp1的距离变化，将距离变化稳定的dsemp1归为距离稳定环境匹配对集合LSEMP1，并通过LSEMP1距离变化的上界和下界对DSSMP1进行距离约束，得到距离稳定静态匹配对集合LSSMP1和距离动态匹配对集合LDMP1，相机运动过程中，两帧图像匹配对的距离变化会因为特征点在空间中距离相机的远近而有所不同，统计环境区域中的匹配对的距离变化，确定环境中匹配对距离的变化区间，同时剔除不符合统计特性的异常值；
距离稳定静态匹配对集合LSSMP1和距离动态匹配对集合LDMP1的获取方法为：
步骤(3d1)3D约束模块计算DSEMP1中每一个匹配对dsemp1的距离变化Δdise：
其中，dsep1为dsemp1中属于C1的特征点，dsep1'为dsemp1中属于C2的特征点，(u,v)为特征点的像素坐标，得到一组数量与匹配对dsemp1数量相同的距离变化值Δdis，Δdis＝{Δdise1,Δdise2,...,Δdisei,...,Δdisen″}；
步骤(3d2)3D约束模块计算Δdis的比例中位数绝对偏差SMADΔdis，将与Δdis的中位数相差超过三倍SMADΔdis的距离变化值Δdise定义为异常距离变化值，剔除Δdis中的异常距离变化值得到Δdis′，并删除异常距离变化值对应的匹配对dsemp1，将其余的dsemp1归为距离稳定环境匹配对集合LSEMP1；
步骤(3d3)3D约束模块将Δdis'中的最大值作为深度变化上界Udis，Δdis'中的最小值作为深度变化下界Ddis，并计算DSSMP1中每一个匹配对dssmp1的距离变化Δdisp，将Δdisp大于Ddis且小于Udis的dssmp1归为距离稳定静态匹配对集合LSSMP1，其余的dssmp1归为距离动态匹配对集合LDMP1。
参照图3，步骤(3e)3D约束模块采用方向约束方法计算LSEMP1中每一个匹配对lsemp1的方向向量，将方向一致的lsemp1归为稳定环境匹配对集合SEMP1，并通过SEMP1方向向量的方向角对LSSMP1进行方向约束，得到稳定静态匹配对集合SSMP1和方向动态匹配对集合MDMP1；
稳定静态匹配对集合SSMP1和方向动态匹配对集合MDMP1的获取方法为：
步骤(3e1)3D约束模块计算LSEMP1中每一个匹配对lsemp1的方向向量
其中，lsep1为lsemp1中属于C1的特征点，lsep1′为lsemp1中属于C2的特征点，(u,v)为特征点的像素坐标，得到一组数量与匹配对lsemp1数量相同的方向向量
步骤(3e2)3D约束模块计算中各方向向量的方向角θe：
得到一组数量与数量相同的方向角θ，θ＝{θe1,θe2,...,θei,...,θen″′}；
步骤(3e3)3D约束模块设置方向角区间，将设为区间I1，为区间I2，为区间I3，为区间I4，为区间I5，为区间I6，为区间I7，为区间I8，π为区间I9；
步骤(3e4)3D约束模块对方向向量进行分类，将θe∈I1的归为方向集合NN，θe∈I2的归为方向集合YN，θe∈I3的归为方向集合PN，θe∈I4的归为方向集合XP，θe∈I5的归为方向集合PP，θe∈I6的归为方向集合YP，θe∈I7的归为方向集合NP，θe∈I8的归为方向集合XN，θe＝I9的归为方向集合ZERO，并将除ZERO之外的所有方向集合按照{NN,YN,PN,XP,PP,YP,NP,XN,NN}的循环顺序设定方向集合之间的顺序关系；
步骤(3e5)3D约束模块统计各方向集合中方向向量的数量N，若N最大的方向集合为ZERO，则将ZERO所对应的匹配对lsemp1设为方向一致匹配对semp1，同时设定I9为主方向区间I，并将semp1归为稳定环境匹配对集合SEMP1，否则，将N最大的方向集合设为主方向集合MD，MD的左相邻集合为MDL，MD的右相邻集合为MDR；
步骤(3e6)3D约束模块分别计算MDL中方向向量的数量与MD中方向向量的数量之比PropL和MDR中方向向量的数量与MD中方向向量的数量之比PropR，若PropL大于18.71％则将MDL和MD所对应的匹配对lsemp1设为方向一致匹配对semp1，若PropR大于18.71％则将MDR和MD所对应的lsemp1设为semp1，若PropL大于18.71％且PropR大于18.71％则将MDL、MDR和MD所对应的lsemp1设为semp1，否则，只将MD所对应的lsemp1设为semp1，然后设定semp1对应的方向角区间为主方向区间I，并将semp1归为稳定环境匹配对集合SEMP1；
步骤(3e7)3D约束模块计算LSSMP1中每一个匹配对lssmp1的方向向量并计算的方向角θe，将θe∈I的lssmp1归为稳定静态匹配对集合SSMP1，其余的lssmp1归为方向动态匹配对集合MDMP1。
参照图3，步骤(3f)3D约束模块将SEMP1和SSMP1合并，得到稳定匹配对集合SMP1，并对DDMP1、LDMP1和MDMP1进行合并，得到动态匹配对集合DMP1；
步骤(4)目标分类模块获取静态目标实例分割图像CSS2和动态目标实例分割图像CDS2：
步骤(4a)目标分类模块对实例分割图像CS2分割出来的潜在动态目标实例进行分类，将标签为人和动物的潜在动态目标实例归为柔性目标实例，其余的潜在动态目标实例归为刚性目标实例；
步骤(4b)目标分类模块计算动态匹配对比率τd和潜在动态匹配对比率τp，并将CS2中τd和τp均小于预设阈值的刚性目标实例归为静态目标实例，得到静态目标实例分割图像CSS2，其余的实例归为动态目标实例，同时对动态目标实例进行膨胀处理，得到动态目标实例分割图像CDS2；
目标分类模块计算动态匹配对比率τd和潜在动态匹配对比率τp的实现步骤为：
步骤(4b1)目标分类模块统计SEMP1的匹配对数量NSEMP、SSMP1中位于CS2各潜在动态目标实例的匹配对数量NSSMP1,NSSMP2,...,NSSMPi,...,NSSMPm和DMP1中位于CS2各潜在动态目标实例的匹配对数量NDMP1,NDMP2,...,NDMPi,...,NDMPm；
步骤(4b2)目标分类模块计算各潜在动态目标实例的动态匹配对比率τd和潜在动态匹配对比率τp：
设定τd的阈值为0.5，τp的阈值为0.15，将τd≤0.5且τp≤0.15的刚性目标实例归为静态目标实例，其余的实例归为动态目标实例。
步骤(5)视觉前端模块获取深度相机位姿和路标点集合：
步骤(5a)视觉前端模块将C1的相机坐标系设定为世界坐标系，并采用迭代最近点ICP方法通过SMP1中的匹配对smp1计算C2的深度相机位姿T2；
步骤(5b)视觉前端模块通过深度相机内参和T2，将匹配对smp1中C2特征点sp1′的像素坐标转化为世界坐标，得到三维空间中的路标点集合L2；
步骤(5c)视觉前端模块按照获取T2和L2的方法获取C3,C4,...,Ci,...,CN的深度相机位姿T3,T4,...,Ti,...,TN和路标点集合L3,L4,...,Li,...,LN；
步骤(6)后端优化模块获取全局最优的深度相机位姿和路标点集合：
后端优化模块将L2,L3,...,Li,...,LN合并为路标点集合L，L中含有路标点l1,l2,...,lj,...,lM，构建以深度相机位姿T2,T3,...,Ti,...,TN和路标点l1,l2,...,lj,...,lM为变量的代价函数Err，并利用列文伯格-马夸尔特法对Err进行非线性优化，得到全局最优深度相机位姿T2′,T3′,...,Ti′,...,TN′和路标点l1′,l2′,...,lj′,...,lM′；
构建代价函数Err的实现步骤为：
步骤(6a)后端优化模块根据相机位姿Ti中的旋转矩阵R和平移向量t，将L中的路标点lj三维坐标pj＝[X,Y,Z]T转化为相机坐标pj′：
pj'＝Rpj+t＝[X',Y',Z']T (8)
步骤(6b)后端优化模块通过相机坐标pj′计算归一化坐标[uc,vc,1]T：
步骤(6c)后端优化模块通过深度相机内参x轴缩放因子fx、y轴缩放因子fy、x轴平移因子cx、y轴平移因子cy和归一化坐标计算像素坐标Pj＝[us,vs]T：
步骤(6d)后端优化模块通过lj对应的Ci特征点spi-1像素坐标Pj′＝[us′,vs′]T计算误差erri：
步骤(6f)后端优化模块对err2,err3,...,erri,...,errN进行求和，得到代价函数Err：
步骤(7)语义地图模块获取语义点云地图PL：
步骤(7a)语义地图模块对彩色图像序列C2,C3,...,Ci,...,CN进行逐帧处理，采用深度稳定性检测方法将第i帧彩色图像Ci中深度稳定的像素点归为深度稳定像素点集合YPi，并利用CDSi中的动态目标实例信息将YPi中不属于动态目标实例的像素点归为静态场景像素点集合CPi；
步骤(7b)语义地图模块通过深度相机内参和Ti计算CPi的世界坐标，利用点云库PCL生成CPi对应的三维空间点，并将所有三维空间点合并成点云PLi；
步骤(7c)语义地图模块利用CSSi中的静态目标实例信息，对静态目标实例像素点对应的三维空间点进行语义标注，得到语义点云PLi′；
步骤(7d)语义地图模块对语义点云PL2′,PL3′,...,PLi′,...,PLN′进行拼接，得到全局语义点云地图PL。

Claims (8)

1.一种基于3D约束的语义SLAM系统，其特征在于，包括数据采集模块、神经网络模块、3D约束模块、目标分类模块、视觉前端模块、后端优化模块和语义地图模块，其中：
数据采集模块，采用深度相机，用于采集室内环境的多帧彩色图像和深度图像，以获取彩色图像序列和深度图像序列；
神经网络模块，用于通过训练好的BlitzNet网络模型，对彩色图像序列逐帧进行前向传播处理，以获取带有潜在动态目标框的目标检测图像和带有潜在动态目标实例的实例分割图像；
3D约束模块，用于对相邻两帧彩色图像进行特征匹配，并对获取的匹配对进行3D约束，将3D约束得到的多个匹配对进行归类，以获取各类别匹配对集合；
目标分类模块，用于通过目标检测图像和实例分割图像的内容信息与各类别匹配对集合的联系对潜在动态目标实例进行分类，以获取静态目标实例分割图像和动态目标实例分割图像；
视觉前端模块，用于通过各类别匹配对集合中的稳定匹配对集合计算深度相机位姿，并利用深度相机位姿和深度相机内参计算三维空间中的路标点坐标，以获取深度相机位姿和路标点集合；
后端优化模块，用于通过深度相机位姿和路标点集合构建代价函数，并对代价函数进行非线性优化，以获取全局最优的深度相机位姿和路标点集合；
语义地图模块，用于根据全局最优的深度相机位姿建立点云地图，并将静态目标实例分割图像中静态目标实例像素点的语义信息映射到点云地图上，以获取语义点云地图。
2.一种基于3D约束的语义SLAM的实现方法，其特征在于，包括如下步骤：
(1)数据采集模块获取图像序列：
数据采集模块采用深度相机，对室内环境进行持续拍摄，得到N帧彩色图像和N帧深度图像，并按照拍摄时间由前到后顺序分别对N帧彩色图像和N帧深度图像进行排序，得到彩色图像序列C1,C2,...,Ci,...,CN和深度图像序列D1,D2,...,Di,...,DN，i＝1,2,...,N，N≥100；
(2)神经网络模块获取目标检测图像和实例分割图像：
神经网络模块采用通过COCO数据集训练的BlitzNet网络模型，对C1,C2,...,Ci,...,CN中的N帧彩色图像逐帧进行前向传播处理，对具有运动性质的物体进行检测和分割，得到带有潜在动态目标框的检测图像CD1,CD2,...,CDi,...,CDN，以及带有潜在动态目标实例的实例分割图像CS1,CS2,...,CSi,...,CSN；
(3)3D约束模块获取不同类别的匹配对集合SEMP1、SSMP1、SMP1和DMP1：
(3a)3D约束模块对C1和C2分别提取ORB特征，得到C1的特征点集合FP1和C2的特征点集合FP1′，并采用深度稳定性检测方法对FP1和FP1′进行检测，得到深度稳定特征点集合DSP1和DSP1′；
(3b)3D约束模块对DSP1和DSP1′进行特征匹配，将得到的多个匹配对归为匹配对集合DSMP1，DSMP1中每一个匹配对dsmp1包含属于C1的特征点dsp1和属于C2的特征点dsp1′，并将像素坐标位于CD2潜在动态目标框内的dsp1′对应的dsmp1归为潜在动态匹配对集合PMP1，将其余的dsmp1归为环境匹配对集合EMP1；
(3c)3D约束模块采用深度约束方法计算EMP1中每一个匹配对emp1的深度变化，将深度变化稳定的emp1归为深度稳定环境匹配对集合DSEMP1，并通过DSEMP1深度变化的上界和下界对PMP1进行深度约束，得到深度稳定静态匹配对集合DSSMP1和深度动态匹配对集合DDMP1；
(3d)3D约束模块采用距离约束方法计算DSEMP1中每一个匹配对dsemp1的距离变化，将距离变化稳定的dsemp1归为距离稳定环境匹配对集合LSEMP1，并通过LSEMP1距离变化的上界和下界对DSSMP1进行距离约束，得到距离稳定静态匹配对集合LSSMP1和距离动态匹配对集合LDMP1；
(3e)3D约束模块采用方向约束方法计算LSEMP1中每一个匹配对lsemp1的方向向量，将方向一致的lsemp1归为稳定环境匹配对集合SEMP1，并通过SEMP1方向向量的方向角对LSSMP1进行方向约束，得到稳定静态匹配对集合SSMP1和方向动态匹配对集合MDMP1；
(3f)3D约束模块将SEMP1和SSMP1合并，得到稳定匹配对集合SMP1，并对DDMP1、LDMP1和MDMP1进行合并，得到动态匹配对集合DMP1；
(4)目标分类模块获取静态目标实例分割图像CSS2和动态目标实例分割图像CDS2：
(4a)目标分类模块对实例分割图像CS2分割出来的潜在动态目标实例进行分类，将标签为人和动物的潜在动态目标实例归为柔性目标实例，其余的潜在动态目标实例归为刚性目标实例；
(4b)目标分类模块计算动态匹配对比率τd和潜在动态匹配对比率τp，并将CS2中τd和τp均小于预设阈值的刚性目标实例归为静态目标实例，得到静态目标实例分割图像CSS2，其余的实例归为动态目标实例，同时对动态目标实例进行膨胀处理，得到动态目标实例分割图像CDS2；
(5)视觉前端模块获取深度相机位姿和路标点集合：
(5a)视觉前端模块将C1的相机坐标系设定为世界坐标系，并采用迭代最近点ICP方法通过SMP1中的匹配对smp1计算C2的深度相机位姿T2；
(5b)视觉前端模块通过深度相机内参和T2，将匹配对smp1中C2特征点sp1′的像素坐标转化为世界坐标，得到三维空间中的路标点集合L2；
(5c)视觉前端模块按照获取T2和L2的方法获取C3,C4,...,Ci,...,CN的深度相机位姿T3,T4,...,Ti,...,TN和路标点集合L3,L4,...,Li,...,LN；
(6)后端优化模块获取全局最优的深度相机位姿和路标点集合：
后端优化模块将L2,L3,...,Li,...,LN合并为路标点集合L，L中含有路标点l1,l2,...,lj,...,lM，构建以深度相机位姿T2,T3,...,Ti,...,TN和路标点l1,l2,...,lj,...,lM为变量的代价函数Err，并利用列文伯格-马夸尔特法对Err进行非线性优化，得到全局最优深度相机位姿T2′,T3′,...,Ti′,...,TN′和路标点l1′,l2′,...,lj′,...,lM′；
(7)语义地图模块获取语义点云地图PL：
(7a)语义地图模块对彩色图像序列C2,C3,...,Ci,...,CN进行逐帧处理，采用深度稳定性检测方法将第i帧彩色图像Ci中深度稳定的像素点归为深度稳定像素点集合YPi，并利用CDSi中的动态目标实例信息将YPi中不属于动态目标实例的像素点归为静态场景像素点集合CPi；
(7b)语义地图模块通过深度相机内参和Ti计算CPi的世界坐标，利用点云库PCL生成CPi对应的三维空间点，并将所有三维空间点合并成点云PLi；
(7c)语义地图模块利用CSSi中的静态目标实例信息，对静态目标实例像素点对应的三维空间点进行语义标注，得到语义点云PLi′；
(7d)语义地图模块对语义点云PL2′,PL3′,...,PLi′,...,PLN′进行拼接，得到全局语义点云地图PL。
3.根据权利要求2所述的基于3D约束的语义SLAM方法，其特征在于，步骤(3a)中所述的采用深度稳定性检测方法对FP1和FP1′进行检测，实现步骤为：
(3a1)3D约束模块以FP1中每一个特征点fp1的像素坐标为中心构建大小为3×3的图像块，检测图像块中是否存在深度值为0的像素，将存在零深度值的图像块对应的特征点fp1删除，保留其余的fp1；
(3a2)3D约束模块计算保留下来所有特征点fp1对应图像块内9个深度值的标准差，得到一组数量与特征点fp1数量相同的标准差值SD，SD＝{SD1,SD2,...,SDi,...,SDn}，计算SD的比例中位数绝对偏差SMADSD：
SMADSD＝c×median(|SDi-median(SD)|) (1)
其中，c为常数值1.4826，median(·)为一组数值的中位数；
(3a3)3D约束模块将与median(SD)相差超过三倍SMADSD的标准差值SDi定义为异常标准差值，并删除异常标准差值对应的特征点fp1，将其余的fp1归为深度稳定特征点集合DSP1；
(3a4)3D约束模块按照步骤(3a1)-(3a3)的方法对FP1′进行相同的处理，得到深度稳定特征点集合DSP1′。
4.根据权利要求2所述的基于3D约束的语义SLAM方法，其特征在于，步骤(3c)中所述的深度稳定静态匹配对集合DSSMP1和深度动态匹配对集合DDMP1，获取方法为：
(3c1)3D约束模块计算EMP1中每一个匹配对emp1的深度变化Δdepe：
Δdepe＝|depth(ep1)-depth(ep1') (2)
其中，ep1为emp1中属于C1的特征点，ep1′为emp1中属于C2的特征点，depth(·)为特征点的深度值，得到一组数量与匹配对emp1数量相同的深度变化值Δdep，Δdep＝{Δdepe1,Δdepe2,...,Δdepei,...,Δdepen′}；
(3c2)3D约束模块计算Δdep的比例中位数绝对偏差SMADΔdep，将与median(Δdep)相差超过三倍SMADΔdep的深度变化值Δdepe定义为异常深度变化值，剔除Δdep中的异常深度变化值得到Δdep′，并删除异常深度变化值对应的匹配对emp1，将其余的emp1归为深度稳定环境匹配对集合DSEMP1；
(3c3)3D约束模块将Δdep'中的最大值作为深度变化上界Udep，Δdep′中的最小值作为深度变化下界Ddep，并计算PMP1中每一个匹配对pmp1的深度变化Δdepp，将Δdepp大于Ddep且小于Udep的pmp1归为深度稳定静态匹配对集合DSSMP1，其余的pmp1归为深度动态匹配对集合DDMP1。
5.根据权利要求2所述的基于3D约束的语义SLAM方法，其特征在于，步骤(3d)中所述的距离稳定静态匹配对集合LSSMP1和距离动态匹配对集合LDMP1，获取方法为：
(3d1)3D约束模块计算DSEMP1中每一个匹配对dsemp1的距离变化Δdise：
其中，dsep1为dsemp1中属于C1的特征点，dsep1'为dsemp1中属于C2的特征点，(u,v)为特征点的像素坐标，得到一组数量与匹配对dsemp1数量相同的距离变化值Δdis，Δdis＝{Δdise1,Δdise2,...,Δdisei,...,Δdisen″}；
(3d2)3D约束模块计算Δdis的比例中位数绝对偏差SMADΔdis，将与Δdis的中位数相差超过三倍SMADΔdis的距离变化值Δdise定义为异常距离变化值，剔除Δdis中的异常距离变化值得到Δdis′，并删除异常距离变化值对应的匹配对dsemp1，将其余的dsemp1归为距离稳定环境匹配对集合LSEMP1；
(3d3)3D约束模块将Δdis'中的最大值作为深度变化上界Udis，Δdis'中的最小值作为深度变化下界Ddis，并计算DSSMP1中每一个匹配对dssmp1的距离变化Δdisp，将Δdisp大于Ddis且小于Udis的dssmp1归为距离稳定静态匹配对集合LSSMP1，其余的dssmp1归为距离动态匹配对集合LDMP1。
6.根据权利要求2所述的基于3D约束的语义SLAM方法，其特征在于，步骤(3e)中所述的稳定静态匹配对集合SSMP1和方向动态匹配对集合MDMP1，获取方法为：
(3e1)3D约束模块计算LSEMP1中每一个匹配对lsemp1的方向向量
其中，lsep1为lsemp1中属于C1的特征点，lsep1′为lsemp1中属于C2的特征点，(u,v)为特征点的像素坐标，得到一组数量与匹配对lsemp1数量相同的方向向量
(3e2)3D约束模块计算中各方向向量的方向角θe：
得到一组数量与数量相同的方向角θ，θ＝{θe1,θe2,...,θei,...,θen″′}；
(3e3)3D约束模块设置方向角区间，将设为区间I1，为区间I2，为区间I3，为区间I4，为区间I5，为区间I6，为区间I7，为区间I8，π为区间I9；
(3e4)3D约束模块对方向向量进行分类，将θe∈I1的归为方向集合NN，θe∈I2的归为方向集合YN，θe∈I3的归为方向集合PN，θe∈I4的归为方向集合XP，θe∈I5的归为方向集合PP，θe∈I6的归为方向集合YP，θe∈I7的归为方向集合NP，θe∈I8的归为方向集合XN，θe＝I9的归为方向集合ZERO，并将除ZERO之外的所有方向集合按照{NN,YN,PN,XP,PP,YP,NP,XN,NN}的循环顺序设定方向集合之间的顺序关系；
(3e5)3D约束模块统计各方向集合中方向向量的数量N，若N最大的方向集合为ZERO，则将ZERO所对应的匹配对lsemp1设为方向一致匹配对semp1，同时设定I9为主方向区间I，并将semp1归为稳定环境匹配对集合SEMP1，否则，将N最大的方向集合设为主方向集合MD，MD的左相邻集合为MDL，MD的右相邻集合为MDR；
(3e6)3D约束模块分别计算MDL中方向向量的数量与MD中方向向量的数量之比PropL和MDR中方向向量的数量与MD中方向向量的数量之比PropR，若PropL大于18.71％则将MDL和MD所对应的匹配对lsemp1设为方向一致匹配对semp1，若PropR大于18.71％则将MDR和MD所对应的lsemp1设为semp1，若PropL大于18.71％且PropR大于18.71％则将MDL、MDR和MD所对应的lsemp1设为semp1，否则，只将MD所对应的lsemp1设为semp1，然后设定semp1对应的方向角区间为主方向区间I，并将semp1归为稳定环境匹配对集合SEMP1；
(3e7)3D约束模块计算LSSMP1中每一个匹配对lssmp1的方向向量并计算的方向角θe，将θe∈I的lssmp1归为稳定静态匹配对集合SSMP1，其余的lssmp1归为方向动态匹配对集合MDMP1。
7.根据权利要求2所述的基于3D约束的语义SLAM方法，其特征在于，步骤(4b)中所述的目标分类模块计算动态匹配对比率τd和潜在动态匹配对比率τp，实现步骤为：
(4b1)目标分类模块统计SEMP1的匹配对数量NSEMP、SSMP1中位于CS2各潜在动态目标实例的匹配对数量NSSMP1,NSSMP2,...,NSSMPi,...,NSSMPm和DMP1中位于CS2各潜在动态目标实例的匹配对数量NDMP1,NDMP2,...,NDMPi,...,NDMPm；
(4b2)目标分类模块计算各潜在动态目标实例的动态匹配对比率τd和潜在动态匹配对比率τp：
。
8.根据权利要求2所述的基于3D约束的语义SLAM方法，其特征在于，步骤(6)中所述的构建代价函数Err，实现步骤为：
(6a)后端优化模块根据相机位姿Ti中的旋转矩阵R和平移向量t，将L中的路标点lj三维坐标pj＝[X,Y,Z]T转化为相机坐标pj′：
pj'＝Rpj+t＝[X',Y',Z']T (8)
(6b)后端优化模块通过相机坐标pj′计算归一化坐标[uc,vc,1]T：
(6c)后端优化模块通过深度相机内参x轴缩放因子fx、y轴缩放因子fy、x轴平移因子cx、y轴平移因子cy和归一化坐标计算像素坐标Pj＝[us,vs]T：
(6d)后端优化模块通过lj对应的Ci特征点spi-1像素坐标Pj′＝[us′,vs′]T计算误差erri：
(6f)后端优化模块对err2,err3,...,erri,...,errN进行求和，得到代价函数Err：
。
CN201910768405.2A 2019-08-20 2019-08-20 一种基于3d约束的语义slam系统及方法 Active CN110533716B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910768405.2A CN110533716B (zh) 2019-08-20 2019-08-20 一种基于3d约束的语义slam系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910768405.2A CN110533716B (zh) 2019-08-20 2019-08-20 一种基于3d约束的语义slam系统及方法

Publications (2)

Publication Number Publication Date
CN110533716A true CN110533716A (zh) 2019-12-03
CN110533716B CN110533716B (zh) 2022-12-02

Family

ID=68663726

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910768405.2A Active CN110533716B (zh) 2019-08-20 2019-08-20 一种基于3d约束的语义slam系统及方法

Country Status (1)

Country Link
CN (1) CN110533716B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111190981A (zh) * 2019-12-25 2020-05-22 中国科学院上海微系统与信息技术研究所 一种三维语义地图的构建方法、装置、电子设备及存储介质
CN111402336A (zh) * 2020-03-23 2020-07-10 中国科学院自动化研究所 基于语义slam的动态环境相机位姿估计及语义地图构建方法
CN111862216A (zh) * 2020-07-29 2020-10-30 上海高仙自动化科技发展有限公司 计算机设备定位方法、装置、计算机设备和存储介质
CN112435278A (zh) * 2021-01-26 2021-03-02 华东交通大学 一种基于动态目标检测的视觉slam方法及装置
CN112947415A (zh) * 2021-01-26 2021-06-11 同济大学 一种基于障碍物语义信息的室内路径规划方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107909612A (zh) * 2017-12-01 2018-04-13 驭势科技（北京）有限公司 一种基于3d点云的视觉即时定位与建图的方法与系统
CN109559320A (zh) * 2018-09-18 2019-04-02 华东理工大学 基于空洞卷积深度神经网络实现视觉slam语义建图功能的方法及系统
US20190114777A1 (en) * 2017-10-18 2019-04-18 Tata Consultancy Services Limited Systems and methods for edge points based monocular visual slam
CN110097553A (zh) * 2019-04-10 2019-08-06 东南大学 基于即时定位建图与三维语义分割的语义建图系统

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190114777A1 (en) * 2017-10-18 2019-04-18 Tata Consultancy Services Limited Systems and methods for edge points based monocular visual slam
CN107909612A (zh) * 2017-12-01 2018-04-13 驭势科技（北京）有限公司 一种基于3d点云的视觉即时定位与建图的方法与系统
CN109559320A (zh) * 2018-09-18 2019-04-02 华东理工大学 基于空洞卷积深度神经网络实现视觉slam语义建图功能的方法及系统
CN110097553A (zh) * 2019-04-10 2019-08-06 东南大学 基于即时定位建图与三维语义分割的语义建图系统

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111190981A (zh) * 2019-12-25 2020-05-22 中国科学院上海微系统与信息技术研究所 一种三维语义地图的构建方法、装置、电子设备及存储介质
CN111190981B (zh) * 2019-12-25 2020-11-06 中国科学院上海微系统与信息技术研究所 一种三维语义地图的构建方法、装置、电子设备及存储介质
CN111402336A (zh) * 2020-03-23 2020-07-10 中国科学院自动化研究所 基于语义slam的动态环境相机位姿估计及语义地图构建方法
CN111402336B (zh) * 2020-03-23 2024-03-12 中国科学院自动化研究所 基于语义slam的动态环境相机位姿估计及语义地图构建方法
CN111862216A (zh) * 2020-07-29 2020-10-30 上海高仙自动化科技发展有限公司 计算机设备定位方法、装置、计算机设备和存储介质
CN111862216B (zh) * 2020-07-29 2023-05-26 上海高仙自动化科技发展有限公司 计算机设备定位方法、装置、计算机设备和存储介质
CN112435278A (zh) * 2021-01-26 2021-03-02 华东交通大学 一种基于动态目标检测的视觉slam方法及装置
CN112947415A (zh) * 2021-01-26 2021-06-11 同济大学 一种基于障碍物语义信息的室内路径规划方法

Also Published As

Publication number Publication date
CN110533716B (zh) 2022-12-02

Similar Documents

Publication Publication Date Title
CN111462135B (zh) 基于视觉slam与二维语义分割的语义建图方法
CN110533716A (zh) 一种基于3d约束的语义slam系统及方法
Deng et al. PoseRBPF: A Rao–Blackwellized particle filter for 6-D object pose tracking
CN110533720A (zh) 基于联合约束的语义slam系统及方法
US20210060787A1 (en) Education assisting robot and control method thereof
CN111563442A (zh) 基于激光雷达的点云和相机图像数据融合的slam方法及系统
CN110135485A (zh) 单目相机与毫米波雷达融合的物体识别与定位方法和系统
CN110827395A (zh) 一种适用于动态环境的即时定位与地图构建方法
CN110688905B (zh) 一种基于关键帧的三维物体检测与跟踪方法
Ding et al. Vehicle pose and shape estimation through multiple monocular vision
CN105719352B (zh) 人脸三维点云超分辨率融合方法及应用其的数据处理装置
CN112418288B (zh) 一种基于gms和运动检测的动态视觉slam方法
CN112101160B (zh) 一种面向自动驾驶场景的双目语义slam方法
Wen et al. Hybrid semi-dense 3D semantic-topological mapping from stereo visual-inertial odometry SLAM with loop closure detection
CN112446882A (zh) 一种动态场景下基于深度学习的鲁棒视觉slam方法
CN112396655B (zh) 一种基于点云数据的船舶目标6d位姿估计方法
Cui et al. Dense depth-map estimation based on fusion of event camera and sparse LiDAR
CN103824305A (zh) 一种改进的Meanshift目标跟踪方法
Dong et al. A novel texture-less object oriented visual SLAM system
CN113487631A (zh) 基于lego-loam的可调式大角度探测感知及控制方法
CN113985435A (zh) 一种融合多激光雷达的建图方法及系统
Guo et al. 3d detection and pose estimation of vehicle in cooperative vehicle infrastructure system
Yu et al. Accurate and robust visual localization system in large-scale appearance-changing environments
CN113689459B (zh) 动态环境下基于gmm结合yolo实时跟踪与建图方法
CN117036484A (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