CN112560648A - 一种基于rgb-d图像的slam方法 - Google Patents
一种基于rgb-d图像的slam方法 Download PDFInfo
- Publication number
- CN112560648A CN112560648A CN202011448199.6A CN202011448199A CN112560648A CN 112560648 A CN112560648 A CN 112560648A CN 202011448199 A CN202011448199 A CN 202011448199A CN 112560648 A CN112560648 A CN 112560648A
- Authority
- CN
- China
- Prior art keywords
- frame
- color image
- computer
- feature point
- key frame
- 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
Links
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
- 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
- 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/044—Recurrent networks, e.g. Hopfield 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/04—Architecture, e.g. interconnection topology
- G06N3/049—Temporal neural networks, e.g. delay elements, oscillating neurons or pulsed inputs
-
- 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/44—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components
Landscapes
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Life Sciences & Earth Sciences (AREA)
- Artificial Intelligence (AREA)
- General Engineering & Computer Science (AREA)
- Evolutionary Computation (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Computational Linguistics (AREA)
- Multimedia (AREA)
- Software Systems (AREA)
- Health & Medical Sciences (AREA)
- Biomedical Technology (AREA)
- Biophysics (AREA)
- Mathematical Physics (AREA)
- General Health & Medical Sciences (AREA)
- Molecular Biology (AREA)
- Computing Systems (AREA)
- Evolutionary Biology (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Bioinformatics & Computational Biology (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于RGB‑D图像的SLAM方法,该方法包括以下步骤:一、RGB‑D图像的获取;二、RGB‑D图像的特征点提取与匹配;三、计算旋转矩阵和平移矩阵;四、RGB‑D图像中关键帧的获取;步骤五、关键帧的闭环判断;步骤六、RGB‑D图像的全局优化;步骤七、构建网格地图。本发明方法步骤简单、设计合理且实现方便,解决暴力匹配问题,且减少匹配点对质量差别造成深度相机位姿的误差,提高了SLAM的精度和实时性,从而能够满足机器人实时SLAM的要求,并且具有极高的精度。
Description
技术领域
本发明属于网格地图构建技术领域,尤其是涉及一种基于RGB-D图像的SLAM方法。
背景技术
随着智能机器人技术的不断发展,研究者们希望有朝一日机器人可以像人类一样,通过眼睛去观察和理解周围的世界,完成自身的定位和对周围环境的建模。一般将这个问题称为即时定位与地图构建(Simultaneous Localization and Mapping,SLAM)。VSLAM是指通过视觉传感器的方式完成即时定位与地图构建。把视觉传感器和红外传感器集成到一起并实现两种传感器信息之间的良好同步,这样就构成了RGB-D传感器。基于RGB-D传感器的VSLAM则被称为RGB-D SLAM。目前的SLAM方法主要有RGB-D图像采用SIFT特征点提取,然后采用最近迭代点ICP算法求解两帧图像之间的最优变换,最后利用图优化工具TORO进行全局优化;还有利用三维重建的方案,可以在不需要RGB图只需要深度图的情况下就能实时地建立三维模型。还有基于单目、双目和RGB-D相机的完整的SLAM系统。但是上述方法还存一些问题:第一,采用暴力匹配对特征点进行匹配,需要每个特征与其他所有的特征进行匹配,匹配范围大,耗时严重;第二,精度和实时性不太理性。因此,需要一种基于RGB-D图像的SLAM方法,方法步骤简单、设计合理且实现方便,解决暴力匹配问题,且提高了SLAM的精度和实时性,从而进一步便于满足机器人实时SLAM的要求,并且具有极高的精度。
发明内容
本发明所要解决的技术问题在于针对上述现有技术中的不足,提供一种基于RGB-D图像的SLAM方法,其方法步骤简单、设计合理且实现方便,解决暴力匹配问题,且提高了SLAM的精度和实时性,从而进一步便于满足机器人实时SLAM的要求,并且具有极高的精度。
为解决上述技术问题,本发明采用的技术方案是:一种基于RGB-D图像的SLAM方法,其特征在于,该方法包括以下步骤:
步骤一、RGB-D图像的获取:
采用深度相机按照预设设定的采集时间对待拍摄空间进行实时采集,并将各个采样时刻采集到的RGB-D图像发送至计算机;其中,第i个采样时刻采集到的RGB-D图像包括第i帧彩色图像和第i帧彩色图像所对应的深度值,i为正整数,且1≤i≤M,M表示彩色图像的总帧数;
步骤二、RGB-D图像的特征点提取与匹配:
步骤201、计算机采用ORB算法对第i帧彩色图像进行特征点提取,得到第i帧彩色图像的特征点描述子集;其中,第i帧彩色图像中第a个特征点对应的特征点描述子记作第i帧彩色图像中第a个特征点描述子a为正整数,且1≤a≤A,A表示特征点的总数;
同时,计算机采用ORB算法对第i+1帧彩色图像进行特征点提取,得到第i+1帧彩色图像的特征点描述子集;其中,第i+1帧彩色图像中第a′个特征点对应的特征点描述子记作第i+1帧彩色图像中第a′个特征点描述子a′为正整数,1≤a′≤A;
步骤202、计算机采用K-means++算法对第i帧彩色图像的特征点描述子和第i+1帧彩色图像中的特征点描述子进行匹配,得到多组匹配点对;
步骤三、计算旋转矩阵和平移矩阵:
步骤301、计算机将每组匹配点对之间的汉明距离按照从小到大的顺序进行排序,并选取前N组匹配点对;
步骤302、计算机采用随机抽样一致RANSAC算法和PnP算法,对N组匹配点对进行姿态估计,得到第i+1帧彩色图像和第i帧彩色图像之间的姿态、特征匹配的内点数Inliersi,i+1;其中,第i+1帧彩色图像和第i帧彩色图像之间的姿态包括第i+1帧彩色图像和第i帧彩色图像之间的旋转矩阵Ri+1,i和平移向量ti+1,i;其中,旋转矩阵Ri+1,i为3×3矩阵,平移向量ti+1,i为3×1向量;
步骤四、RGB-D图像中关键帧的获取:
步骤401、选取第1帧彩色图像作为关键帧;
步骤402、选取下一帧彩色图像即第2帧彩色图像作为当前帧彩色图像;
步骤403、按照步骤二和步骤三所述的方法,得到第2帧彩色图像和第1帧彩色图像即当前帧彩色图像和上一个关键帧之间的特征匹配的内点数Inliersc,s,计算机将Inliersc,s和设定的最小内点数Inmin进行判断,如果Inliersc,s>Inmin,执行步骤404和步骤405;否则,舍弃该当前帧彩色图像;
步骤404、根据公式Dc,s=||tc,s *||+min(2π-||Rc,s *||,||Rc,s *||),得到当前帧彩色图像和上一个关键帧之间的相对运动距离Dc,s;其中,Rc,s *表示第2帧彩色图像和第1帧彩色图像之间即当前帧彩色图像和上一个关键帧之间的优化后的旋转矩阵,tc,s *表示第2帧彩色图像和第1帧彩色图像之间即当前帧彩色图像和上一个关键帧之间的优化后的平移向量,||·||表示向量的模;
步骤405、计算机将Dc,s与运动距离最小阈值Dmin和运动距离最大阈值Dmax进行判断,如果Dmin<Dc,s<Dmax成立,则第2帧彩色图像即当前帧彩色图像为关键帧;否则,舍弃该当前帧彩色图像;
步骤406、按照步骤401和步骤405所述的方法,对上一个关键帧和下一帧彩色图像之间进行判断,直至完成M-2帧彩色图像的判断,得到各个关键帧;
步骤五、关键帧的闭环判断:
步骤501、计算机将步骤406中得到的各个关键帧按照采样时间先后顺序存入数据库图像中;其中,关键帧的总数为B;
步骤502、计算机根据公式Nz=B×A,得到数据库图像中特征点的总数Nz;
步骤503、计算机根据公式得到第b个关键帧中第t个特征点的词频-逆文档频率其中,Nz,t表示第b个关键帧中第t个特征点在数据库图像中出现的数量,Nb,t表示第b个关键帧中第t个特征点出现的数量,t为正整数,1≤t≤A,b为正整数,且1≤b≤B;
步骤504、计算机将剩余B-b个关键帧记作待判断关键帧,计算机根据公式得到第b′个待判断关键帧中第t′个特征点的词频-逆文档频率其中,Nz,t′表示第b′个待判断关键帧中第t′个特征点在数据库图像中出现的数量,Nb′,t′表示第b′个待判断关键帧中第t′个特征点出现的数量,b′和t′均为正整数,且b′的取值位于b+1~B,且b+1不大于B,1≤t′≤A;
步骤505、计算机根据,得到第b个关键帧和第b′个待判断关键帧之间的相似度S(qb,db′);其中,qb表示第b个关键帧中各个特征点的词频-逆文档频率向量,且db′表示第b′个待判断关键帧中各个特征点的词频-逆文档频率向量,且||·||表示向量的模;
步骤507、按照步骤506所述的方法,得到各个关键帧之间的相似度,获取相似度最大值并记作Smax,并设定相似度阈值系数为Sth;
步骤508、计算机将S(qb,db′)和Sth×Smax进行判断,如果S(qb,db′)大于Sth×Smax,则第b个关键帧和第b′个待判断关键帧作为候选闭环帧序列;
步骤509、按照步骤二和步骤三所述的方法,得到第b个关键帧和第b′个待判断关键帧之间优化后的旋转矩阵和优化后的平移向量以及第b个关键帧和第b′个待判断关键帧之间的特征匹配的内点数Inliersb,b′(key);
步骤5010、计算机将第b个关键帧和第b′个待判断关键帧之间的特征匹配的内点数Inliersb,b′(key)和特征匹配的内点数阈值进行判断,如果Inliersb,b′(key)大于内点数阈值,则第b个关键帧和第b′个待判断关键帧作为一组闭环帧,
步骤5011、按照步骤502至步骤5010所述的方法,对第b+1个关键帧和待判断关键帧进行判断,直至完成B-1个关键帧的判断,得到多组闭环帧;
步骤六、RGB-D图像的全局优化:
计算机对存在多组闭环帧的各个关键帧进行全局优化,得到优化后的位姿图;
步骤七、构建网格地图:
计算机根据优化后的位姿图和关键帧构建三角化网格地图。
上述的一种基于RGB-D图像的SLAM方法,其特征在于:步骤202中采用计算机对第i帧彩色图像和第i+1帧彩色图像中的特征点描述子进行匹配,得到多组匹配点对,具体过程如下:
步骤2021、计算机设定聚类种类k为5,设定聚类层数L为10,计算机采用K-means++算法对第i帧彩色图像的特征点描述子集进行聚类,得到第i帧彩色图像的特征点描述子词汇树,并记作第i个特征点描述子词汇树;
计算机采用K-means++算法对第i+1帧彩色图像的特征点描述子集进行聚类,得到第i+1帧彩色图像的特征点描述子词汇树,并记作第i+1个特征点描述子词汇树;
步骤2022、计算机获取第i个特征点描述子词汇树L层中各个叶子节点的聚类中心和第i+1个特征点描述子词汇树L层中各个叶子节点的聚类中心;
步骤2023、计算机将第i个特征点描述子词汇树第10层中各个叶子节点中的特征点描述子分别和第i+1个特征点描述子词汇树从第4层开始进行特征匹配判断的方法均相同,其中,计算机将第i个特征点描述子词汇树第10层中第m10个叶子节点中的特征点描述子分别和第i+1个特征点描述子词汇树第4层进行特征匹配判断,具体过程如下:
步骤20231、计算机获取第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子和第i+1个特征点描述子词汇树第4层中各个叶子节点的聚类中心之间的汉明距离,获取第i+1个特征点描述子词汇树第4层中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点记作其中,m10和m′10均为正整数;
步骤20232、计算机获取第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子和第i+1个特征点描述子词汇树第5层中与叶子节点相连接的各个叶子节点的聚类中心之间的汉明距离,获取第i+1个特征点描述子词汇树第5层中与第i个特征点描述子词汇树的第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点并记作
步骤20233、多次重复步骤20231和步骤20232,计算机获取第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子和第i+1个特征点描述子词汇树第l′层中与叶子节点相连接的各个叶子节点的聚类中心之间的汉明距离,并获取第i+1个特征点描述子词汇树第l′层中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点并记作其中,l′为正整数,且4≤l′≤L;其中,表示第i+1个特征点描述子词汇树第l′-1层中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点;
步骤20234、多次重复步骤20233,直至计算机获取第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子和第i+1个特征点描述子词汇树第L层中与叶子节点相连接的各个叶子节点的聚类中心之间的汉明距离,获取第i+1个特征点描述子词汇树第L层中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点并记作其中,表示第i+1个特征点描述子词汇树第L-1层中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点;
步骤20235、计算机获取第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子和第i+1个特征点描述子词汇树第L层中叶子节点中各个特征点描述子之间的汉明距离,并获取第i+1个特征点描述子词汇树第L层中叶子节点中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的特征点描述子,并将该特征点描述子记作第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子的匹配特征点描述子;
步骤20236、按照步骤20231至步骤20235所述的方法,对第i个特征点描述子词汇树第10层中各个叶子节点中各个特征点描述子均进行特征匹配判断,得到第i个特征点描述子词汇树第10层中各个叶子节点中各个特征点描述子的匹配特征点描述子;
步骤20237、计算机获取第i个特征点描述子词汇树中各个特征点描述子的匹配特征点描述子,从而得到多组匹配点对;其中,一个特征点描述子和与其对应的匹配特征点描述子记作一组匹配点对。
上述的一种基于RGB-D图像的SLAM方法,其特征在于:步骤303中计算机对旋转矩阵Ri+1,i和平移向量ti+1,i进行优化,得到第i+1帧彩色图像和第i帧彩色图像之间优化后的旋转矩阵和优化后的平移向量具体过程如下:
步骤3031、计算机将拍摄第i帧彩色图像时相机坐标系记作第i个相机坐标系,将拍摄第i+1帧彩色图像时相机坐标系记作第i+1个相机坐标系;设定第f组匹配点对应第f个空间点在第i个相机坐标系下的三维坐标记作在第i+1个相机坐标系下的三维坐标记作
步骤3032、计算机根据公式得到第f个空间点在第i+1帧彩色图像和第i帧彩色图像之间的姿态误差项其中,||·||2表示向量的模的平方;R′i+1,i表示第i+1帧彩色图像和第i帧彩色图像之间的设定旋转矩阵,t′i+1,i第i+1帧彩色图像和第i帧彩色图像之间的设定平移向量;
步骤3034、R′i+1,i的初始值取Ri+1,i,t′i+1,i的初始值取ti+1,i,计算机采用最小二乘法对步骤3033中的姿态误差函数进行求解,得到姿态误差函数最小时的旋转矩阵R′i,i+1和平移向量t′i,i+1,并将姿态误差函数最小时的R′i,i+1作为第i+1帧彩色图像和第i帧彩色图像之间优化后的旋转矩阵姿态误差函数最小时的t′i+1,i作为第i+1帧彩色图像和第i帧彩色图像之间优化后的平移向量
上述的一种基于RGB-D图像的SLAM方法,其特征在于:步骤六中计算机对存在多组闭环帧的各个关键帧进行全局优化,得到优化后的位姿图,具体过程如下:
步骤601、计算机根据各个关键帧建立位姿图,并设定第b个关键帧拍摄时相机位姿记作第b个相机位姿,设定第b+1个关键帧拍摄时相机位姿记作第b+1个相机位姿;且第b个相机位姿记作Xb,第b+1个相机位姿记作Xb+1;其中,第b个关键帧和第b+1个关键帧不是闭环帧;
步骤603、计算机将步骤5011中多组关键帧中第r组闭环帧记作第r(1)个关键帧和第r(2)个关键帧,并设定位姿图中第r(1)个关键帧拍摄时相机位姿记作第r(1)个相机位姿Xr(1),第r(2)个关键帧拍摄时相机位姿记作第r(2)个相机位姿Xr(2);其中,r为正整数,且1≤r≤R,R表示闭环帧的总组数;
步骤605、计算机根据公式得到第b+1个相机位姿和第b个相机位姿之间的位姿差值矩阵eb,b+1,并将第b+1个相机位姿和第b个相机位姿之间的位姿差值矩阵eb,b+1经罗德里格斯公式转换,得到第b+1个相机位姿和第b个相机位姿之间的位姿差值向量e′b,b+1;其中e′b,b+1的大小为6×1;R″b+1,b和t″b+1,b分别表示第b+1个关键帧和第b个关键帧之间的设定的旋转矩阵和设定的平移向量;
步骤607、计算机根据公式得到第r(2)个相机位姿和第r(1)个相机位姿之间的位姿差值矩阵er(1),r(2);并将第r(2)个相机位姿和第r(1)个相机位姿之间的位姿差值矩阵er(1),r(2)经罗德里格斯公式转换,得到第r(2)个相机位姿和第r(1)个相机位姿之间的位姿差值向量e′r(1),r(2);其中e′r(1),r(2)的大小为6×1;R″r(1),r(2)和t″r(1),r(2)分别表示第r(1)个关键帧和第r(2)个关键帧之间的设定的旋转矩阵和设定的平移向量;
步骤6011、设定第b个相机位姿Xb的初始值、第b+1个相机位姿Xb+1的的初始值、第r(1)个相机位姿Xr(1)的初始值和第r(2)个相机位姿Xr(2)的初始值均为4×4的单位矩阵;设定R″b+1,b的初始值取t″b+1,b的初始值取R″r(1),r(2)的初始值取t″r(1),r(2)的初始值取
步骤6012、计算机利用g2o求解器选择高斯牛顿法对位姿图进行优化,得到总误差函数Cz最小的各个相机位姿、R″b+1,b、t″b+1,b、R″r(1),r(2)和t″r(1),r(2)解,形成优化后的位姿图。
上述的一种基于RGB-D图像的SLAM方法,其特征在于:步骤七中计算机根据优化后的位姿图和关键帧构建三角化网格地图,具体过程如下:
步骤701、计算机将B个关键帧转换分别转换为点云数据,并根据优化后的位姿图,将转换后的点云数据进行拼接,得到拼接后的点云数据;
步骤702、计算机采用三维体素栅格对拼接后的点云数据进行滤波,得到过滤后的点云数据;其中,三维体素栅格为立方体,且三维体素栅格的体积不大于27cm3;
步骤703、计算机采用移动最小二乘法对过滤后的点云数据进行重采样,得到重采样后的点云数据;
步骤704、计算机调取计算点云法线模块对重采样后的点云数据计算点云法线,构建有向点云;
步骤705、计算机采用贪心投影三角化算法对构建的有向点云进行三角化,构建三角化网格地图。
上述的一种基于RGB-D图像的SLAM方法,其特征在于:步骤405中运动距离最小阈值Dmin的取值为0.07~0.15,运动距离最大阈值Dmax的取值为0.3~0.5;
步骤5010中特征匹配的内点数阈值的取值范围为40~50;
步骤507中相似度阈值系数Sth的取值为0.7~0.85。
步骤403中设定的最小内点数Inmin取值范围为25~40;
步骤301中N的取值为70~100。
本发明与现有技术相比具有以下优点:
1、本发明方法步骤简单、设计合理且实现方便,提高了即时定位与地图构建的精度。
2、本发明基于RGB-D图像的SLAM方法使用效果好,首先是RGB-D图像的获取,其次是RGB-D图像的特征点提取与匹配,接着根据匹配点计算旋转矩阵和平移矩阵,并从RGB-D图像中获取关键帧,然后经关键帧的闭环判断,接着是对存在多组闭环帧的各个关键帧进行全局优化,得到优化后的位姿图,最后根据优化后的位姿图和关键帧构建三角化网格地图,实现了与待拍摄空间真实逼近的地图模型,提高了精度。
3、本发明RGB-D图像的特征点提取中采用ORB算法进行特征点提取,与SIFT算法和SURF算法相比,ORB算法在速度上拥有绝对的优势,适合在嵌入式设备上使用。
4、本发明RGB-D图像的特征点匹配中采用K-means++算法得到多组匹配点对,通过K-means++算法进行聚类得到特征点描述子词汇树,通过特征点描述子词汇树的匹配,极大地缩小图像特征点的匹配范围,加快匹配速度。
5、本发明采用随机抽样一致RANSAC算法和PnP算法结合,计算旋转矩阵和平移矩阵,并对旋转矩阵和平移向量进行局部优化,减少匹配点对质量差别造成位姿的误差,提高SLAM方法的精度。
6、本发明从RGB-D图像中获取关键帧,是为了后续使用关键帧进行即时定位与地图构建,可以避免因逐帧插入导致系统实时性降低、计算代价增加、系统内存消耗过大的问题。
7、本发明关键帧的闭环判断中首先采用相似度和相似度最大值与相似度阈值系数的乘积进行判断,实现一次判断,然后经过RGB-D图像的特征点提取与匹配进行二次判断,得到多组闭环帧,这样通过一次判断可以大大节省闭环检测所需的时间,一次判断和二次判断结合从而可以极大的提高闭环检测的精度。
8、本发明计算机对存在多组闭环帧的各个关键帧进行全局优化时,位姿图节点为相机位姿,位姿节点之间的边为两个关键帧之间的运动估计即旋转矩阵和平移向量,可以在保证精度的同时,极大的减少全局优化所需要的时间,提高SLAM方法的实时性,另外便于根据优化后的位姿图和关键帧构建三角化网格地图,确保地图分辨率和精度。
综上所述,本发明方法步骤简单、设计合理且实现方便,解决暴力匹配问题,且提高了SLAM的精度和实时性,从而能够满足机器人实时SLAM的要求,并且具有极高的精度。
下面通过附图和实施例,对本发明的技术方案做进一步的详细描述。
附图说明
图1为本发明的方法流程框图。
具体实施方式
如图1所示的一种基于RGB-D图像的SLAM方法,该方法包括以下步骤:
步骤一、RGB-D图像的获取:
采用深度相机按照预设设定的采集时间对待拍摄空间进行实时采集,并将各个采样时刻采集到的RGB-D图像发送至计算机;其中,第i个采样时刻采集到的RGB-D图像包括第i帧彩色图像和第i帧彩色图像所对应的深度值,i为正整数,且1≤i≤M,M表示彩色图像的总帧数;
步骤二、RGB-D图像的特征点提取与匹配:
步骤201、计算机采用ORB算法对第i帧彩色图像进行特征点提取,得到第i帧彩色图像的特征点描述子集;其中,第i帧彩色图像中第a个特征点对应的特征点描述子记作第i帧彩色图像中第a个特征点描述子a为正整数,且1≤a≤A,A表示特征点的总数;
同时,计算机采用ORB算法对第i+1帧彩色图像进行特征点提取,得到第i+1帧彩色图像的特征点描述子集;其中,第i+1帧彩色图像中第a′个特征点对应的特征点描述子记作第i+1帧彩色图像中第a′个特征点描述子a′为正整数,1≤a′≤A;
步骤202、计算机采用K-means++算法对第i帧彩色图像的特征点描述子和第i+1帧彩色图像中的特征点描述子进行匹配,得到多组匹配点对;
步骤三、计算旋转矩阵和平移矩阵:
步骤301、计算机将每组匹配点对之间的汉明距离按照从小到大的顺序进行排序,并选取前N组匹配点对;
步骤302、计算机采用随机抽样一致RANSAC算法和PnP算法,对N组匹配点对进行姿态估计,得到第i+1帧彩色图像和第i帧彩色图像之间的姿态、特征匹配的内点数Inliersi,i+1;其中,第i+1帧彩色图像和第i帧彩色图像之间的姿态包括第i+1帧彩色图像和第i帧彩色图像之间的旋转矩阵Ri+1,i和平移向量ti+1,i;其中,旋转矩阵Ri+1,i为3×3矩阵,平移向量ti+1,i为3×1向量;
步骤四、RGB-D图像中关键帧的获取:
步骤401、选取第1帧彩色图像作为关键帧;
步骤402、选取下一帧彩色图像即第2帧彩色图像作为当前帧彩色图像;
步骤403、按照步骤二和步骤三所述的方法,得到第2帧彩色图像和第1帧彩色图像即当前帧彩色图像和上一个关键帧之间的特征匹配的内点数Inliersc,s,计算机将Inliersc,s和设定的最小内点数Inmin进行判断,如果Inliersc,s>Inmin,执行步骤404和步骤405;否则,舍弃该当前帧彩色图像;
步骤404、根据公式Dc,s=||tc,s *||+min(2π-||Rc,s *||,||Rc,s *||),得到当前帧彩色图像和上一个关键帧之间的相对运动距离Dc,s;其中,Rc,s *表示第2帧彩色图像和第1帧彩色图像之间即当前帧彩色图像和上一个关键帧之间的优化后的旋转矩阵,tc,s *表示第2帧彩色图像和第1帧彩色图像之间即当前帧彩色图像和上一个关键帧之间的优化后的平移向量,||·||表示向量的模;
步骤405、计算机将Dc,s与运动距离最小阈值Dmin和运动距离最大阈值Dmax进行判断,如果Dmin<Dc,s<Dmax成立,则第2帧彩色图像即当前帧彩色图像为关键帧;否则,舍弃该当前帧彩色图像;
步骤406、按照步骤401和步骤405所述的方法,对上一个关键帧和下一帧彩色图像之间进行判断,直至完成M-2帧彩色图像的判断,得到各个关键帧;
步骤五、关键帧的闭环判断:
步骤501、计算机将步骤406中得到的各个关键帧按照采样时间先后顺序存入数据库图像中;其中,关键帧的总数为B;
步骤502、计算机根据公式Nz=B×A,得到数据库图像中特征点的总数Nz;
步骤503、计算机根据公式得到第b个关键帧中第t个特征点的词频-逆文档频率其中,Nz,t表示第b个关键帧中第t个特征点在数据库图像中出现的数量,Nb,t表示第b个关键帧中第t个特征点出现的数量,t为正整数,1≤t≤A,b为正整数,且1≤b≤B;
步骤504、计算机将剩余B-b个关键帧记作待判断关键帧,计算机根据公式得到第b′个待判断关键帧中第t′个特征点的词频-逆文档频率其中,Nz,t′表示第b′个待判断关键帧中第t′个特征点在数据库图像中出现的数量,Nb′,t′表示第b′个待判断关键帧中第t′个特征点出现的数量,b′和t′均为正整数,且b′的取值位于b+1~B,且b+1不大于B,1≤t′≤A;
步骤505、计算机根据,得到第b个关键帧和第b′个待判断关键帧之间的相似度S(qb,db′);其中,qb表示第b个关键帧中各个特征点的词频-逆文档频率向量,且db′表示第b′个待判断关键帧中各个特征点的词频-逆文档频率向量,且||·||表示向量的模;
步骤507、按照步骤506所述的方法,得到各个关键帧之间的相似度,获取相似度最大值并记作Smax,并设定相似度阈值系数为Sth;
步骤508、计算机将S(qb,db′)和Sth×Smax进行判断,如果S(qb,db′)大于Sth×Smax,则第b个关键帧和第b′个待判断关键帧作为候选闭环帧序列;
步骤509、按照步骤二和步骤三所述的方法,得到第b个关键帧和第b′个待判断关键帧之间优化后的旋转矩阵和优化后的平移向量以及第b个关键帧和第b′个待判断关键帧之间的特征匹配的内点数Inliersb,b′(key);
步骤5010、计算机将第b个关键帧和第b′个待判断关键帧之间的特征匹配的内点数Inliersb,b′(key)和特征匹配的内点数阈值进行判断,如果Inliersb,b′(key)大于内点数阈值,则第b个关键帧和第b′个待判断关键帧作为一组闭环帧,
步骤5011、按照步骤502至步骤5010所述的方法,对第b+1个关键帧和待判断关键帧进行判断,直至完成B-1个关键帧的判断,得到多组闭环帧;
步骤六、RGB-D图像的全局优化:
计算机对存在多组闭环帧的各个关键帧进行全局优化,得到优化后的位姿图;
步骤七、构建网格地图:
计算机根据优化后的位姿图和关键帧构建三角化网格地图。
本实施例中,步骤202中采用计算机对第i帧彩色图像和第i+1帧彩色图像中的特征点描述子进行匹配,得到多组匹配点对,具体过程如下:
步骤2021、计算机设定聚类种类k为5,设定聚类层数L为10,计算机采用K-means++算法对第i帧彩色图像的特征点描述子集进行聚类,得到第i帧彩色图像的特征点描述子词汇树,并记作第i个特征点描述子词汇树;
计算机采用K-means++算法对第i+1帧彩色图像的特征点描述子集进行聚类,得到第i+1帧彩色图像的特征点描述子词汇树,并记作第i+1个特征点描述子词汇树;
步骤2022、计算机获取第i个特征点描述子词汇树L层中各个叶子节点的聚类中心和第i+1个特征点描述子词汇树L层中各个叶子节点的聚类中心;
步骤2023、计算机将第i个特征点描述子词汇树第10层中各个叶子节点中的特征点描述子分别和第i+1个特征点描述子词汇树从第4层开始进行特征匹配判断的方法均相同,其中,计算机将第i个特征点描述子词汇树第10层中第m10个叶子节点中的特征点描述子分别和第i+1个特征点描述子词汇树第4层进行特征匹配判断,具体过程如下:
步骤20231、计算机获取第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子和第i+1个特征点描述子词汇树第4层中各个叶子节点的聚类中心之间的汉明距离,获取第i+1个特征点描述子词汇树第4层中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点记作其中,m10和m′10均为正整数;
步骤20232、计算机获取第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子和第i+1个特征点描述子词汇树第5层中与叶子节点相连接的各个叶子节点的聚类中心之间的汉明距离,获取第i+1个特征点描述子词汇树第5层中与第i个特征点描述子词汇树的第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点并记作
步骤20233、多次重复步骤20231和步骤20232,计算机获取第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子和第i+1个特征点描述子词汇树第l′层中与叶子节点相连接的各个叶子节点的聚类中心之间的汉明距离,并获取第i+1个特征点描述子词汇树第l′层中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点并记作其中,l′为正整数,且4≤l′≤L;其中,表示第i+1个特征点描述子词汇树第l′-1层中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点;
步骤20234、多次重复步骤20233,直至计算机获取第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子和第i+1个特征点描述子词汇树第L层中与叶子节点相连接的各个叶子节点的聚类中心之间的汉明距离,获取第i+1个特征点描述子词汇树第L层中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点并记作其中,表示第i+1个特征点描述子词汇树第L-1层中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点;
步骤20235、计算机获取第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子和第i+1个特征点描述子词汇树第L层中叶子节点中各个特征点描述子之间的汉明距离,并获取第i+1个特征点描述子词汇树第L层中叶子节点中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的特征点描述子,并将该特征点描述子记作第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子的匹配特征点描述子;
步骤20236、按照步骤20231至步骤20235所述的方法,对第i个特征点描述子词汇树第10层中各个叶子节点中各个特征点描述子均进行特征匹配判断,得到第i个特征点描述子词汇树第10层中各个叶子节点中各个特征点描述子的匹配特征点描述子;
步骤20237、计算机获取第i个特征点描述子词汇树中各个特征点描述子的匹配特征点描述子,从而得到多组匹配点对;其中,一个特征点描述子和与其对应的匹配特征点描述子记作一组匹配点对。
步骤3031、计算机将拍摄第i帧彩色图像时相机坐标系记作第i个相机坐标系,将拍摄第i+1帧彩色图像时相机坐标系记作第i+1个相机坐标系;设定第f组匹配点对应第f个空间点在第i个相机坐标系下的三维坐标记作在第i+1个相机坐标系下的三维坐标记作
步骤3032、计算机根据公式得到第f个空间点在第i+1帧彩色图像和第i帧彩色图像之间的姿态误差项其中,||·||2表示向量的模的平方;R′i+1,i表示第i+1帧彩色图像和第i帧彩色图像之间的设定旋转矩阵,t′i+1,i第i+1帧彩色图像和第i帧彩色图像之间的设定平移向量;
步骤3034、R′i+1,i的初始值取Ri+1,i,t′i+1,i的初始值取ti+1,i,计算机采用最小二乘法对步骤3033中的姿态误差函数进行求解,得到姿态误差函数最小时的旋转矩阵R′i,i+1和平移向量t′i,i+1,并将姿态误差函数最小时的R′i,i+1作为第i+1帧彩色图像和第i帧彩色图像之间优化后的旋转矩阵姿态误差函数最小时的t′i+1,i作为第i+1帧彩色图像和第i帧彩色图像之间优化后的平移向量
本实施例中,步骤六中计算机对存在多组闭环帧的各个关键帧进行全局优化,得到优化后的位姿图,具体过程如下:
步骤601、计算机根据各个关键帧建立位姿图,并设定第b个关键帧拍摄时相机位姿记作第b个相机位姿,设定第b+1个关键帧拍摄时相机位姿记作第b+1个相机位姿;且第b个相机位姿记作Xb,第b+1个相机位姿记作Xb+1;其中,第b个关键帧和第b+1个关键帧不是闭环帧;
步骤603、计算机将步骤5011中多组关键帧中第r组闭环帧记作第r(1)个关键帧和第r(2)个关键帧,并设定位姿图中第r(1)个关键帧拍摄时相机位姿记作第r(1)个相机位姿Xr(1),第r(2)个关键帧拍摄时相机位姿记作第r(2)个相机位姿Xr(2);其中,r为正整数,且1≤r≤R,R表示闭环帧的总组数;
步骤605、计算机根据公式得到第b+1个相机位姿和第b个相机位姿之间的位姿差值矩阵eb,b+1,并将第b+1个相机位姿和第b个相机位姿之间的位姿差值矩阵eb,b+1经罗德里格斯公式转换,得到第b+1个相机位姿和第b个相机位姿之间的位姿差值向量e′b,b+1;其中e′b,b+1的大小为6×1;R″b+1,b和t″b+1,b分别表示第b+1个关键帧和第b个关键帧之间的设定的旋转矩阵和设定的平移向量;
步骤607、计算机根据公式得到第r(2)个相机位姿和第r(1)个相机位姿之间的位姿差值矩阵er(1),r(2);并将第r(2)个相机位姿和第r(1)个相机位姿之间的位姿差值矩阵er(1),r(2)经罗德里格斯公式转换,得到第r(2)个相机位姿和第r(1)个相机位姿之间的位姿差值向量e′r(1),r(2);其中e′r(1),r(2)的大小为6×1;R″r(1),r(2)和t″r(1),r(2)分别表示第r(1)个关键帧和第r(2)个关键帧之间的设定的旋转矩阵和设定的平移向量;
步骤6011、设定第b个相机位姿Xb的初始值、第b+1个相机位姿Xb+1的的初始值、第r(1)个相机位姿Xr(1)的初始值和第r(2)个相机位姿Xr(2)的初始值均为4×4的单位矩阵;设定R″b+1,b的初始值取t″b+1,b的初始值取R″r(1),r(2)的初始值取t″r(1),r(2)的初始值取
步骤6012、计算机利用g2o求解器选择高斯牛顿法对位姿图进行优化,得到总误差函数Cz最小的各个相机位姿、R″b+1,b、t″b+1,b、R″r(1),r(2)和t″r(1),r(2)解,形成优化后的位姿图。
本实施例中,步骤七中计算机根据优化后的位姿图和关键帧构建三角化网格地图,具体过程如下:
步骤701、计算机将B个关键帧转换分别转换为点云数据,并根据优化后的位姿图,将转换后的点云数据进行拼接,得到拼接后的点云数据;
步骤702、计算机采用三维体素栅格对拼接后的点云数据进行滤波,得到过滤后的点云数据;其中,三维体素栅格为立方体,且三维体素栅格的体积不大于27cm3;
步骤703、计算机采用移动最小二乘法对过滤后的点云数据进行重采样,得到重采样后的点云数据;
步骤704、计算机调取计算点云法线模块对重采样后的点云数据计算点云法线,构建有向点云;
步骤705、计算机采用贪心投影三角化算法对构建的有向点云进行三角化,构建三角化网格地图。
本实施例中,步骤405中运动距离最小阈值Dmin的取值为0.07~0.15,运动距离最大阈值Dmax的取值为0.3~0.5;
步骤5010中特征匹配的内点数阈值的取值范围为40~50;
步骤507中相似度阈值系数Sth的取值为0.7~0.85。
步骤403中设定的最小内点数Inmin取值范围为25~40;
步骤301中N的取值为70~100。
本实施例中,J表示特征点描述子元素的总数,j和J均为正整数,且J=500,1≤j≤J,表示第i帧彩色图像中第a个特征点描述子的第一个元素,表示第i帧彩色图像中第a个特征点描述子的第j个元素,表示第i帧彩色图像中第a个特征点描述子的第J个元素。
本实施例中,预设设定的采集时间的取值范围为0.01~0.1。
本实施例中,进一步地优选,预设设定的采集时间的取值范围为0.04。
本实施例中,RGB-D图像即为深度图像。
本实施例中,特征点的总数A=500。
本实施例中,需要说明的是,SLAM为(Simultaneous Localization and Mapping)即时定位与地图构建。
本实施例中,进一步地优选,步骤507中相似度阈值系数Sth的取值为0.8。
本实施例中,需要说明的是,ORB算法即Oriented FAST and Rotated BRIEF算法,是一种快速特征点提取和描述的算法。
本实施例中,需要说明的是,实际使用时,Nb,t和Nb′,t的值均为1。
本实施例中,需要说明的是,实际使用时,步骤302中还利用第i帧彩色图像所对应的深度值和第i+1帧彩色图像所对应的深度值。
本实施例中,需要说明的是,实际使用时,步骤701中计算机将B个关键帧转换分别转换为点云数据时,还需要利用B个关键帧所对应的深度值。
本实施例中,需要说明的是,实际使用时,当步骤202中多组匹配点对的组数小于N时,则步骤302中对步骤202中得到的多组匹配点对进行姿态估计。
综上所述,本发明方法步骤简单、设计合理且实现方便,解决暴力匹配问题,且减少匹配点对质量差别造成深度相机位姿的误差,提高了SLAM的精度和实时性,从而能够满足机器人实时SLAM的要求,并且具有极高的精度。
以上所述,仅是本发明的较佳实施例,并非对本发明作任何限制,凡是根据本发明技术实质对以上实施例所作的任何简单修改、变更以及等效结构变化,均仍属于本发明技术方案的保护范围内。
Claims (6)
1.一种基于RGB-D图像的SLAM方法,其特征在于,该方法包括以下步骤:
步骤一、RGB-D图像的获取:
采用深度相机按照预设设定的采集时间对待拍摄空间进行实时采集,并将各个采样时刻采集到的RGB-D图像发送至计算机;其中,第i个采样时刻采集到的RGB-D图像包括第i帧彩色图像和第i帧彩色图像所对应的深度值,i为正整数,且1≤i≤M,M表示彩色图像的总帧数;
步骤二、RGB-D图像的特征点提取与匹配:
步骤201、计算机采用ORB算法对第i帧彩色图像进行特征点提取,得到第i帧彩色图像的特征点描述子集;其中,第i帧彩色图像中第a个特征点对应的特征点描述子记作第i帧彩色图像中第a个特征点描述子a为正整数,且1≤a≤A,A表示特征点的总数;
同时,计算机采用ORB算法对第i+1帧彩色图像进行特征点提取,得到第i+1帧彩色图像的特征点描述子集;其中,第i+1帧彩色图像中第a′个特征点对应的特征点描述子记作第i+1帧彩色图像中第a′个特征点描述子a′为正整数,1≤a′≤A;
步骤202、计算机采用K-means++算法对第i帧彩色图像的特征点描述子和第i+1帧彩色图像中的特征点描述子进行匹配,得到多组匹配点对;
步骤三、计算旋转矩阵和平移矩阵:
步骤301、计算机将每组匹配点对之间的汉明距离按照从小到大的顺序进行排序,并选取前N组匹配点对;
步骤302、计算机采用随机抽样一致RANSAC算法和PnP算法,对N组匹配点对进行姿态估计,得到第i+1帧彩色图像和第i帧彩色图像之间的姿态、特征匹配的内点数Inliersi,i+1;其中,第i+1帧彩色图像和第i帧彩色图像之间的姿态包括第i+1帧彩色图像和第i帧彩色图像之间的旋转矩阵Ri+1,i和平移向量ti+1,i;其中,旋转矩阵Ri+1,i为3×3矩阵,平移向量ti+1,i为3×1向量;
步骤四、RGB-D图像中关键帧的获取:
步骤401、选取第1帧彩色图像作为关键帧;
步骤402、选取下一帧彩色图像即第2帧彩色图像作为当前帧彩色图像;
步骤403、按照步骤二和步骤三所述的方法,得到第2帧彩色图像和第1帧彩色图像即当前帧彩色图像和上一个关键帧之间的特征匹配的内点数Inliersc,s,计算机将Inliersc,s和设定的最小内点数Inmin进行判断,如果Inliersc,s>Inmin,执行步骤404和步骤405;否则,舍弃该当前帧彩色图像;
步骤404、根据公式Dc,s=||tc,s *||+min(2π-||Rc,s *||,||Rc,s *||),得到当前帧彩色图像和上一个关键帧之间的相对运动距离Dc,s;其中,Rc,s *表示第2帧彩色图像和第1帧彩色图像之间即当前帧彩色图像和上一个关键帧之间的优化后的旋转矩阵,tc,s *表示第2帧彩色图像和第1帧彩色图像之间即当前帧彩色图像和上一个关键帧之间的优化后的平移向量,||·||表示向量的模;
步骤405、计算机将Dc,s与运动距离最小阈值Dmin和运动距离最大阈值Dmax进行判断,如果Dmin<Dc,s<Dmax成立,则第2帧彩色图像即当前帧彩色图像为关键帧;否则,舍弃该当前帧彩色图像;
步骤406、按照步骤401和步骤405所述的方法,对上一个关键帧和下一帧彩色图像之间进行判断,直至完成M-2帧彩色图像的判断,得到各个关键帧;
步骤五、关键帧的闭环判断:
步骤501、计算机将步骤406中得到的各个关键帧按照采样时间先后顺序存入数据库图像中;其中,关键帧的总数为B;
步骤502、计算机根据公式Nz=B×A,得到数据库图像中特征点的总数Nz;
步骤503、计算机根据公式得到第b个关键帧中第t个特征点的词频-逆文档频率其中,Nz,t表示第b个关键帧中第t个特征点在数据库图像中出现的数量,Nb,t表示第b个关键帧中第t个特征点出现的数量,t为正整数,1≤t≤A,b为正整数,且1≤b≤B;
步骤504、计算机将剩余B-b个关键帧记作待判断关键帧,计算机根据公式得到第b′个待判断关键帧中第t′个特征点的词频-逆文档频率其中,Nz,t′表示第b′个待判断关键帧中第t′个特征点在数据库图像中出现的数量,Nb′,t′表示第b′个待判断关键帧中第t′个特征点出现的数量,b′和t′均为正整数,且b′的取值位于b+1~B,且b+1不大于B,1≤t′≤A;
步骤505、计算机根据 得到第b个关键帧和第b′个待判断关键帧之间的相似度S(qb,db′);其中,qb表示第b个关键帧中各个特征点的词频-逆文档频率向量,且db′表示第b′个待判断关键帧中各个特征点的词频-逆文档频率向量,且||·||表示向量的模;
步骤507、按照步骤506所述的方法,得到各个关键帧之间的相似度,获取相似度最大值并记作Smax,并设定相似度阈值系数为Sth;
步骤508、计算机将S(qb,db′)和Sth×Smax进行判断,如果S(qb,db′)大于Sth×Smax,则第b个关键帧和第b′个待判断关键帧作为候选闭环帧序列;
步骤509、按照步骤二和步骤三所述的方法,得到第b个关键帧和第b′个待判断关键帧之间优化后的旋转矩阵和优化后的平移向量以及第b个关键帧和第b′个待判断关键帧之间的特征匹配的内点数Inliersb,b′(key);
步骤5010、计算机将第b个关键帧和第b′个待判断关键帧之间的特征匹配的内点数Inliersb,b′(key)和特征匹配的内点数阈值进行判断,如果Inliersb,b′(key)大于内点数阈值,则第b个关键帧和第b′个待判断关键帧作为一组闭环帧,
步骤5011、按照步骤502至步骤5010所述的方法,对第b+1个关键帧和待判断关键帧进行判断,直至完成B-1个关键帧的判断,得到多组闭环帧;
步骤六、RGB-D图像的全局优化:
计算机对存在多组闭环帧的各个关键帧进行全局优化,得到优化后的位姿图;
步骤七、构建网格地图:
计算机根据优化后的位姿图和关键帧构建三角化网格地图。
2.按照权利要求1所述的一种基于RGB-D图像的SLAM方法,其特征在于:步骤202中采用计算机对第i帧彩色图像和第i+1帧彩色图像中的特征点描述子进行匹配,得到多组匹配点对,具体过程如下:
步骤2021、计算机设定聚类种类k为5,设定聚类层数L为10,计算机采用K-means++算法对第i帧彩色图像的特征点描述子集进行聚类,得到第i帧彩色图像的特征点描述子词汇树,并记作第i个特征点描述子词汇树;
计算机采用K-means++算法对第i+1帧彩色图像的特征点描述子集进行聚类,得到第i+1帧彩色图像的特征点描述子词汇树,并记作第i+1个特征点描述子词汇树;
步骤2022、计算机获取第i个特征点描述子词汇树L层中各个叶子节点的聚类中心和第i+1个特征点描述子词汇树L层中各个叶子节点的聚类中心;
步骤2023、计算机将第i个特征点描述子词汇树第10层中各个叶子节点中的特征点描述子分别和第i+1个特征点描述子词汇树从第4层开始进行特征匹配判断的方法均相同,其中,计算机将第i个特征点描述子词汇树第10层中第m10个叶子节点中的特征点描述子分别和第i+1个特征点描述子词汇树第4层进行特征匹配判断,具体过程如下:
步骤20231、计算机获取第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子和第i+1个特征点描述子词汇树第4层中各个叶子节点的聚类中心之间的汉明距离,获取第i+1个特征点描述子词汇树第4层中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点记作其中,m10和m′10均为正整数;
步骤20232、计算机获取第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子和第i+1个特征点描述子词汇树第5层中与叶子节点相连接的各个叶子节点的聚类中心之间的汉明距离,获取第i+1个特征点描述子词汇树第5层中与第i个特征点描述子词汇树的第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点并记作
步骤20233、多次重复步骤20231和步骤20232,计算机获取第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子和第i+1个特征点描述子词汇树第l′层中与叶子节点相连接的各个叶子节点的聚类中心之间的汉明距离,并获取第i+1个特征点描述子词汇树第l′层中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点并记作其中,l′为正整数,且4≤l′≤L;其中,表示第i+1个特征点描述子词汇树第l′-1层中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点;
步骤20234、多次重复步骤20233,直至计算机获取第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子和第i+1个特征点描述子词汇树第L层中与叶子节点相连接的各个叶子节点的聚类中心之间的汉明距离,获取第i+1个特征点描述子词汇树第L层中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点并记作其中,表示第i+1个特征点描述子词汇树第L-1层中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的叶子节点;
步骤20235、计算机获取第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子和第i+1个特征点描述子词汇树第L层中叶子节点中各个特征点描述子之间的汉明距离,并获取第i+1个特征点描述子词汇树第L层中叶子节点中与第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子汉明距离最小的特征点描述子,并将该特征点描述子记作第i个特征点描述子词汇树第10层中第m10个叶子节点中第m′10个特征点描述子的匹配特征点描述子;
步骤20236、按照步骤20231至步骤20235所述的方法,对第i个特征点描述子词汇树第10层中各个叶子节点中各个特征点描述子均进行特征匹配判断,得到第i个特征点描述子词汇树第10层中各个叶子节点中各个特征点描述子的匹配特征点描述子;
步骤20237、计算机获取第i个特征点描述子词汇树中各个特征点描述子的匹配特征点描述子,从而得到多组匹配点对;其中,一个特征点描述子和与其对应的匹配特征点描述子记作一组匹配点对。
3.按照权利要求1所述的一种基于RGB-D图像的SLAM方法,其特征在于:步骤303中计算机对旋转矩阵Ri+1,i和平移向量ti+1,i进行优化,得到第i+1帧彩色图像和第i帧彩色图像之间优化后的旋转矩阵和优化后的平移向量具体过程如下:
步骤3031、计算机将拍摄第i帧彩色图像时相机坐标系记作第i个相机坐标系,将拍摄第i+1帧彩色图像时相机坐标系记作第i+1个相机坐标系;设定第f组匹配点对应第f个空间点在第i个相机坐标系下的三维坐标记作在第i+1个相机坐标系下的三维坐标记作
步骤3032、计算机根据公式得到第f个空间点在第i+1帧彩色图像和第i帧彩色图像之间的姿态误差项其中,||·||2表示向量的模的平方;R′i+1,i表示第i+1帧彩色图像和第i帧彩色图像之间的设定旋转矩阵,t′i+1,i第i+1帧彩色图像和第i帧彩色图像之间的设定平移向量;
4.按照权利要求1所述的一种基于RGB-D图像的SLAM方法,其特征在于:步骤六中计算机对存在多组闭环帧的各个关键帧进行全局优化,得到优化后的位姿图,具体过程如下:
步骤601、计算机根据各个关键帧建立位姿图,并设定第b个关键帧拍摄时相机位姿记作第b个相机位姿,设定第b+1个关键帧拍摄时相机位姿记作第b+1个相机位姿;且第b个相机位姿记作Xb,第b+1个相机位姿记作Xb+1;其中,第b个关键帧和第b+1个关键帧不是闭环帧;
步骤603、计算机将步骤5011中多组关键帧中第r组闭环帧记作第r(1)个关键帧和第r(2)个关键帧,并设定位姿图中第r(1)个关键帧拍摄时相机位姿记作第r(1)个相机位姿Xr(1),第r(2)个关键帧拍摄时相机位姿记作第r(2)个相机位姿Xr(2);其中,r为正整数,且1≤r≤R,R表示闭环帧的总组数;
步骤605、计算机根据公式得到第b+1个相机位姿和第b个相机位姿之间的位姿差值矩阵eb,b+1,并将第b+1个相机位姿和第b个相机位姿之间的位姿差值矩阵eb,b+1经罗德里格斯公式转换,得到第b+1个相机位姿和第b个相机位姿之间的位姿差值向量e′b,b+1;其中e′b,b+1的大小为6×1;R″b+1,b和t″b+1,b分别表示第b+1个关键帧和第b个关键帧之间的设定的旋转矩阵和设定的平移向量;
步骤607、计算机根据公式得到第r(2)个相机位姿和第r(1)个相机位姿之间的位姿差值矩阵er(1),r(2);并将第r(2)个相机位姿和第r(1)个相机位姿之间的位姿差值矩阵er(1),r(2)经罗德里格斯公式转换,得到第r(2)个相机位姿和第r(1)个相机位姿之间的位姿差值向量e′r(1),r(2);其中e′r(1),r(2)的大小为6×1;R″r(1),r(2)和t″r(1),r(2)分别表示第r(1)个关键帧和第r(2)个关键帧之间的设定的旋转矩阵和设定的平移向量;
步骤6011、设定第b个相机位姿Xb的初始值、第b+1个相机位姿Xb+1的的初始值、第r(1)个相机位姿Xr(1)的初始值和第r(2)个相机位姿Xr(2)的初始值均为4×4的单位矩阵;设定R″b+1,b的初始值取t″b+1,b的初始值取R″r(1),r(2)的初始值取t″r(1),r(2)的初始值取
步骤6012、计算机利用g2o求解器选择高斯牛顿法对位姿图进行优化,得到总误差函数Cz最小的各个相机位姿、R″b+1,b、t″b+1,b、R″r(1),r(2)和t″r(1),r(2)解,形成优化后的位姿图。
5.按照权利要求1所述的一种基于RGB-D图像的SLAM方法,其特征在于:步骤七中计算机根据优化后的位姿图和关键帧构建三角化网格地图,具体过程如下:
步骤701、计算机将B个关键帧转换分别转换为点云数据,并根据优化后的位姿图,将转换后的点云数据进行拼接,得到拼接后的点云数据;
步骤702、计算机采用三维体素栅格对拼接后的点云数据进行滤波,得到过滤后的点云数据;其中,三维体素栅格为立方体,且三维体素栅格的体积不大于27cm3;
步骤703、计算机采用移动最小二乘法对过滤后的点云数据进行重采样,得到重采样后的点云数据;
步骤704、计算机调取计算点云法线模块对重采样后的点云数据计算点云法线,构建有向点云;
步骤705、计算机采用贪心投影三角化算法对构建的有向点云进行三角化,构建三角化网格地图。
6.按照权利要求1所述的一种基于RGB-D图像的SLAM方法,其特征在于:步骤405中运动距离最小阈值Dmin的取值为0.07~0.15,运动距离最大阈值Dmax的取值为0.3~0.5;
步骤5010中特征匹配的内点数阈值的取值范围为40~50;
步骤507中相似度阈值系数Sth的取值为0.7~0.85。
步骤403中设定的最小内点数Inmin取值范围为25~40;
步骤301中N的取值为70~100。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011448199.6A CN112560648B (zh) | 2020-12-09 | 2020-12-09 | 一种基于rgb-d图像的slam方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011448199.6A CN112560648B (zh) | 2020-12-09 | 2020-12-09 | 一种基于rgb-d图像的slam方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112560648A true CN112560648A (zh) | 2021-03-26 |
CN112560648B CN112560648B (zh) | 2023-04-07 |
Family
ID=75061022
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011448199.6A Active CN112560648B (zh) | 2020-12-09 | 2020-12-09 | 一种基于rgb-d图像的slam方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112560648B (zh) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113094457A (zh) * | 2021-04-15 | 2021-07-09 | 成都纵横自动化技术股份有限公司 | 一种数字正射影像地图的增量式生成方法及相关组件 |
CN113768419A (zh) * | 2021-09-17 | 2021-12-10 | 安克创新科技股份有限公司 | 确定扫地机清扫方向的方法、装置及扫地机 |
CN113784026A (zh) * | 2021-08-30 | 2021-12-10 | 鹏城实验室 | 基于图像的计算位置信息的方法、装置、设备及存储介质 |
CN115115780A (zh) * | 2022-06-29 | 2022-09-27 | 聚好看科技股份有限公司 | 基于多视角rgbd相机的三维重建方法及系统 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107680133A (zh) * | 2017-09-15 | 2018-02-09 | 重庆邮电大学 | 一种基于改进闭环检测算法的移动机器人视觉slam方法 |
WO2018048353A1 (en) * | 2016-09-09 | 2018-03-15 | Nanyang Technological University | Simultaneous localization and mapping methods and apparatus |
CN109816686A (zh) * | 2019-01-15 | 2019-05-28 | 山东大学 | 基于物体实例匹配的机器人语义slam方法、处理器及机器人 |
WO2019169540A1 (zh) * | 2018-03-06 | 2019-09-12 | 斯坦德机器人(深圳)有限公司 | 紧耦合视觉slam的方法、终端及计算机可读存储介质 |
CN110501017A (zh) * | 2019-08-12 | 2019-11-26 | 华南理工大学 | 一种基于orb_slam2的移动机器人导航地图生成方法 |
CN111060115A (zh) * | 2019-11-29 | 2020-04-24 | 中国科学院计算技术研究所 | 一种基于图像边缘特征的视觉slam方法及系统 |
CN111179426A (zh) * | 2019-12-23 | 2020-05-19 | 南京理工大学 | 基于深度学习的机器人室内环境三维语义地图构建方法 |
US20200240793A1 (en) * | 2019-01-28 | 2020-07-30 | Qfeeltech (Beijing) Co., Ltd. | Methods, apparatus, and systems for localization and mapping |
-
2020
- 2020-12-09 CN CN202011448199.6A patent/CN112560648B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2018048353A1 (en) * | 2016-09-09 | 2018-03-15 | Nanyang Technological University | Simultaneous localization and mapping methods and apparatus |
CN107680133A (zh) * | 2017-09-15 | 2018-02-09 | 重庆邮电大学 | 一种基于改进闭环检测算法的移动机器人视觉slam方法 |
WO2019169540A1 (zh) * | 2018-03-06 | 2019-09-12 | 斯坦德机器人(深圳)有限公司 | 紧耦合视觉slam的方法、终端及计算机可读存储介质 |
CN109816686A (zh) * | 2019-01-15 | 2019-05-28 | 山东大学 | 基于物体实例匹配的机器人语义slam方法、处理器及机器人 |
US20200240793A1 (en) * | 2019-01-28 | 2020-07-30 | Qfeeltech (Beijing) Co., Ltd. | Methods, apparatus, and systems for localization and mapping |
CN110501017A (zh) * | 2019-08-12 | 2019-11-26 | 华南理工大学 | 一种基于orb_slam2的移动机器人导航地图生成方法 |
CN111060115A (zh) * | 2019-11-29 | 2020-04-24 | 中国科学院计算技术研究所 | 一种基于图像边缘特征的视觉slam方法及系统 |
CN111179426A (zh) * | 2019-12-23 | 2020-05-19 | 南京理工大学 | 基于深度学习的机器人室内环境三维语义地图构建方法 |
Non-Patent Citations (4)
Title |
---|
FELIX ENDRES ET AL: "An evaluation of the RGB-D SLAM system", 《2012 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION》 * |
伍锡如等: "改进ORB特征的机器人RGB-D SLAM算法", 《计算机工程与应用》 * |
张震: "基于RGB-D相机的室内同步定位与建图关键技术研究", 《中国优秀硕士学位论文全文数据库》 * |
胡凌燕等: "基于RGB-D图像的三维同步定位与建图研究", 《系统仿真学报》 * |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113094457A (zh) * | 2021-04-15 | 2021-07-09 | 成都纵横自动化技术股份有限公司 | 一种数字正射影像地图的增量式生成方法及相关组件 |
CN113094457B (zh) * | 2021-04-15 | 2023-11-03 | 成都纵横自动化技术股份有限公司 | 一种数字正射影像地图的增量式生成方法及相关组件 |
CN113784026A (zh) * | 2021-08-30 | 2021-12-10 | 鹏城实验室 | 基于图像的计算位置信息的方法、装置、设备及存储介质 |
CN113768419A (zh) * | 2021-09-17 | 2021-12-10 | 安克创新科技股份有限公司 | 确定扫地机清扫方向的方法、装置及扫地机 |
CN115115780A (zh) * | 2022-06-29 | 2022-09-27 | 聚好看科技股份有限公司 | 基于多视角rgbd相机的三维重建方法及系统 |
Also Published As
Publication number | Publication date |
---|---|
CN112560648B (zh) | 2023-04-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112560648B (zh) | 一种基于rgb-d图像的slam方法 | |
TWI742690B (zh) | 人體檢測方法、裝置、電腦設備及儲存媒體 | |
CN108242079B (zh) | 一种基于多特征视觉里程计和图优化模型的vslam方法 | |
CN107122705B (zh) | 基于三维人脸模型的人脸关键点检测方法 | |
CN106679648B (zh) | 一种基于遗传算法的视觉惯性组合的slam方法 | |
CN110503680B (zh) | 一种基于非监督的卷积神经网络单目场景深度估计方法 | |
CN111414968B (zh) | 一种基于卷积神经网络特征图的多模态遥感影像匹配方法 | |
WO2020259481A1 (zh) | 定位方法及装置、电子设备、可读存储介质 | |
CN108038420B (zh) | 一种基于深度视频的人体行为识别方法 | |
CN107953329B (zh) | 物体识别和姿态估计方法、装置及机械臂抓取系统 | |
CN111046734B (zh) | 基于膨胀卷积的多模态融合视线估计方法 | |
CN108280858B (zh) | 多视图重建中的一种线性全局相机运动参数估计方法 | |
CN111368759B (zh) | 基于单目视觉的移动机器人语义地图构建系统 | |
CN112001278A (zh) | 一种基于结构化知识蒸馏的人群计数模型及其方法 | |
CN111709980A (zh) | 基于深度学习的多尺度图像配准方法和装置 | |
CN113283525A (zh) | 一种基于深度学习的图像匹配方法 | |
Liu et al. | Rotation-invariant siamese network for low-altitude remote-sensing image registration | |
CN111598995B (zh) | 一种基于原型分析的自监督多目三维人体姿态估计方法 | |
CN111881716A (zh) | 一种基于多视角生成对抗网络的行人重识别方法 | |
CN115331259A (zh) | 一种三维人体姿态估计方法、系统及存储介质 | |
CN110069655B (zh) | 一种私人相册的人脸搜索方法 | |
CN114663880A (zh) | 基于多层级跨模态自注意力机制的三维目标检测方法 | |
CN113255569B (zh) | 一种基于图空洞卷积编码器解码器的3d姿态估计方法 | |
CN114882537A (zh) | 一种基于神经辐射场的手指新视角图像生成方法 | |
CN108921106B (zh) | 一种基于capsule的人脸识别方法 |
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 |