CN114862949B - 一种基于点线面特征的结构化场景视觉slam方法 - Google Patents
一种基于点线面特征的结构化场景视觉slam方法 Download PDFInfo
- Publication number
- CN114862949B CN114862949B CN202210346890.6A CN202210346890A CN114862949B CN 114862949 B CN114862949 B CN 114862949B CN 202210346890 A CN202210346890 A CN 202210346890A CN 114862949 B CN114862949 B CN 114862949B
- Authority
- CN
- China
- Prior art keywords
- plane
- line
- feature
- features
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 50
- 230000000007 visual effect Effects 0.000 title claims abstract description 26
- 239000013598 vector Substances 0.000 claims abstract description 60
- 238000005457 optimization Methods 0.000 claims abstract description 32
- 238000006073 displacement reaction Methods 0.000 claims abstract description 17
- 238000001514 detection method Methods 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 32
- 238000012216 screening Methods 0.000 claims description 9
- 238000013507 mapping Methods 0.000 claims description 8
- 230000003287 optical effect Effects 0.000 claims description 7
- 230000011218 segmentation Effects 0.000 claims description 5
- 238000012545 processing Methods 0.000 claims description 4
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000007423 decrease Effects 0.000 abstract 1
- 238000005286 illumination Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000004321 preservation Methods 0.000 description 2
- 230000003190 augmentative effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007797 corrosion Effects 0.000 description 1
- 238000005260 corrosion Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
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
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/97—Determining parameters from multiple pictures
-
- 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/10024—Color image
-
- 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
-
- 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)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Image Analysis (AREA)
Abstract
本发明公开的一种基于点线面特征的结构化场景视觉SLAM方法,首先输入彩色图像及其对应的深度图像,在图像中提取点线面特征并进行特征匹配;然后根据平面法向量检测曼哈顿世界坐标系,若存在曼哈顿世界坐标系且在曼哈顿世界地图中出现则求出相机姿态并跟踪点线面特征估计位移,否则跟踪点线面特征估计位姿;然后对当前帧进行关键帧判断,若为关键帧则***到局部地图;接着维护地图信息并对当前关键帧、相邻关键帧和三维特征进行联合优化;最后进行回环检测,若检测到闭环帧则闭合回环并进行全局优化。本发明是一种高精度、鲁棒性强的视觉SLAM方法,解决了低纹理的结构化场景下仅基于点特征的视觉SLAM精度下降甚至***失效的问题。
Description
技术领域
本发明属于机器人同时定位与地图构建技术领域,具体设计一种基于点线面特征的结构化场景视觉SLAM方法。
背景技术
近年来智能车辆定位***在城市交通中的应用越来越广泛,传统定位方法如基于GNSS技术的室外定位***目前已经非常成熟。然而,在GNSS信号受到遮蔽的室内环境,高性能的实时定位仍然是一个难点。虽然基于无线信号的室内定位***得到了长足发展,如基于蓝牙、WiFi、UWB以及RFID等,但由于其设备部署成本高昂,而且在室内容易受到遮挡以及多路径效应的干扰,难以有效用于室内定位与建图。
针对上述问题,同时定位与建图方法(SLAM)被提出。SLAM是一种在未知环境中,进行自身定位并同时建立地图的技术。主要应用在移动机器人、自动驾驶、虚拟现实、增强现实等方面。针对室内环境已经有成熟的基于激光的SLAM***如cartographer、hector map、gmapping等***提出。但由于激光雷达造价仍然较为昂贵,因此人们普遍期望采用价格低廉的视觉传感器,比如单目相机,双目相机,深度相机等,***将图像作为输入,输出相机轨迹以及重建的三维地图,如DSO利用摄像机等实现高精度的室内定位。然而,现有的基于视觉的SLAM***大量依赖对图像中的低层次的点特征的提取,如目前先进的基于点特征的ORB-SLAM2(ORB-SLAM2:An Open Source SLAM System for Monocular Stereo and RGB-DCameras),该SLAM方法仅基于点特征点实现位姿估计,虽然在纹理丰富的环境下能够实现高精度定位,但在纹理特征缺乏、光照变化比较大的人造室内环境,点特征难以提取且不稳定,该方法位姿估计精度会变差甚至会跟踪失败。在该类人造室内环境中富含墙面、地面、天花板等规则的几何结构,以线、面为代表的结构特征相比于点特征,更易获取,且不易受光照变化的影响,因此如何在纹理特征缺乏的室内几何结构化场景利用视觉信息找到适用于SLAM定位的结构特征显得尤为重要。通过利用室内几何结构元素,本发明提出了一种基于点线面特征的结构化场景视觉SLAM方法,解决在纹理缺乏、光照变化的人造结构化室内场景中位姿估计精度低、鲁棒性差的问题。
发明内容
针对现有技术存在的上述问题,本发明的目的是提供一种基于点线面特征的结构化场景视觉SLAM方法,以解决目前的视觉SLAM技术在纹理缺乏、光照变化的人造结构化室内场景中位姿估计精度低、鲁棒性差的问题。
本发明至少通过如下技术方案之一实现。
一种基于点线面特征的结构化场景视觉SLAM方法,包括以下步骤:
S1、输入彩色图像,根据彩色图像提取点特征和线特征并进行特征匹配;
S2、输入深度图像,转换成点云序列结构,然后提取图像平面,接着对提取的图像平面与地图面进行匹配;
S3、检测图像中是否存在曼哈顿世界坐标系,若存在且在曼哈顿世界地图中的历史关键帧观测到该坐标系,则根据所提取到的曼哈顿世界坐标系估计相机姿态并跟踪点特征、线特征和平面特征去优化相机位移,否则跟踪点特征、线特征和平面特征去优化相机位姿;
S4、判断当前帧是否为关键帧,若为关键帧,则加入到局部地图中,并更新地图中的三维点、三维线和曼哈顿世界地图,对当前关键帧和相邻的关键帧进行联合优化,优化相机的位姿和三维点和三维线特征,并剔除部分外点和冗余关键帧;
S5、对关键帧进行回环检测,若检测到回环则进行闭合回环并全局优化,减少累积误差。
进一步地,所述步骤S1具体为:输入彩色图像,首先采用ORB算法进行点特征的提取,并根据描述子进行点特征匹配,再通过PROSAC方法剔除点特征间的误匹配;之后采用EDLine算法对线特征提取,根据距离、角度信息和描述子作为筛选标准合并割裂线段,并根据LBD描述子对线特征进行匹配。
进一步地,所述步骤S2具体为:首先对深度有效的像素点求出其对应的三维点,形成有组织的点云序列结构,接着使用层次聚类算法使小平面块融合为更大的平面,并对融合的平面进行分割优化,最后对分割好的平面求出平面的三维中心点PC=[XCYCZC]T和平面的单位法向量n=[nxnynz]T,其中XC、YC、ZC为中心点PC对应的三维坐标值,nx、ny、nz为法向量n对应的值;则相机光心到平面的距离该平面的面特征表示为π=[nTd]T;接着对图像中的平面投影到世界坐标系上并根据法线夹角大小和世界坐标系原点到平面距离的差值大小对投影平面与地图面进行匹配。
进一步地,所述步骤S3具体为:
S31、对步骤S2所提取的平面的法向量进行遍历,寻找是否存在三个互相正交的法向量组合,若是只存在两个互相正交的法向量组合,其另外一维法向量通过两个相互正交的法向量叉积获得;
S32、若步骤S31所寻找的法向量组合存在,则在曼哈顿世界地图中查询历史关键帧中是否观测到与这组正交法向量一致的互相正交的平面,若存在则构成一个曼哈顿世界坐标系,取对应平面点数总和最多的法向量组合所构成的曼哈顿世界坐标系,并求出该曼哈顿世界坐标系到该帧相机坐标系的旋转矩阵为:
其中n1、n2、n3表示找到的相互正交的法向量,ci表示当前帧的id,mk表示曼哈顿世界坐标系的id;对作SVD分解,得到正交化后曼哈顿世界坐标系到相机坐标系的旋转矩阵/>则相机坐标系到世界坐标系的旋转矩阵为:
其中cj表示所找到的历史关键帧的id,为第cj帧时曼哈顿世界坐标系mk到相机坐标系的旋转矩阵,/>为第cj帧时世界坐标系到相机坐标系的旋转矩阵;
然后跟踪点特征、线特征和平面特征去优化相机位移t,其位移的误差模型et为:
其中ep、el、eπ分别表示点特征、线特征、平面特征的重投影误差,具体形式分别为:
ep=p-(KRcwPw+tcw)
el=lT(KRcwPL+tcw)
其中K为相机内参,Rcw为世界坐标系到相机坐标系的旋转矩阵,tcw为世界坐标系到相机坐标系的位移,Tcw为世界坐标系到相机坐标系的位姿变换矩阵;p为该帧识别到的点特征的像素坐标,Pw为该点特征对应的三维点;l为该帧识别到的图像中的线特征,L为该线特征对应的三维线;PL为三维线的三维端点;为面特征参数表达形式,π=[nx ny nz]T,nx、ny、nz为平面π的法向量对应的值,d为相机光心到平面π的距离;πc为该帧检测到的平面,πw为该平面对应的地图面;其中Λp、Λl、Λπ分别表示该点特征、线特征和平面特征的信息矩阵,ρp、ρl、ρπ分别为点特征、线特征和平面特征的Huber鲁棒核函数,Φp(p)、Φl(l)、Φn(π)分别表示点特征、线特征和平面特征的置信度系数,分别为:
其中np、nl、nπ分别表示对应的点特征、线特征、平面被观测的次数,tp、tl、tπ分别为点特征、线特征和平面特征的权重系数,leveli代表该点特征所在ORB金字塔的层数;α为权重系数,α∈[0.5,1];θi表示第i帧相机视线与地图线的夹角,
S33、假如不存在步骤S31所求的曼哈顿世界坐标系或曼哈顿世界地图中的历史关键帧没有观测到对应的法向量组合,则跟踪点特征、线特征和平面特征去优化相机位姿R,t,相机位姿误差模型eR,t为:
进一步地,所述步骤S4具体为:
S41、根据点特征和线特征的匹配对数、是否检测到新的平面、特征跟踪情况以及局部地图内的关键帧情况来判断是否设该帧为关键帧,若为关键帧则加入局部地图,否则返回步骤S1;
S42、对于新***的关键帧,更新共视图和生成树,加入新关键帧节点;根据观测一致性对从被创建开始未被连续可靠观测到的点特征、线特征和平面特征进行剔除;对新关键帧没有匹配的点特征和线特征根据深度信息反投影生成新的地图点和地图线并***到地图中;根据图像中的平面垂直关系记录垂直平面组合与当前关键帧的关联,并更新曼哈顿世界地图;
S43、完整地图更新之后,对当前关键帧和与之关联的关键帧进行联合优化,并在优化中剔除外点以最大程度优化相机位姿;优化对象为相关关键帧的相机位姿R,t以及三维特征参数P,L,优化过程中采用的重投影误差e为:
其中ep、el分别表示点特征和线特征的重投影误差,Φp(p)、Φl(l)分别表示点特征和线特征的置信度系数,Λp、Λl分别表示该点特征和线特征的信息矩阵,ρp、ρl分别为点特征和线特征的Huber鲁棒核函数;
S44、将关键帧之间特征的重合度大于90%的关键帧剔除。
进一步地,判断关键帧的条件具体为:
(1)***关键帧之后已处理多于10帧图像,且当前局部建图线程处于空闲状态,则判断为关键帧;
(2)上一次***关键帧之后已处理超过20帧图像,则判断为关键帧;
(3)当前图像上匹配的点特征、线特征和平面特征总和不少于20,否则不能作为关键帧;
(4)当前图像上跟踪的特征相比最近关键帧跟踪的特征少于90%,则判断为关键帧;
(5)图像中提取到新平面,则判断为关键帧。
进一步地,所述步骤S5具体为:基于点线特征词典模型,首先根据当前关键帧所有点线特征对应的描述子,在词典K叉树中找到所述的单词种类,计算每种单词对应的权重,得到当前关键帧的单词向量;计算当前关键帧的单词向量与其他关键帧单词向量之间的预选相似度,获得帧与帧之间的相似度并根据相似度获得当前关键帧的回环帧;构建回环内所有关键帧的位姿图,进行全局位姿图优化,减少累积误差。
进一步地,预选相似度为:
其中vc为当前关键帧的单词向量,vo为其他关键帧的单词向量,其中单词向量v的具体形式为:
v={(w1,η1),(w2,η2),…,(wi,ηi)}
其中wi为视觉词典中的第i个单词,ηi为wi的单词权重,其中单词权重ηi计算方式为:
其中ni为图像中属于单词wi的特征数量,n为图像中的点线特征总数量,N为关键帧数据库中所有特征数量,Ni为数据库中属于单词wi的点线特征总数量。
进一步地,所述步骤S1具体为:输入彩色图像,首先采用ORB算法进行点特征的提取,并根据描述子进行点特征匹配,再通过使用RANSAC方法剔除点特征间的误匹配;之后采用EDLine算法对线特征提取,根据距离、角度信息和描述子作为筛选标准合并割裂线段,并根据LBD描述子对线特征进行匹配。
进一步地,所述步骤S1具体为:输入彩色图像,首先使用LSD算法提取线特征,并根据描述子进行点特征匹配,再通过使用RANSAC方法剔除点特征间的误匹配;之后采用EDLine算法对线特征提取,根据距离、角度信息和描述子作为筛选标准合并割裂线段,并根据LBD描述子对线特征进行匹配。
与现有技术相比,本发明具有如下显著性有益效果:
(1)本发明改善了仅基于点特征法的视觉SLAM***定位精度低、鲁棒性差等难题,能够在低纹理、光照变化大的室内人工结构化场景下达到满意的定位跟踪效果;
(2)本发明通过利用室内场景中的结构化特征,基于曼哈顿世界假设,通过提取曼哈顿世界坐标系求出绝对的曼哈顿世界坐标系到相机坐标系的旋转矩阵,进而直接获得世界坐标系到相机坐标系的旋转矩阵,从而极大减小帧间累积漂移误差,提高跟踪效果和精度。
附图说明
图1为实施例一种基于点线面特征的结构化场景视觉SLAM方法的流程图;
图2为本发明在TUM fr3_str_notext_far数据序列中估计的轨迹对比图;
图3为本发明在TUM fr3_str_notext_far数据序列中估计的xyz位移对比图;
图4为本发明在TUM fr3_str_notext_far数据序列中估计的三轴欧拉角姿态对比图;
图5为本发明在TUM fr3_str_notext_far数据序列中重建的稀疏三维特征图;
图6为本发明在TUM fr3_str_notext_far数据序列中重建的平面网格图。
具体实施方式
下面结合具体的实施例对本发明技术方案做进一步详细、完整地说明。
如图1所示,本发明提供的一种基于点线面特征的结构化场景视觉SLAM方法,包括如下步骤:
步骤S1、输入彩色图像,首先采用ORB算法对点特征提取和根据描述子去匹配,然后通过PROSAC方法剔除点特征间误匹配,其中在本实施例中除了可以用PROSAC方法剔除点特征间的误匹配,也可以使用RANSAC方法剔除点特征间的误匹配;接着采用EDLine算法对线特征提取,根据端点距离、角度和描述子信息作为筛选标准合并割裂线段,并根据LBD描述子对线特征进行匹配;
步骤S2、输入深度图像,首先对深度有效的像素点求出其对应的三维点,形成有组织的点云序列结构;然后根据所求点云序列使用层次聚类算法快速提取平面;接着根据平面参数对图像中的平面和地图面进行匹配;其具体步骤为:
步骤S21、输入深度图像,对深度有效的像素点求出其对应的三维点,形成有组织的点云序列结构,方便接下来算法处理;
步骤S22、初始化图模型,将图像中的点云均匀分成10×10大小的方块,每一块相当于图模型中的一个节点,当节点的均方误差高于设定阈值或节点含有深度缺失的数据或节点包含的深度不连续的点或节点在两个平面的边界时就把该节点和连接的边从图模型中删除;
步骤S23、使用层次聚类算法,首先建立一个最小堆的数据结构,使得能够更有效找到有最小均方误差的节点进行融合;然后再次计算融合后的平面拟合均方误差,找到获得均方误差最小的对应两个节点;若均方误差超过一个预先设定的阈值,一个平面的分割节点就找到并从图中提出,否则就把这两个节点融合重新加到构建的图中,对建立的最小堆进行更新;重复前两个步骤直到图中的节点全部被取出;
步骤S24、对平面进一步分割优化,对边缘处产生的锯齿状分割采用腐蚀边界区域进行优化;对未使用的数据点采用就近分类到该点周围的平面;对过度分割的面在特别小的图中再次进行层次聚类;
步骤S25、对分割好的平面求出平面的中心点PC=[XCYCZC]T和其单位法向量n=[nxnynz]T,其中XC,YC,ZC为中心点PC对应的三维坐标值,nx,nynz为法向量n对应的值;则相机光心到平面的距离则该平面的面特征表示为π=[nTd]T;
步骤S26、将图像中的平面投影到世界坐标系上并根据法线夹角大小和世界坐标系原点到平面距离的差值大小对投影平面与地图面进行匹配;
步骤S3、检测图像中是否存在曼哈顿世界坐标系,若存在且曼哈顿世界地图中的历史关键帧观测到,则根据所提取得到的曼哈顿世界坐标系求相机旋转矩阵并跟踪点特征、线特征和平面特征优化相机位移,否则跟踪点特征、线特征和平面特征优化相机位姿,具体步骤为:
步骤S31、对步骤S24所提取的平面其对应的法向量进行遍历寻找是否存在3个互相正交的法向量组合,或是否存在2个互相正交的法向量组合,其另外一维法向量通过2个相互正交的法向量叉积获得;
步骤S32、假如存在则在曼哈顿世界地图中查询历史关键帧中是否存在与这组正交法向量一致的3个互相正交的平面组合,若存在则构成一个曼哈顿世界坐标系,取对应平面点数总和最多的法向量组合所构成的曼哈顿世界坐标系,并求出该曼哈顿世界坐标系到该帧相机坐标系的旋转矩阵为:
其中n1,n2,n3代表找到的相互正交的法向量,ci表示当前帧的id,mk表示曼哈顿世界坐标系的id;因为传感器噪声的存在会导致n1,n2,n3不完全正交或不为正交矩阵,需要对/>作SVD分解,得到正交化后曼哈顿世界坐标系到相机坐标系的旋转矩阵:则相机坐标系到世界坐标系的旋转矩阵为:
其中cj表示所找到的历史关键帧的id,为第cj帧时曼哈顿世界坐标系mk到相机坐标系的旋转矩阵,/>为第cj帧时世界坐标系到相机坐标系的旋转矩阵;
然后跟踪点特征、线特征和平面特征优化相机位移t,其位移的误差模型et为:
其中ep,el,eπ分别表示点特征、线特征和平面特征的重投影误差,具体形式分别为:
ep=p-(KRcwPw+tcw)
el=lT(KRcwPL+tcw)
其中K为相机内参,Rcw为世界坐标系到相机坐标系的旋转矩阵,tcw为世界坐标系到相机坐标系的位移,Tcw为世界坐标系到相机坐标系的位姿变换矩阵;p为该帧识别到的点特征的像素坐标,Pw为该点特征对应的三维点;l为该帧识别到的线特征,L为该线特征对应的三维线;PL为该三维线的三维端点;为本发明采取的平面特征参数表达形式,π=[nx ny nz]T,nx、ny、nz为平面π的法向量对应的值,d为相机光心到平面π的距离;πc为该帧检测到的平面,πw为该平面对应的地图面;其中Λp,Λl,Λπ分别表示点特征、线特征和平面特征的信息矩阵,ρp、ρl、ρπ分别为点特征、线特征和平面特征的Huber鲁棒核函数,Φp(p),Φl(l),Φπ(π)分别表示点特征、线特征和平面特征的置信度,分别为:
其中np,nl,nπ分别表示对应的点特征、线特征和平面特征被观测的次数,tp,tl,tπ为点特征、线特征和平面特征的权重系数,leveli代表该点特征所在ORB金字塔的层数;α为权重系数,α∈[0.5,1];θi表示第i帧相机视线与地图线的夹角,
其中在本实施例平面特征采用的是球坐标参数表示形式π=[nx ny nz]T,nx、ny、nz为平面π的法向量对应的值,d为相机光心到平面π的距离;此外平面特征也可以采用单位四元数参数表示形式或者是最近点参数表示形式表示平面特征。
步骤S33、假如不存在S31所求的曼哈顿世界坐标系或曼哈顿世界地图中的历史关键帧没有观测到对应的法向量组合,则跟踪点特征、线特征和平面特征优化相机位姿R,t,相机位姿误差模型eR,t为:
其中ep、el、eπ分别表示点特征、线特征和平面特征的重投影误差,Φp(p)、Φl(l)、Φπ(π)分别表示点特征、线特征和平面特征的置信度系数,Λp、Λl、Λπ分别表示该点特征、线特征和平面特征的信息矩阵,ρp、ρl、ρπ分别为点特征、线特征和平面特征的Huber鲁棒核函数;
步骤S4、判断当前帧是否为关键帧,若为关键帧,则加入到局部地图中,并更新地图中的三维点、三维线和曼哈顿世界地图,对当前关键帧和相邻的关键帧进行联合优化,优化相机的位姿和三维点和三维线特征,并剔除部分外点和冗余关键帧,具体步骤为:
步骤S41、判断该帧是否为关键帧,若为关键帧,则将其加入到局部地图,否则返回步骤S1,继续处理下一帧图像,其判断的条件为:(1)***关键帧之后已处理多于10帧图像,且当前局部建图线程处于空闲状态,则判断为关键帧;(2)上一次***关键帧之后已处理超过20帧图像,则判断为关键帧;(3)当前图像上匹配的点特征、线特征和平面特征总和不少于20,否则不能作为关键帧;(4)当前图像上跟踪的特征相比最近关键帧跟踪的特征少于90%,则判断为关键帧;(5)图像中提取到新平面,则判断为关键帧;
步骤S42、对于新***的关键帧,首先更新共视图和生成树,加入新关键帧节点;然后剔除局部地图中的三维特征被观测次数少于三次的点特征和线特征;对新关键帧没有匹配的点线特征根据深度信息反投影生成新的地图点和地图线并***到地图中;最后根据图像中的平面垂直关系记录垂直平面组合与当前关键帧的关联,并更新曼哈顿世界地图;
步骤S43、完整地图更新之后,对当前关键帧和与之关联的关键帧进行联合优化,主要优化对象为相关关键帧的相机位姿以及三维特征参数,这里平面的参数不作为优化对象;在迭代优化10次后将重投影误差过大的外点移除,再继续迭代优化,重复4次,以尽可能地清除外点,最大程度优化相机位姿R,t和三维特征信息P,L,这里不优化平面的参数;优化过程中采用的重投影误差e如下式所示:
其中ep、el分别表示点特征、线特征的重投影误差,Φp(p)、Φl(l)分别表示点特征和线特征的置信度系数,Λp、Λl分别表示该点特征和线特征的信息矩阵,ρp、ρl分别为点特征和线特征的Huber鲁棒核函数;
步骤S44、将关键帧之间特征的重合度大于90%的关键帧剔除,以精简共视图;
步骤S5、对关键帧进行回环检测以及全局优化,其具体步骤为:
步骤S51、基于点线特征词典模型,首先根据当前关键帧所有点特征和线特征对应的描述子,在词典K叉树中找到所述的单词种类,计算每种单词对应的权重,得到当前关键帧的单词向量;
步骤S52、计算当前关键帧的单词向量与其他关键帧单词向量之间的预选相似度s,选取相似度最高且超过设定阈值的关键帧作为当前关键帧的回环帧,其中当前关键帧的单词向量与其他关键帧单词向量之间的预选相似度为:
其中vc为当前关键帧的单词向量,vo为其他关键帧的单词向量,其中单词向量v的具体形式为:
v={(w1,η1),(w2,η2),…,(wk,ηk)}
其中wi为视觉词典中的第i个单词,ηi为wi的单词权重,其中单词权重ηi计算方式为:
其中ni为图像中属于单词wi的特征数量,n为图像中的点线特征总数量,N为关键帧数据库中所有特征数量,Ni为数据库中属于单词wi的点线特征总数量。
步骤S53、构建回环内所有关键帧的位姿图,位姿图的顶点为每一个关键帧的位姿,边为关键帧之间的共视点特征或共视线特征,实现全局位姿图优化。
步骤S6、结果输出:位姿保存,路径绘制和地图的保存与同步实时显示,如图2为本发明在TUM fr3_str_notext_far数据序列中估计的轨迹对比图,图3的a、b、c为本发明在TUM fr3_str_notext_far数据序列中估计的xyz轴位移对比图,图4的a、b、c为本发明在TUMfr3_str_notext_far数据序列中估计的xyz轴欧拉角姿态对比图;如图5为本发明在TUMfr3_str_notext_far数据序列中重建的稀疏三维特征图,图6为本发明在TUM fr3_str_notext_far数据序列中重建的平面网格图。
实施例2
一种基于点线面特征的结构化场景视觉SLAM方法,包括以下步骤:
S1、输入彩色图像,根据彩色图像提取点特征和线特征并进行特征匹配;其中在本实施例中采用EDLine算法提取线特征,也可以使用LSD算法提取线特征,并且对LSD算法提取的割裂线段的合并方法同样是根据端点距离、角度和描述子信息作为筛选标准;
S2、输入深度图像,转换成点云序列结构,然后提取图像平面,接着对提取的图像平面与地图面进行匹配;在本实施例中关于平面匹配除了根据法线夹角大小和世界坐标系原点到平面距离的差值大小作为判断平面匹配标准,也可以采用法线夹角大小与两平面间是否存在碰撞区域作为判断平面匹配标准;
S3、检测图像中是否存在曼哈顿世界坐标系,若存在且在曼哈顿世界地图中的历史关键帧观测到该坐标系,则根据所提取到的曼哈顿世界坐标系估计相机姿态并跟踪点特征、线特征和平面特征去优化相机位移,否则跟踪点特征、线特征和平面特征去优化相机位姿;
S4、判断当前帧是否为关键帧,若为关键帧,则加入到局部地图中,并更新地图中的三维点、三维线和曼哈顿世界地图,对当前关键帧和相邻的关键帧进行联合优化,优化相机的位姿和三维点和三维线特征,并剔除部分外点和冗余关键帧;
S5、对关键帧进行回环检测,若检测到回环则进行闭合回环并全局优化,减少累积误差。
实施例3
一种基于点线面特征的结构化场景视觉SLAM方法,包括以下步骤:
S1、输入彩色图像,根据彩色图像提取点特征和线特征并进行特征匹配;其中在本实施例中使用LSD算法提取线特征,并且对LSD算法提取的割裂线段的合并方法同样是根据端点距离、角度和描述子信息作为筛选标准;
S2、输入深度图像,转换成点云序列结构,然后提取图像平面,接着对提取的图像平面与地图面进行匹配;在本实施例中关于平面匹配除了根据法线夹角大小和世界坐标系原点到平面距离的差值大小作为判断平面匹配标准,也可以采用法线夹角大小与两平面间是否存在碰撞区域作为判断平面匹配标准;
S3、检测图像中是否存在曼哈顿世界坐标系,若存在且在曼哈顿世界地图中的历史关键帧观测到该坐标系,则根据所提取到的曼哈顿世界坐标系估计相机姿态并跟踪点特征、线特征和平面特征去优化相机位移,否则跟踪点特征、线特征和平面特征去优化相机位姿;
S4、判断当前帧是否为关键帧,若为关键帧,则加入到局部地图中,并更新地图中的三维点、三维线和曼哈顿世界地图,对当前关键帧和相邻的关键帧进行联合优化,优化相机的位姿和三维点和三维线特征,并剔除部分外点和冗余关键帧;
S5、对关键帧进行回环检测,若检测到回环则进行闭合回环并全局优化,减少累积误差。
以上实施例仅用以说明本发明的技术方案而非对其进行限制,在不背离本发明精神及其实质的情况下,熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形,但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。
Claims (9)
1.一种基于点线面特征的结构化场景视觉SLAM方法,其特征在于,包括以下步骤:
S1、输入彩色图像,根据彩色图像提取点特征和线特征并进行特征匹配;
S2、输入深度图像,转换成点云序列结构,然后提取图像平面,接着对提取的图像平面与地图面进行匹配;
S3、检测图像中是否存在曼哈顿世界坐标系,若存在且在曼哈顿世界地图中的历史关键帧观测到该坐标系,则根据所提取到的曼哈顿世界坐标系估计相机姿态并跟踪点特征、线特征和平面特征去优化相机位移,否则跟踪点特征、线特征和平面特征去优化相机位姿;具体为:
S31、对步骤S2所提取的平面的法向量进行遍历,寻找是否存在三个互相正交的法向量组合,若是只存在两个互相正交的法向量组合,其另外一维法向量通过两个相互正交的法向量叉积获得;
S32、若步骤S31所寻找的法向量组合存在,则在曼哈顿世界地图中查询历史关键帧中是否观测到与这组正交法向量一致的互相正交的平面,若存在则构成一个曼哈顿世界坐标系,取对应平面点数总和最多的法向量组合所构成的曼哈顿世界坐标系,并求出该曼哈顿世界坐标系到该帧相机坐标系的旋转矩阵为:
其中n1、n2、n3表示找到的相互正交的法向量,ci表示当前帧的id,mk表示曼哈顿世界坐标系的id;对作SVD分解,得到正交化后曼哈顿世界坐标系到相机坐标系的旋转矩阵则相机坐标系到世界坐标系的旋转矩阵为:
其中cj表示所找到的历史关键帧的id,为第cj帧时曼哈顿世界坐标系mk到相机坐标系的旋转矩阵,/>为第cj帧时世界坐标系到相机坐标系的旋转矩阵;
然后跟踪点特征、线特征和平面特征去优化相机位移t,其位移的误差模型et为:
其中ep、el、eπ分别表示点特征、线特征、平面特征的重投影误差,具体形式分别为:
ep=p-(KRcwPw+tcw)
el=lT(KRcwPL+tcw)
其中K为相机内参,Rcw为世界坐标系到相机坐标系的旋转矩阵,tcw为世界坐标系到相机坐标系的位移,Tcw为世界坐标系到相机坐标系的位姿变换矩阵;p为该帧识别到的点特征的像素坐标,Pw为该点特征对应的三维点;l为该帧识别到的图像中的线特征,L为该线特征对应的三维线;PL为三维线的三维端点;为平面特征参数表达形式,π=[nx、ny、nz]T,nx、my、nz为平面π的法向量对应的值,d为相机光心到平面π的距离;πc为该帧检测到的平面,πw为该平面对应的地图面;其中Λp、Λl、Λπ分别表示该点特征、线特征和平面特征的信息矩阵,ρp、ρl、ρπ分别为点特征、线特征和平面特征的Huber鲁棒核函数,Φp(p)、Φl(l)、Φπ(π)分别表示点特征、线特征和平面特征的置信度系数,分别为:
其中np、nl、nπ分别表示对应的点特征、线特征、平面被观测的次数,tp、tl、tπ分别为点特征、线特征和平面特征的权重系数,leveli代表该点特征所在ORB金字塔的层数;α为权重系数,α∈[0.5,1];θi表示第i帧相机视线与地图线的夹角,
S33、假如不存在步骤S31所求的曼哈顿世界坐标系或曼哈顿世界地图中的历史关键帧没有观测到对应的法向量组合,则跟踪点特征、线特征和平面特征去优化相机位姿R,t,相机位姿误差模型eR,t为:
S4、判断当前帧是否为关键帧,若为关键帧,则加入到局部地图中,并更新地图中的三维点、三维线和曼哈顿世界地图,对当前关键帧和相邻的关键帧进行联合优化,优化相机的位姿和三维点和三维线特征,并剔除部分外点和冗余关键帧;
S5、对关键帧进行回环检测,若检测到回环则进行闭合回环并全局优化,减少累积误差。
2.根据权利要求1所述的一种基于点线面特征的结构化场景视觉SLAM方法,其特征在于:所述步骤S1具体为:输入彩色图像,首先采用ORB算法进行点特征的提取,并根据描述子进行点特征匹配,再通过PROSAC方法剔除点特征间的误匹配;之后采用EDLine算法对线特征提取,根据距离、角度信息和描述子作为筛选标准合并割裂线段,并根据LBD描述子对线特征进行匹配。
3.根据权利要求1所述的一种基于点线面特征的结构化场景视觉SLAM方法,其特征在于:所述步骤S2具体为:首先对深度有效的像素点求出其对应的三维点,形成有组织的点云序列结构,接着使用层次聚类算法使小平面块融合为更大的平面,并对融合的平面进行分割优化,最后对分割好的平面求出平面的三维中心点PC=[XCYCZC]T和平面的单位法向量n=[nxnynz]T,其中XC、YC、ZC为中心点PC对应的三维坐标值,nx、ny、nz为法向量n对应的值;则相机光心到平面的距离该平面的面特征表示为π=[nTd]T;接着对图像中的平面投影到世界坐标系上并根据法线夹角大小和世界坐标系原点到平面距离的差值大小对投影平面与地图面进行匹配。
4.根据权利要求1所述的一种基于点线面特征的结构化场景视觉SLAM方法,其特征在于:所述步骤S4具体为:
S41、根据点特征和线特征的匹配对数、是否检测到新的平面、特征跟踪情况以及局部地图内的关键帧情况来判断是否设该帧为关键帧,若为关键帧则加入局部地图,否则返回步骤S1;
S42、对于新***的关键帧,更新共视图和生成树,加入新关键帧节点;根据观测一致性对从被创建开始未被连续可靠观测到的点特征、线特征和平面特征进行剔除;对新关键帧没有匹配的点特征和线特征根据深度信息反投影生成新的地图点和地图线并***到地图中;根据图像中的平面垂直关系记录垂直平面组合与当前关键帧的关联,并更新曼哈顿世界地图;
S43、完整地图更新之后,对当前关键帧和与之关联的关键帧进行联合优化,并在优化中剔除外点以最大程度优化相机位姿;优化对象为相关关键帧的相机位姿R,t以及三维特征参数P,L,优化过程中采用的重投影误差e为:
其中ep、el分别表示点特征和线特征的重投影误差,Φp(p)、Φl(l)分别表示点特征和线特征的置信度系数,Λp、Λl分别表示该点特征和线特征的信息矩阵,ρp、ρl分别为点特征和线特征的Huber鲁棒核函数;
S44、将关键帧之间特征的重合度大于90%的关键帧剔除。
5.根据权利要求4所述的一种基于点线面特征的结构化场景视觉SLAM方法,其特征在于:判断关键帧的条件具体为:
(1)***关键帧之后已处理多于10帧图像,且当前局部建图线程处于空闲状态,则判断为关键帧;
(2)上一次***关键帧之后已处理超过20帧图像,则判断为关键帧;
(3)当前图像上匹配的点特征、线特征和平面特征总和不少于20,否则不能作为关键帧;
(4)当前图像上跟踪的特征相比最近关键帧跟踪的特征少于90%,则判断为关键帧;
(5)图像中提取到新平面,则判断为关键帧。
6.根据权利要求1所述的一种基于点线面特征的结构化场景视觉SLAM方法,其特征在于:所述步骤S5具体为:基于点线特征词典模型,首先根据当前关键帧所有点线特征对应的描述子,在词典K叉树中找到单词种类,计算每种单词对应的权重,得到当前关键帧的单词向量;计算当前关键帧的单词向量与其他关键帧单词向量之间的预选相似度,获得帧与帧之间的相似度并根据相似度获得当前关键帧的回环帧;构建回环内所有关键帧的位姿图,进行全局位姿图优化,减少累积误差。
7.根据权利要求6所述的一种基于点线面特征的结构化场景视觉SLAM方法,其特征在于:预选相似度为:
其中vc为当前关键帧的单词向量,vo为其他关键帧的单词向量,其中单词向量v的具体形式为:
v={(w1,η1),(w2,η2),…,(wi,ηi)}
其中wi为视觉词典中的第i个单词,ηi为wi的单词权重,其中单词权重ηi计算方式为:
其中ni为图像中属于单词wi的特征数量,n为图像中的点线特征总数量,N为关键帧数据库中所有特征数量,Ni为数据库中属于单词wi的点线特征总数量。
8.根据权利要求1所述的一种基于点线面特征的结构化场景视觉SLAM方法,其特征在于:所述步骤S1具体为:输入彩色图像,首先采用ORB算法进行点特征的提取,并根据描述子进行点特征匹配,再通过使用RANSAC方法剔除点特征间的误匹配;之后采用EDLine算法对线特征提取,根据距离、角度信息和描述子作为筛选标准合并割裂线段,并根据LBD描述子对线特征进行匹配。
9.根据权利要求1所述的一种基于点线面特征的结构化场景视觉SLAM方法,其特征在于:所述步骤S1具体为:输入彩色图像,首先使用LSD算法提取线特征,并根据描述子进行点特征匹配,再通过使用RANSAC方法剔除点特征间的误匹配;之后采用EDLine算法对线特征提取,根据距离、角度信息和描述子作为筛选标准合并割裂线段,并根据LBD描述子对线特征进行匹配。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210346890.6A CN114862949B (zh) | 2022-04-02 | 2022-04-02 | 一种基于点线面特征的结构化场景视觉slam方法 |
PCT/CN2022/128826 WO2023184968A1 (zh) | 2022-04-02 | 2022-10-31 | 一种基于点线面特征的结构化场景视觉slam方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210346890.6A CN114862949B (zh) | 2022-04-02 | 2022-04-02 | 一种基于点线面特征的结构化场景视觉slam方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114862949A CN114862949A (zh) | 2022-08-05 |
CN114862949B true CN114862949B (zh) | 2024-05-17 |
Family
ID=82629058
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210346890.6A Active CN114862949B (zh) | 2022-04-02 | 2022-04-02 | 一种基于点线面特征的结构化场景视觉slam方法 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN114862949B (zh) |
WO (1) | WO2023184968A1 (zh) |
Families Citing this family (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114862949B (zh) * | 2022-04-02 | 2024-05-17 | 华南理工大学 | 一种基于点线面特征的结构化场景视觉slam方法 |
CN115376051B (zh) * | 2022-10-25 | 2023-03-24 | 杭州华橙软件技术有限公司 | 关键帧的管理方法及管理装置、slam方法、电子设备 |
CN115830110B (zh) * | 2022-10-26 | 2024-01-02 | 北京城市网邻信息技术有限公司 | 即时定位与地图构建方法、装置、终端设备及存储介质 |
CN115578620B (zh) * | 2022-10-28 | 2023-07-18 | 北京理工大学 | 一种点线面多维特征-可见光融合slam方法 |
CN115601434B (zh) * | 2022-12-12 | 2023-03-07 | 安徽蔚来智驾科技有限公司 | 回环检测方法、计算机设备、计算机可读存储介质及车辆 |
WO2024124421A1 (en) * | 2022-12-14 | 2024-06-20 | Qualcomm Incorporated | Robot rotation matrix estimation using manhattan world assumption |
CN116468786B (zh) * | 2022-12-16 | 2023-12-26 | 中国海洋大学 | 一种面向动态环境的基于点线联合的语义slam方法 |
CN117671022B (zh) * | 2023-11-02 | 2024-07-12 | 武汉大学 | 一种室内弱纹理环境的移动机器人视觉定位***及方法 |
CN117611677B (zh) * | 2024-01-23 | 2024-05-14 | 北京理工大学 | 一种基于目标检测和结构化特征的机器人定位方法 |
CN117649536B (zh) * | 2024-01-29 | 2024-04-16 | 华东交通大学 | 一种点线和线结构特征融合的视觉同步定位与建图方法 |
CN117649495B (zh) * | 2024-01-30 | 2024-05-28 | 山东大学 | 基于点云描述符匹配的室内三维点云地图生成方法及*** |
CN117893693B (zh) * | 2024-03-15 | 2024-05-28 | 南昌航空大学 | 一种密集slam三维场景重建方法及装置 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110501017A (zh) * | 2019-08-12 | 2019-11-26 | 华南理工大学 | 一种基于orb_slam2的移动机器人导航地图生成方法 |
CN110853100A (zh) * | 2019-10-24 | 2020-02-28 | 东南大学 | 一种基于改进点线特征的结构化场景视觉slam方法 |
CN112509044A (zh) * | 2020-12-02 | 2021-03-16 | 重庆邮电大学 | 一种基于点线特征融合的双目视觉slam方法 |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP3474230B1 (en) * | 2017-10-18 | 2020-07-22 | Tata Consultancy Services Limited | Systems and methods for edge points based monocular visual slam |
CN108682027A (zh) * | 2018-05-11 | 2018-10-19 | 北京华捷艾米科技有限公司 | 基于点、线特征融合的vSLAM实现方法及*** |
JP2021508393A (ja) * | 2018-09-07 | 2021-03-04 | ホアウェイ・テクノロジーズ・カンパニー・リミテッド | 同時に位置特定およびマッピングを実行するためのデバイスおよび方法 |
CN114241050B (zh) * | 2021-12-20 | 2024-05-07 | 东南大学 | 一种基于曼哈顿世界假设及因子图的相机位姿优化方法 |
CN114862949B (zh) * | 2022-04-02 | 2024-05-17 | 华南理工大学 | 一种基于点线面特征的结构化场景视觉slam方法 |
-
2022
- 2022-04-02 CN CN202210346890.6A patent/CN114862949B/zh active Active
- 2022-10-31 WO PCT/CN2022/128826 patent/WO2023184968A1/zh unknown
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110501017A (zh) * | 2019-08-12 | 2019-11-26 | 华南理工大学 | 一种基于orb_slam2的移动机器人导航地图生成方法 |
CN110853100A (zh) * | 2019-10-24 | 2020-02-28 | 东南大学 | 一种基于改进点线特征的结构化场景视觉slam方法 |
CN112509044A (zh) * | 2020-12-02 | 2021-03-16 | 重庆邮电大学 | 一种基于点线特征融合的双目视觉slam方法 |
Non-Patent Citations (1)
Title |
---|
PLP-SLAM:基于点、线、面特征融合的视觉SLAM方法;李海丰 等;机器人;20170315(第02期);第88-94页 * |
Also Published As
Publication number | Publication date |
---|---|
CN114862949A (zh) | 2022-08-05 |
WO2023184968A1 (zh) | 2023-10-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN114862949B (zh) | 一种基于点线面特征的结构化场景视觉slam方法 | |
CN112132893B (zh) | 一种适用于室内动态环境的视觉slam方法 | |
WO2021233029A1 (en) | Simultaneous localization and mapping method, device, system and storage medium | |
CN110097553B (zh) | 基于即时定位建图与三维语义分割的语义建图*** | |
CN107025668B (zh) | 一种基于深度相机的视觉里程计的设计方法 | |
CN102456225B (zh) | 一种运动目标检测与跟踪方法和*** | |
CN108615246B (zh) | 提高视觉里程计***鲁棒性和降低算法计算消耗的方法 | |
CN110631554A (zh) | 机器人位姿的确定方法、装置、机器人和可读存储介质 | |
CN107607107B (zh) | 一种基于先验信息的Slam方法和装置 | |
CN110599545B (zh) | 一种基于特征的构建稠密地图的*** | |
CN113223045B (zh) | 基于动态物体语义分割的视觉与imu传感器融合定位*** | |
Qian et al. | Rf-lio: Removal-first tightly-coupled lidar inertial odometry in high dynamic environments | |
CN113658337B (zh) | 一种基于车辙线的多模态里程计方法 | |
Proença et al. | Fast cylinder and plane extraction from depth cameras for visual odometry | |
CN112419497A (zh) | 基于单目视觉的特征法与直接法相融合的slam方法 | |
CN110570474B (zh) | 一种深度相机的位姿估计方法及*** | |
CN116449384A (zh) | 基于固态激光雷达的雷达惯性紧耦合定位建图方法 | |
CN111998862A (zh) | 一种基于bnn的稠密双目slam方法 | |
CN116879870A (zh) | 一种适用于低线束3d激光雷达的动态障碍物去除方法 | |
CN116468786A (zh) | 一种面向动态环境的基于点线联合的语义slam方法 | |
Zhang et al. | Stereo plane slam based on intersecting lines | |
Zhu et al. | PairCon-SLAM: Distributed, online, and real-time RGBD-SLAM in large scenarios | |
CN117253003A (zh) | 一种融合直接法与点面特征法的室内rgb-d slam方法 | |
CN117011660A (zh) | 一种低纹理场景下融合深度信息的点线特征slam方法 | |
WO2023030062A1 (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 |