CN113192133B - 基于语义平面的单目即时定位与致密语义地图构建方法 - Google Patents

基于语义平面的单目即时定位与致密语义地图构建方法 Download PDF

Info

Publication number
CN113192133B
CN113192133B CN202110394142.0A CN202110394142A CN113192133B CN 113192133 B CN113192133 B CN 113192133B CN 202110394142 A CN202110394142 A CN 202110394142A CN 113192133 B CN113192133 B CN 113192133B
Authority
CN
China
Prior art keywords
semantic
point
points
plane
class
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
Application number
CN202110394142.0A
Other languages
English (en)
Other versions
CN113192133A (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.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
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 Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN202110394142.0A priority Critical patent/CN113192133B/zh
Publication of CN113192133A publication Critical patent/CN113192133A/zh
Application granted granted Critical
Publication of CN113192133B publication Critical patent/CN113192133B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/22—Matching criteria, e.g. proximity measures
    • 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
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/60—Analysis of geometric attributes
    • G06T7/66—Analysis of geometric attributes of image moments or centre of gravity
    • 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/40—Extraction of image or video features
    • G06V10/50—Extraction of image or video features by performing operations within image blocks; by using histograms, e.g. histogram of oriented gradients [HoG]; by summing image-intensity values; Projection analysis
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Data Mining & Analysis (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Biology (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Multimedia (AREA)
  • Geometry (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Image Analysis (AREA)

Abstract

一种基于语义平面的单目视觉即时定位与致密语义地图构建方法，当新一帧图像出现时，首先检测新一帧图像中的特征点，若系统已初始化，根据关键帧队列进行相机当前位置追踪；判断新一帧图像是否为关键帧，若是则将其输入到语义分割网络中获取该帧像素级语义标签；将新关键帧加入关键帧队列，并进行联合优化，判断关键帧队列数量是否超出最大值，若已超出最大值，挑选出旧关键帧中与新关键帧共视点个数最少的一帧关键帧进行边缘化；边缘化帧中静态语义类像素点根据其同语义类最近的语义质心点对应的语义小平面进行3D位置估计，进而恢复出致密的语义地图。本发明提升其定位精度，减少大量的计算复杂度，同时也避免了追踪每一个像素点给系统带来的不稳定性。

Description

基于语义平面的单目即时定位与致密语义地图构建方法
技术领域
本发明涉及即时定位与地图构建(SLAM)技术领域，尤其涉及一种基于语义平面的单目视觉即时定位与致密语义地图构建方法。
背景技术
即时定位与地图构建(Simultaneous Localization and Mapping,SLAM)是机器人相关应用的基础技术之一。SLAM技术按照传感器种类可分为视觉即时定位与地图构建(visual Simultaneous Localization and Mapping,vSLAM)以及激光即时定位与地图构建(LiDAR Simultaneous Localization and Mapping)。由于相对成本低且纹理信息更丰富，vSLAM被广泛应用于自动驾驶、增强现实、服务机器人等领域。具体地，按照相机种类，vSLAM可细分为单目SLAM，RGBD-SLAM，双目SLAM。双目相机的优点是探测距离远，但是标定复杂、视差计算对计算资源消耗大，主要应用于室外场景；RGBD相机的优点是可通过结构光或者ToF直接得到物体深度信息，但是测量范围有限且易受日光干扰，主要应用于室外场景；单目相机的优点是结构简单、成本低且标定简单，但是缺乏尺度信息，室外场景与室内场景均有应用。
随着人工智能技术的不断发展以及相关应用需求的驱动，基于深度学习的vSLAM成为相关领域的研究热点。相关研究表明，神经网络中的高层次特征作为传统手工特征的补充，能够提升vSLAM定位的鲁棒性。同时将高层次特征融入所构建的地图，能够提升系统对环境的理解能力。部分研究将RGBD-SLAM与神经网络结合构建室内场景下致密语义地图，部分研究将双目-SLAM与神经网络结合构建室外场景下致密语义地图。相比于传统点云地图，致密环境语义地图不仅仅是包含几何位置信息的点的集合，还通过语义信息的加入使得机器人能够直接通过该地图理解所身处的环境。
传统单目vSLAM按照所构建环境地图中点云密度可分为稀疏类与致密类，稀疏类方法选择图像中具有代表性的点(例如角点、SIFT特征点)进行追踪以及重建，而致密类方法则尝试追踪所有像素点。不对图像中的像素点加以筛选而尝试追踪所有像素点虽然可以构建致密的环境地图，但是会带来巨大的计算量同时降低系统的鲁棒性以及定位精度。对于单目SLAM而言，现有的方法在定位精度以及构建致密语义地图之间难以达到平衡，无法在保证定位精度的前提下构建致密的环境语义地图。
发明内容
为了克服现有技术的不足，本发明提供一种基于语义平面的单目视觉即时定位与致密语义地图构建方法，将语义分割网络得到环境语义信息融入传统vSLAM系统中，利用人造环境下语义平面分布特性提升定位精度，同时通过对大量语义小平面的进行建模，进而从稀疏点云中恢复出环境的致密语义地图。
本发明解决其技术问题所采用的技术方案是：
一种基于语义平面的单目视觉即时定位与致密语义地图构建方法，包括以下步骤：
步骤1，当新一帧图像出现时，首先检测新一帧图像中的特征点，然后判断系统是否已初始化；若未初始化则进行初始化，建立最初两帧间的对应关系；若已初始化，则根据关键帧队列进行相机当前位置追踪；
步骤2，判断新一帧图像是否为关键帧；若不是关键帧，则根据新一帧图像对当前局部地图进行完善；若是关键帧，则将其输入到语义分割网络中获取该帧像素级语义标签；
步骤3，将新关键帧加入关键帧队列，并进行联合优化，包含光束平差以及语义大平面限制；光束平差利用特征点在不同关键帧中的对应关系，优化相机位姿和特征点位置等参数；语义大平面限制利用语义平面的分布特性滤除外点，便于在后续优化中进一步提升定位精度；
步骤4，判断关键帧队列数量是否超出最大值；若未超出最大值，则跳回步骤1；若已超出最大值，挑选出旧关键帧中与新关键帧共视点个数最少的一帧关键帧进行边缘化；
步骤5，针对边缘化帧进行致密语义地图构建；结合语义分割结果以及特征点在图像中的位置以及颜色信息，进行语义小平面拟合，并计算其语义质心；该边缘化帧中静态语义类像素点根据其同语义类最近的语义质心点对应的语义小平面进行3D位置估计，进而恢复出致密的语义地图；
构建致密语义地图的步骤如下：
步骤5-1，对每一类静态语义类特征点，根据距离公式dis＝disE+disI进行聚类，其中disE表示在图像坐标系中的欧式距离，disI表示灰度值差异；首先将点组合最近点对，之后将点对组合成点组，点组中的点不在同一条直线上且不重复；
步骤5-2，针对每个点组，估计该点组的平面模型，并计算该点组的语义质心，语义质心的位置为点组的几何中心，其灰度为点组中点的平均灰度；
步骤5-3，针对每一个静态语义类像素点，计算其到同一语义类语义质心的距离，选择最近语义质心对应的平面模型去估计该像素点的3D位置。
每当有新一帧图像出现时，重复步骤1到步骤5。
进一步地，在所述步骤3中，对于联合优化的步骤如下：
步骤3-1，光束平差，利用新关键帧中特征与原关键帧队列关键帧中特征的对应关系，优化关键帧队列中关键帧对应的相机位姿以及特征点位置，最小化系统误差；
步骤3-2，对于处于某一平面上的点，其深度表示为
Figure GDA0003102759480000031
其中[uv 1]是该特征点在图像坐标系中的其次坐标；K是相机内参矩阵；[a1 a2 a3]是所在平面的法向量；将新关键帧中的静态类特征点根据其语义标签分为水平分布语义类(例如道路、地面、人行道等)与垂直分布语义类(例如建筑、墙面等)；每一个水平分布语义类特征，其所在平面法向量简化为[0 a2 0]，其深度表示为
Figure GDA0003102759480000041
即处于同一平面上的水平分布类特征的深度差异可以由v体现；每一个垂直分布语义类特征，其所在平面法向量简化为[a1 0a3]，其深度表示为
Figure GDA0003102759480000042
即处于同一平面上的垂直分布类特征的深度差异可以由u体现；对于每一个水平分布语义类，根据其图像坐标v划分成若干个直方图；对于每一个垂直分布语义类，根据其图像坐标u划分成若干个直方图；假定每个直方图中的特征点大致符合一个大语义平面，根据特征点的深度利用置信区间(μ-θ1δ，μ+θ2δ)滤除与周围点不一致的外点，其中μ为该直方图中特征点深度的平均值，δ为深度的方差；
步骤3-3，针对每个直方图，将水平分布语义类以及垂直分布语义类的最大深度值作为动态语义类(人、汽车等)特征点深度阈值，深度大于该阈值的动态语义类特征点将被视为外点。
具体地，在所述步骤3-2中，人造环境下，特定语义类特征所在平面的分布遵循一定规则，例如城市道路环境下，道路与建筑所在平面多数情况下相互垂直，且通常情况下相机朝向与道路所在平面平行；室内环境下，地面与墙面所在平面多数情况下相互垂直，且通常情况下相机朝向与地面所在平面相互平行。
对于某一特征点p:
[X Y Z]T＝dK-1[u v 1]T
其中，[u v 1]是该特征点在图像坐标系中的其次坐标；[X Y Z]是该特征点在相机坐标系中的坐标；K和d分别是相机内参矩阵以及特征点深度。
该特征点所在平面表示为：
a1X+a2Y+a3Z+1＝0
其中[a1 a2 a3]是所在平面的法向量；
对于水平分布语义类特征(例如道路、地面、人行道等)，其所在平面法向量简化为[0 a2 0]，其深度表示为：
Figure GDA0003102759480000051
对于垂直分布语义类特征(例如建筑、墙面等)，其所在平面法向量简化为[a1 0a3]，其深度表示为：
Figure GDA0003102759480000052
即对于同一平面上的点，水平分布语义类特征的深度差异由该特征在图像坐标下的v体现，垂直分布语义类特征的深度差异由该特征在图像坐标下的u体现。现实情况不一定完全符合假定的条件，例如相机朝向没有完全与水平分布语义类特征所在平面平行，水平分布类语义特征所在平面也没有完全与垂直分布类语义特征所在平面相互垂直，但大多数情况下仍可以将其视为基本符合本专利所假定的情况。
在该环节，本专利并不直接估计平面模型，而是根据特征点在图像坐标系中的坐标，将特征点划分到若干个直方图，假定每个直方图中的特征点符合一个语义大平面，然后利用高斯滤波去除与周围点不一致的点；具体地，对于每一类水平分布语义类特征，将其按v划分成若干个直方图，然后利用置信区间(μ-θ1δ，μ+θ2δ)滤除外点，其中μ为该直方图中特征点深度的平均值，δ为深度的方差；对于每一类垂直分布语义类特征，将其按u划分成若干个直方图，然后利用置信区间(μ-θ1δ，μ+θ2δ)滤除外点，其中μ为该直方图中特征点深度的平均值，δ为深度的方差。
具体地，在所述步骤3-3中，人造环境下，水平分布语义平面与垂直分布类语义平面一起构成了相机可视范围的边界。例如城市道路环境下，路面下方以及建筑物背后的点是无法被看到的；室内环境下，地面下方以及墙面背后的点是无法被看到的。即出现在水平分布语义平面与垂直分布语义平面另一侧的点应被视为外点。具体地，针对每个直方图中的动态语义类特征(人、汽车等)，将水平分布语义类以及垂直分布语义类的最大深度值作为深度阈值，深度大于该阈值的动态语义类特征点将被视为外点。
进一步地，在所述步骤5-1中，对每一类静态语义类特征点，按照以下距离公式计算两个特征点间的距离，把距离最近的点组合成点对：
disp，q＝disE+disI
其中，disE表示p，q两点在图像坐标系中的欧式距离，disI表示p，q两点在灰度值上的差异，得到最近点对后，根据以下公式计算最近点对间的距离：
dispair＝dism-E+dism-I
其中，dism-E表示两个点对中间点的在图像坐标系中的欧式距离，dism-I表示两个点对灰度均值的差异，若与该点对最近的两个点对的距离均小于阈值disth，同时该点对以及最近的两个点对的所有点不在同一条直线上，则将该点对以及最近的两个点对组合形成一个点组并去除重复点。阈值disth的设置可以防止点组中的点跨越同一个语义类中的不同目标。
具体地，在所述步骤5-2中，每个点组至少有4个不同点，对于4个点的点组，利用最小二乘法估计该点组的平面模型；对于5个以及6个点的点组，利用RANSAC方法估计该点组的平面模型，语义质心由两个参数组成，分别是在图像坐标系中的位置以及灰度，每个点组的语义质心在图像坐标系中的位置为点组中点的平均位置，其灰度为点组中点的平均灰度。
具体地，在所述步骤5-3中，针对每一个静态类像素点s，根据以下公式分别计算其到同一语义类所有语义质心的距离：
diss，c＝disE+disI
其中disE为像素点s与语义质心c在图像坐标中的欧式距离，disI为像素点s与语义质心c在灰度值上的差异，选择距离最近的语义质心对应的平面模型按照以下公式估计该静态像素的深度，进而得到其在相机坐标系下的3D位置：
Figure GDA0003102759480000071
本发明的有益效果主要表现在：将人造环境下的先验信息以语义大平面限制的形式与传统vSLAM结合，提升其定位精度；同时，通过拟合大量的语义小平面并利用这些语义小平面的模型估计静态语义类像素点的3D位置，从稀疏点云中恢复出致密的环境语义地图。这种恢复致密环境地图的方式相比于立体匹配能够减少大量的计算复杂度，同时也避免了追踪每一个像素点给系统带来的不稳定性。
附图说明
图1为本发明的整体流程图。
具体实施方式
下面结合附图对本发明作进一步描述。
参照图1，一种基于语义平面的单目视觉即时定位与致密语义地图构建方法，包括以下步骤：
步骤1，当新一帧图像出现时，首先检测新一帧图像中的特征点，然后判断系统是否已初始化；若未初始化则进行初始化，建立最初两帧间的对应关系；若已初始化，则根据关键帧队列进行相机当前位置追踪；
步骤2，判断新一帧图像是否为关键帧；若不是关键帧，则根据新一帧图像对当前局部地图进行完善；若是关键帧，则将其输入到语义分割网络中获取该帧像素级语义标签；
步骤3，将新关键帧加入关键帧队列，并进行联合优化，包含光束平差以及语义大平面限制；光束平差利用特征点在不同关键帧中的对应关系，优化相机位姿和特征点位置等参数；语义大平面限制利用语义平面的分布特性滤除外点，便于在后续优化中进一步提升定位精度；
步骤4，判断关键帧队列数量是否超出最大值；若未超出最大值，则跳回步骤1；若已超出最大值，挑选出旧关键帧中与新关键帧共视点个数最少的一帧关键帧进行边缘化；
步骤5，针对边缘化帧进行致密语义地图构建；结合语义分割结果以及特征点在图像中的位置以及颜色信息，进行语义小平面拟合，并计算其语义质心；该边缘化帧中静态语义类像素点根据其同语义类最近的语义质心点对应的语义小平面进行3D位置估计，进而恢复出致密的语义地图。
每当有新一帧图像出现时，重复步骤1到步骤5，整体流程图如图1所示。
进一步地，在所述步骤3中，对于联合优化的步骤如下：
步骤3-1，光束平差，利用新关键帧中特征与原关键帧队列关键帧中特征的对应关系，优化关键帧队列中关键帧对应的相机位姿以及特征点位置，最小化系统误差；
步骤3-2，对于处于某一平面上的点，其深度表示为
Figure GDA0003102759480000081
其中[uv 1]是该特征点在图像坐标系中的其次坐标；K是相机内参矩阵；[a1 a2 a3]是所在平面的法向量；将新关键帧中的静态类特征点根据其语义标签分为水平分布语义类(例如道路、地面、人行道等)与垂直分布语义类(例如建筑、墙面等)；每一个水平分布语义类特征，其所在平面法向量简化为[0 a2 0]，其深度表示为
Figure GDA0003102759480000082
即处于同一平面上的水平分布类特征的深度差异由v体现；每一个垂直分布语义类特征，其所在平面法向量简化为[a1 0 a3]，其深度表示为
Figure GDA0003102759480000083
即处于同一平面上的垂直分布类特征的深度差异由u体现；对于每一个水平分布语义类，根据其图像坐标v划分成若干个直方图；对于每一个垂直分布语义类，根据其图像坐标u划分成若干个直方图；假定每个直方图中的特征点大致符合一个大语义平面，根据特征点的深度利用置信区间(μ-θ1δ，μ+θ2δ)滤除与周围点不一致的外点，其中μ为该直方图中特征点深度的平均值，δ为深度的方差；
步骤3-3，针对每个直方图，将水平分布语义类以及垂直分布语义类的最大深度值作为动态语义类(人、汽车等)特征点深度阈值，深度大于该阈值的动态语义类特征点将被视为外点。
具体地，在所述步骤3-2中，人造环境下，特定语义类特征所在平面的分布遵循一定规则，例如城市道路环境下，道路与建筑所在平面多数情况下相互垂直，且通常情况下相机朝向与道路所在平面平行；室内环境下，地面与墙面所在平面多数情况下相互垂直，且通常情况下相机朝向与地面所在平面相互平行。
对于某一特征点p:
[X Y Z]T＝dK-1[u v 1]T
其中，[u v 1]是该特征点在图像坐标系中的其次坐标；[X Y Z]是该特征点在相机坐标系中的坐标；K和d分别是相机内参矩阵以及特征点深度；
该特征点所在平面表示为：
a1X+a2Y+a3Z+1＝0
其中[a1 a2 a3]是所在平面的法向量；
对于水平分布语义类特征(例如道路、地面、人行道等)，其所在平面法向量简化为[0 a2 0]，其深度表示为：
Figure GDA0003102759480000091
对于垂直分布语义类特征(例如建筑、墙面等)，其所在平面法向量简化为[a1 0a3]，其深度表示为：
Figure GDA0003102759480000092
即对于同一平面上的点，水平分布语义类特征的深度差异可以由该特征在图像坐标下的v体现，垂直分布语义类特征的深度差异可以由该特征在图像坐标下的u体现。现实情况不一定完全符合假定的条件，例如相机朝向没有完全与水平分布语义类特征所在平面平行，水平分布类语义特征所在平面也没有完全与垂直分布类语义特征所在平面相互垂直，但大多数情况下仍可以将其视为基本符合本专利所假定的情况。
在该环节，本专利并不直接估计平面模型，而是根据特征点在图像坐标系中的坐标，将特征点划分到若干个直方图。假定每个直方图中的特征点符合一个语义大平面，然后利用高斯滤波去除与周围点不一致的点。具体地，对于每一类水平分布语义类特征，将其按v划分成若干个直方图，然后利用置信区间(μ-θ1δ，μ+θ2δ)滤除外点，其中μ为该直方图中特征点深度的平均值，δ为深度的方差；对于每一类垂直分布语义类特征，将其按u划分成若干个直方图，然后利用置信区间(μ-θ1δ，μ+θ2δ)滤除外点，其中μ为该直方图中特征点深度的平均值，δ为深度的方差。
具体地，在所述步骤3-3中，人造环境下，水平分布语义平面与垂直分布类语义平面一起构成了相机可视范围的边界。例如城市道路环境下，道路平面与建筑平面构建了相机可视范围的边界，路面下方以及建筑物背后的点是无法被看到的；室内环境下，地面下方以及墙面背后的点是无法被看到的。即出现在水平分布语义平面与垂直分布语义平面另一侧的点应被视为外点。具体地，针对每个直方图中的动态语义类特征(人、汽车等)，将水平分布语义类以及垂直分布语义类的最大深度值作为深度阈值，深度大于该阈值的动态语义类特征点将被视为外点。
进一步地，在所述步骤5中，构建致密语义地图的步骤如下：
步骤5-1，对每一类静态语义类特征点，根据距离公式dis＝disE+disI进行聚类，其中disE表示在图像坐标系中的欧式距离，disI表示灰度值差异；首先将点组合最近点对，之后将点对组合成点组，点组中的点不在同一条直线上且不重复；
步骤5-2，针对每个点组，估计该点组的平面模型，并计算该点组的语义质心，语义质心的位置为点组的几何中心，其灰度为点组中点的平均灰度；
步骤5-3，针对每一个静态语义类像素点，计算其到同一语义类语义质心的距离，选择最近语义质心对应的平面模型去估计该像素点的3D位置。
具体地，在所述步骤5-1中，对每一类静态语义类特征点，按照以下距离公式计算两个特征点间的距离，把距离最近的点组合成点对：
disp，q＝disE+disI
其中，disE表示p，q两点在图像坐标系中的欧式距离，disI表示p，q两点在灰度值上的差异。得到最近点对后，根据以下公式计算最近点对间的距离：
dispair＝dism-E+dism-I
其中，dism-E表示两个点对中间点的在图像坐标系中的欧式距离，dism-I表示两个点对灰度均值的差异。若与该点对最近的两个点对的距离均小于阈值disth，同时该点对以及最近的两个点对的所有点不在同一条直线上，则将该点对以及最近的两个点对组合形成一个点组并去除重复点。阈值disth的设置可以防止点组中的点跨越同一个语义类中的不同目标。
具体地，在所述步骤5-2中，每个点组至少有4个不同点，对于4个点的点组，利用最小二乘法估计该点组的平面模型；对于5个以及6个点的点组，利用RANSAC方法估计该点组的平面模型。语义质心由两个参数组成，分别是在图像坐标系中的位置以及灰度。每个点组的语义质心在图像坐标系中的位置为点组中点的平均位置，其灰度为点组中点的平均灰度。
具体地，在所述步骤5-3中，针对每一个静态类像素点s，根据以下公式分别计算其到同一语义类所有语义质心的距离：
diss，c＝disE+disI
其中disE为像素点s与语义质心c在图像坐标中的欧式距离，disI为像素点s与语义质心c在灰度值上的差异。选择距离最近的语义质心对应的平面模型按照以下公式估计该静态像素的深度，进而得到其在相机坐标系下的3D位置：
Figure GDA0003102759480000121
本专利所提出的基于语义平面的单目视觉即时定位与地图构建方法可以在不改变原传统vSLAM基本框架的前提下，结合语义分割网络的输出，通过语义大平面限制提升定位精度，同时通过拟合大量语义小平面从稀疏点云中恢复出致密的环境语义地图。本实施例在传统DSO方法上融入本专利所提出的方法，选择领域内具有代表性的KITTI数据集验证有效性。语义分割网络模型选择Deeplabv3+，该模型在Cityscapes数据集上进行预训练，然后在KITTI的语义分割数据集上进行微调。表1为本专利与DSO在KITTI数据集上定位平均均方根误差的比较。从表1中，我们可以看出，通过融入本专利提出的方法，定位精度有了较大的提升。
序列 DSO(m) 本专利(m) 提升(％)
0 104.57 82.10 21.49
1 66.55 44.23 33.55
2 70.46 57.96 17.74
3 1.19 0.88 25.77
4 0.65 0.46 29.51
5 56.64 42.50 24.96
6 55.51 43.84 21.01
7 15.70 11.43 27.20
8 97.41 81.69 16.14
9 38.88 34.27 11.87
10 12.79 10.92 14.61
表1。
以上实施方式仅用于说明本发明，而非对本发明的限制。尽管参照实施例对本发明进行了详细说明，本领域的技术人员应当理解，对本发明的技术方案进行各种组合、修改或者等同替换，都不脱离本发明技术方案的精神和范围，均应涵盖在本发明的权利要求范围当中。

Claims (8)

1.一种基于语义平面的单目即时定位与致密语义地图构建方法，其特征在于，所述方法包括以下步骤：
步骤1，当新一帧图像出现时，首先检测新一帧图像中的特征点，然后判断系统是否已初始化；若未初始化则进行初始化，建立最初两帧间的对应关系；若已初始化，则根据关键帧队列进行相机当前位置追踪；
步骤2，判断新一帧图像是否为关键帧；若不是关键帧，则根据新一帧图像对当前局部地图进行完善；若是关键帧，则将其输入到语义分割网络中获取该帧像素级语义标签；
步骤3，将新关键帧加入关键帧队列，并进行联合优化，包含光束平差以及语义大平面限制；光束平差利用特征点在不同关键帧中的对应关系，优化相机位姿和特征点位置参数；语义大平面限制利用语义平面的分布特性滤除外点，便于在后续优化中进一步提升定位精度；
步骤4，判断关键帧队列数量是否超出最大值；若未超出最大值，则跳回步骤1；若已超出最大值，挑选出旧关键帧中与新关键帧共视点个数最少的一帧关键帧进行边缘化；
步骤5，针对边缘化帧进行致密语义地图构建；结合语义分割结果以及特征点在图像中的位置以及颜色信息，进行语义小平面拟合，并计算其语义质心；该边缘化帧中静态语义类像素点根据其同语义类最近的语义质心对应的语义小平面进行3D位置估计，进而恢复出致密的语义地图；
构建致密语义地图的步骤如下：
步骤5-1，对每一类静态语义类特征点，根据距离公式dis＝disE+disI进行聚类，其中disE表示在图像坐标系中的欧式距离，disI表示灰度值差异；首先将点组合最近点对，之后将点对组合成点组，点组中的点不在同一条直线上且不重复；
步骤5-2，针对每个点组，估计该点组的平面模型，并计算该点组的语义质心，语义质心的位置为点组的几何中心，其灰度为点组中点的平均灰度；
步骤5-3，针对每一个静态语义类像素点，计算其到同一语义类语义质心的距离，选择最近语义质心对应的平面模型去估计该像素点的3D位置。
2.如权利要求1所述的基于语义平面的单目即时定位与致密语义地图构建方法，其特征在于，在所述步骤3中，联合优化的步骤如下：
步骤3-1，光束平差，利用新关键帧中特征与原关键帧队列关键帧中特征的对应关系，优化关键帧队列中关键帧对应的相机位姿以及特征点位置，最小化系统误差；
步骤3-2，对于处于某一平面上的点，其深度表示为
Figure FDA0003642704910000011
其中[u v 1]是该点在图像坐标系中的齐次坐标；K是相机内参矩阵；[a1 a2 a3]是所在平面的法向量；将新关键帧中的静态类特征点根据其语义标签分为水平分布语义类与垂直分布语义类；每一个水平分布语义类特征，其所在平面法向量简化为[0 a2 0]，其深度表示为
Figure FDA0003642704910000012
即处于同一平面上的水平分布类特征的深度差异由v体现；每一个垂直分布语义类特征，其所在平面法向量简化为[a1 0 a3]，其深度表示为
Figure FDA0003642704910000013
即处于同一平面上的垂直分布类特征的深度差异可以由u体现；对于每一个水平分布语义类，根据其图像坐标v划分成若干个直方图；对于每一个垂直分布语义类，根据其图像坐标u划分成若干个直方图；假定每个直方图中的特征点大致符合一个大语义平面，根据特征点的深度利用置信区间(μ-θ1δ，μ+θ2δ)滤除与周围点不一致的外点，其中μ为该直方图中特征点深度的平均值，δ为深度的方差；
步骤3-3，针对每个直方图，将水平分布语义类以及垂直分布语义类的最大深度值作为动态语义类特征点深度阈值，深度大于该阈值的动态语义类特征点将被视为外点。
3.如权利要求2所述的基于语义平面的单目即时定位与致密语义地图构建方法，其特征在于，在所述步骤3-2中，对于某一特征点p:
[X Y Z]T＝dK-1[u v 1]T
其中，[u v 1]是该特征点在图像坐标系中的其次坐标；[X Y Z]是该特征点在相机坐标系中的坐标；K和d分别是相机内参矩阵以及特征点深度；
该特征点所在平面表示为：
a1X+a2Y+a3Z+1＝0
其中[a1 a2 a3]是所在平面的法向量；
对于水平分布语义类特征，其所在平面法向量简化为[0 a2 0]，其深度表示为：
Figure FDA0003642704910000021
对于垂直分布语义类特征，其所在平面法向量简化为[a1 0 a3]，其深度表示为：
Figure FDA0003642704910000022
即对于同一平面上的点，水平分布语义类特征的深度差异由该特征在图像坐标下的v体现，垂直分布语义类特征的深度差异由该特征在图像坐标下的u体现；
根据特征点在图像坐标系中的坐标，将特征点划分到若干个直方图，假定每个直方图中的特征点符合一个语义大平面，然后利用高斯滤波去除与周围点不一致的点；对于每一类水平分布语义类特征，将其按v划分成若干个直方图，然后利用置信区间(μ-θ1δ，μ+θ2δ)滤除外点，其中μ为该直方图中特征点深度的平均值，δ为深度的方差；对于每一类垂直分布语义类特征，将其按u划分成若干个直方图，然后利用置信区间(μ-θ1δ，μ+θ2δ)滤除外点，其中μ为该直方图中特征点深度的平均值，δ为深度的方差。
4.如权利要求2所述的基于语义平面的单目即时定位与致密语义地图构建方法，其特征在于，在所述步骤3-3中，人造环境下，水平分布语义平面与垂直分布类语义平面一起构成了相机可视范围的边界，出现在水平分布语义平面与垂直分布语义平面另一侧的点应被视为外点；针对每个直方图中的动态语义类特征，将水平分布语义类以及垂直分布语义类的最大深度值作为深度阈值，深度大于该阈值的动态语义类特征点将被视为外点。
5.如权利要求1或2或3或4所述的一种基于语义平面的单目即时定位与致密语义地图构建方法，其特征在于，在所述步骤5-1中，对每一类静态语义类特征点，按照以下距离公式计算两个特征点间的距离，把距离最近的点组合成点对：
disp,q＝disE+disI
其中，disE表示p,q两点在图像坐标系中的欧式距离，disI表示p,q两点在灰度值上的差异，得到最近点对后，根据以下公式计算最近点对间的距离：
dispair＝dism-E+dism-I
其中，dism-E表示两个点对中间点的在图像坐标系中的欧式距离，dism-I表示两个点对灰度均值的差异，若与该点对最近的两个点对的距离均小于阈值disth，同时该点对以及最近的两个点对的所有点不在同一条直线上，则将该点对以及最近的两个点对组合形成一个点组并去除重复点，阈值disth的设置可以防止点组中的点跨越同一个语义类中的不同目标。
6.如权利要求1或2或3或4所述的一种基于语义平面的单目即时定位与致密语义地图构建方法，其特征在于，在所述步骤5-2中，每个点组至少有4个不同点，对于4个点的点组，利用最小二乘法估计该点组的平面模型；对于5个以及6个点的点组，利用RANSAC方法估计该点组的平面模型，语义质心由两个参数组成，分别是在图像坐标系中的位置以及灰度，每个点组的语义质心在图像坐标系中的位置为点组中点的平均位置，其灰度为点组中点的平均灰度。
7.如权利要求2或3或4所述的一种基于语义平面的单目即时定位与致密语义地图构建方法，其特征在于，在所述步骤5-3中，针对每一个静态类像素点s，根据以下公式分别计算其到同一语义类所有语义质心的距离：
diss,c＝disE+disI
其中disE为像素点s与语义质心c在图像坐标中的欧式距离，disI为像素点s与语义质心c在灰度值上的差异，选择距离最近的语义质心对应的平面模型按照以下公式估计该静态像素的深度，进而得到其在相机坐标系下的3D位置：
Figure FDA0003642704910000031
8.如权利要求1或2或3或4所述的一种基于语义平面的单目即时定位与致密语义地图构建方法，其特征在于，在所述步骤1中，特征点检测部分尽可能提取数量较多的特征点，且在纹理信息丰富处均匀分布，高密度且在纹理信息丰富处均匀分布的特征点有利于后续语义小平面的拟合以及致密语义地图的恢复。
CN202110394142.0A 2021-04-13 2021-04-13 基于语义平面的单目即时定位与致密语义地图构建方法 Active CN113192133B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110394142.0A CN113192133B (zh) 2021-04-13 2021-04-13 基于语义平面的单目即时定位与致密语义地图构建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110394142.0A CN113192133B (zh) 2021-04-13 2021-04-13 基于语义平面的单目即时定位与致密语义地图构建方法

Publications (2)

Publication Number Publication Date
CN113192133A CN113192133A (zh) 2021-07-30
CN113192133B true CN113192133B (zh) 2022-08-05

Family

ID=76975675

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110394142.0A Active CN113192133B (zh) 2021-04-13 2021-04-13 基于语义平面的单目即时定位与致密语义地图构建方法

Country Status (1)

Country Link
CN (1) CN113192133B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116399326B (zh) * 2023-04-06 2023-10-13 安徽工程大学 一种基于自适应关键帧选取的机器人地图构建方法、存储介质及设备

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019185170A1 (en) * 2018-03-30 2019-10-03 Toyota Motor Europe Electronic device, robotic system and method for localizing a robotic system
CN111325843A (zh) * 2020-03-09 2020-06-23 北京航空航天大学 一种基于语义逆深度滤波的实时语义地图构建方法
CN111325842A (zh) * 2020-03-04 2020-06-23 Oppo广东移动通信有限公司 地图构建方法、重定位方法及装置、存储介质和电子设备
CN111968129A (zh) * 2020-07-15 2020-11-20 上海交通大学 具有语义感知的即时定位与地图构建系统及方法
CN112132893A (zh) * 2020-08-31 2020-12-25 同济人工智能研究院(苏州)有限公司 一种适用于室内动态环境的视觉slam方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019185170A1 (en) * 2018-03-30 2019-10-03 Toyota Motor Europe Electronic device, robotic system and method for localizing a robotic system
CN111325842A (zh) * 2020-03-04 2020-06-23 Oppo广东移动通信有限公司 地图构建方法、重定位方法及装置、存储介质和电子设备
CN111325843A (zh) * 2020-03-09 2020-06-23 北京航空航天大学 一种基于语义逆深度滤波的实时语义地图构建方法
CN111968129A (zh) * 2020-07-15 2020-11-20 上海交通大学 具有语义感知的即时定位与地图构建系统及方法
CN112132893A (zh) * 2020-08-31 2020-12-25 同济人工智能研究院(苏州)有限公司 一种适用于室内动态环境的视觉slam方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Large-scale 3D Semantic Mapping Using Stereo Vision;Yi Yang 等;《 International Journal of Automation & Computing》;20181231;第15卷(第2期);第194-206页 *
动态环境下的语义地图构建;齐少华 等;《计算机科学》;20200831;第198-203页 *

Also Published As

Publication number Publication date
CN113192133A (zh) 2021-07-30

Similar Documents

Publication Publication Date Title
CN111563442B (zh) 基于激光雷达的点云和相机图像数据融合的slam方法及系统
CN108648161B (zh) 非对称核卷积神经网络的双目视觉障碍物检测系统及方法
CN114862949B (zh) 一种基于点线面特征的结构化场景视觉slam方法
CN112396595B (zh) 一种动态环境下基于点线特征的语义slam方法
CN107862735B (zh) 一种基于结构信息的rgbd三维场景重建方法
CN113506318B (zh) 一种车载边缘场景下的三维目标感知方法
CN113327296B (zh) 基于深度加权的激光雷达与相机在线联合标定方法
CN112381841A (zh) 一种动态场景下基于gms特征匹配的语义slam方法
CN112435262A (zh) 基于语义分割网络和多视图几何的动态环境信息检测方法
CN106651897B (zh) 一种基于超像素分割的视差修正方法
Qian et al. Rf-lio: Removal-first tightly-coupled lidar inertial odometry in high dynamic environments
CN110533716B (zh) 一种基于3d约束的语义slam系统及方法
CN107564095A (zh) 一种基于单幅自然图像重建积云三维形状的方法
CN112435267B (zh) 一种高分辨率城市卫星立体图像的视差图计算方法
CN112446882A (zh) 一种动态场景下基于深度学习的鲁棒视觉slam方法
CN106408596A (zh) 基于边缘的局部立体匹配方法
Zhou et al. Monet3d: Towards accurate monocular 3d object localization in real time
CN113192133B (zh) 基于语义平面的单目即时定位与致密语义地图构建方法
Ni et al. Second-order semi-global stereo matching algorithm based on slanted plane iterative optimization
CN113487631B (zh) 基于lego-loam的可调式大角度探测感知及控制方法
Feng et al. D2nt: A high-performing depth-to-normal translator
CN112258631B (zh) 一种基于深度神经网络的三维目标检测方法及系统
Neverova et al. 2 1/2 D scene reconstruction of indoor scenes from single RGB-D images
CN108921852A (zh) 基于视差与平面拟合的双分支室外非结构化地形分割网络
CN115830116A (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