一种基于直线引导特征提取的视觉slam建图方法
技术领域
1.本发明涉及地图构建技术领域，特别涉及一种视觉slam建图方法。
背景技术：2.slam(simultaneous localization and mapping)即时定位与地图构建，是自主移动机器人、无人机、汽车自动驾驶的关键技术。在基于视觉进行地图构建中，准确提取出图像中的特征点进行位姿估计，是准确判断关键帧和降低建图误差的重要保障。
3.针对室内环境进行slam，视觉相机采集到的图像中含有大量的直线特征。现有技术中通常会用到edlines等直线特征提取算法获得图像中的直线特征。但是现有的edlines算法受噪声的影响，在非直线特征处会提取出较短的直线，因此其抗噪声干扰能力还有待提高。并且采用现有edlines直线特征提取算法提取得到的线段特征，中间断点较多，与实际情况的吻合度还有待提升。
4.并且现有方法在提取图像中的特征点时，特征点多数位于线段的端点、线段间交点、纹理丰富的区域，而在线段中部、低纹理区域提取出特征点较少，特征点的分布不够合理，这在slam过程中不利于准确判断关键帧图像和提高位姿估计准确度。
5.而且现有技术在构建八叉树地图时，由于在根据相邻帧图像进行位姿估计时存在累计误差，建立的地图容易出现参考平面不平、角度与实际不一致的问题，导致建图准确度降低；且在将三维点云转换为八叉树地图时，由于三维点云数据量大，转换过程耗时长，很难保证算法的实时性。
技术实现要素：6.有鉴于此，本发明的目的是提供一种基于直线引导特征提取的视觉slam建图方法，以解决现有技术中在提取相机实时采集的图像中的线段特征与实际情况吻合度有待提升，提取到的特征点分布不够合理，进而会影响所构建地图的准确度的技术问题；以及解决现有技术中根据相邻帧图像进行位姿估计产生的累计误差会导入所建立的地图出现参考平面不平、角度与实际不一致的问题，在将三维点云转换为八叉树地图时转换过程耗时长，很难保证算法的实时性等技术问题。
7.本发明基于直线引导特征提取的视觉slam建图方法，包括步骤：
8.1)通过相机实时拍摄环境图像；
9.2)对图像进行高斯滤波，以平滑图像；
10.3)提取图像中的线段特征，其又包括以下步骤：
11.3.1)计算图像中每个像素的梯度方向和梯度大小；
12.3.2)遍历每个像素，选取在梯度方向上梯度值大于或等于相邻像素梯度值的像素，定义为锚点，
13.3.3)连接锚点，形成边缘像素链；
14.3.4)对每条边缘像素链采用prosac方法剔除噪声点，拟合线段，其又包括以下步
骤：
15.a)从边缘像素链中任选2个像素作为线段的起点和终点，并根据所选的2个像素计算出一条直线；
16.b)求取边缘像素链中的其它像素到步骤a)计算所得直线的加权距离，以加权距离在dh范围内的像素作为内点构成内点集；其中dh为给定阈值；
17.di＝αd
i1
+βd
i2
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0018][0019]di2
＝cos(θ
1-θ2)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0020]
其中，di为像素距直线的加权距离，d
i1
为像素(xi,yi)到直线y＝kx+b的欧式距离，k为直线斜率，b为直线截距，d
i2
为像素(xi,yi)梯度方向θ1与直线y＝kx+b的法线方向θ2的单位向量距离，α和β为权值因子；
[0021]
c)以最大化内点数量为指标进行迭代过程，每次迭代过程中用于确定新直线的2个像素从上一次迭代得到的内点集里面选取，直至内点占像素链中所有像素的比例超过比例阈值τ或达到迭代次数num；
[0022]
d)以最后一次迭代得到的线段作为最终结果，并输出提取到的线段方程；
[0023]
3.5)对步骤3.4)得到的线段进行筛选和合并，其包括以下步骤：
[0024]
a)任意选择图片中的一条线段作为主线段，再拓展一个长为l+δl,宽为δw的矩形，主线段位于矩形的中心位置，矩形的长边与主线段平行，其中l为主线段的长度，δl和δw为设定的阈值，以向量相似度筛选矩形内及与矩形相交的线段，将满足向量相似度筛选公式的线段放入集合中得到线段集合；向量相似度筛选公式为：
[0025][0026][0027][0028]
cosθ≥cosθ
min
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
[0029]
其中为主线段l1的向量表示，为待合并线段l2的向量表示；主线段l1的两个端点坐标分别为(x
1a
,y
1a
)、(x
1b
,y
1b
)，待合并线段l2的两个端点坐标分别为(x
2a
,y
2a
)、(x
2b
,y
2b
)，θ为主线段l1与待合并线段l2的夹角，θ
min
为设定的夹角阈值；
[0030]
b)对线段集合中的线段按照与主线段的距离升序排序，距离度量公式为：
[0031][0032][0033][0034]
其中，为待合并线段的中点p的坐标，d
p
为中点p到主线段y＝k1x+b1的欧式距
离，k1为直线斜率，b1为直线截距；
[0035]
c)依次选择步骤b)中升序排序中的线段作为待合并线段，将待合并线段正投影到包含该主线段的直线上，得到待合并线段的投影线段，再分情况合并线段：
[0036]
情况一：当投影线段的两个端点主线段之外，且投影线段的两个端点在主线段的同一侧，则计算主线段和投影线段的两相邻端点的距离，如果两个相邻端点的距离小于设定的阈值则在主线段的两个端点和投影线段的两个端点共四个端点中，取距离最大的两个端点所确定的线段作为主线段和待合并线段合并后的新线段；如果两个相邻端点的距离大于设定的阈值则不进行线段合并；
[0037]
情况二：当投影线段的两个端点在主线段的两端外侧，则取投影线段作为主线段和待合并线段合并后的新线段；
[0038]
情况三：当投影线段的一个端点在主线段上，投影线段的另一端点主线段外，则在主线段的两个端点和投影线段的两个端点共四个端点中，取距离最大的两个端点所确定的线段作为主线段和待合并线段合并后的新线段；
[0039]
情况四：当投影线段的两个端点都在主线段上，则取主线段作为主线段和待合并线段合并后的新线段；
[0040]
d)重复步骤a)-步骤c)，完成图片中线段的合并；
[0041]
4)基于步骤3)中提取到的线段特征提取图像中的orb特征点，其又包括以下步骤：
[0042]
4.1)对图像中的线段，沿第i条线段的长度方向对第i条线段进行等距的预选点划分，确定划分距离的公式为：
[0043][0044]
其中，di为第i条线段对应的划分距离；n为要提取的特征点数；li为第i条线段的长度，lj为第j条线段的长度；m为此幅图像中线段的总数；
[0045]
4.2)对每个预选点进行orb特征提取的自适应阈值计算，阈值计算公式为：
[0046][0047]
其中，λ为每个特征点对应的阈值，λ
max
为设置的阈值最大值，λ
min
为设置的阈值最小值，x为特征点距线段左端点的距离；
[0048]
4.3)根据自适应阈值采用orbslam中的特征点提取算法对每个预选点进行orb特征提取；
[0049]
5)构建八叉树地图，其又包括以下步骤：
[0050]
5.1)根据相邻两帧图像提取出的orb特征点进行特征点的匹配，根据匹配后的orb特征点进行位姿估计和优化，确定是否生成关键帧；
[0051]
5.2)根据新加入关键帧进行地图点的生成；
[0052]
5.3)进行闭环检测和闭环校正，优化步骤5.1)和步骤5.2)根据orb特征点生成的位姿和地图点，生成闭环轨迹；
[0053]
5.4)保存图像文件和闭环轨迹文件：将每个关键帧对应的rgb图像保存在rgb文件夹下，rgbd图像保存在depth文件夹下，以图像对应的时间戳命名文件，形成图像文件；将每个关键帧对应的位姿存入轨迹文件，轨迹文件内容格式为时间戳和其对应的位姿；
[0054]
5.5)读取轨迹文件，获取每个关键帧对应的时间戳和位姿；
[0055]
5.6)读取图像文件，根据时间戳获取每个关键帧对应的rgbd图像，从rbgd图像获取地图点的三维点坐标，坐标转换公式如下，生成三维点云；
[0056][0057]
其中，(u,v)为地图点在图像坐标系下的坐标，(x,y,z)为地图点在世界坐标系下的坐标，d从rgbd图像中获取的深度；c
x
图像采集相机的光轴在图像坐标x轴方向上的偏移量，cy图像采集相机的光轴在图像坐标y轴方向上的偏移量，f
x
图像采集相机在x轴方向上的焦距，fy图像采集相机在y轴方向上的焦距；
[0058]
5.7)对三维点云进行滤波处理，其又包括以下步骤：
[0059]
5.7.1)对三维点云进行z方向的直通滤波，其包括步骤：
[0060]
a)将三维点云中所有地图点投影到世界坐标系的z轴，统计每个z坐标出现的次数和z坐标出现的总次数c；
[0061]
b)选取z坐标的最大值z
max
、最小值z
min
为初始值，以k为步长，进行迭代，统计(z
bottom
,z
top
)内的点数c
ik
，直到点数c
ik
的占比q大于98％，其中i为迭代变量，初始值为0；
[0062][0063]
c)以最后一次迭代的z
bottom
为直通滤波下限值，z
top
为直通滤波上限值，进行直通滤波；
[0064]
5.7.2)对三维点云进行体素滤波：在三维点云中按设定大小划分三维体素栅格，每个体素栅格内的所有点都由该体素栅格内所有点的重心来显示；
[0065]
5.8)将三维点云转为八叉树地图；
[0066]
5.9)对新生成的八叉树地图按高度进行染色，同一高度对应同一颜色。
[0067]
本发明的有益效果：
[0068]
1、本发明基于直线引导特征提取的视觉slam建图方法，其在提取图像中的线段特征时，与现有的edlines相比较本发明方法在非特征处提取的短线段更少，抗噪声感染能力更强；本发明方法通过对提取的线段进行筛选和合并处理，将断裂的线段合并为较长的线段，还原出的线段与实际吻合度更好，与现有的edlines相比本发明方法提高了直线特征提
取的准确度。
[0069]
2、本发明基于直线引导特征提取的视觉slam建图方法，在提取出的线段特征引导下进行图像中的orb特征点提取时，再结合本发明提出的自适应阈值算法，不仅能在线段的端点、线段间交点、纹理丰富的区域提取处较多的特征点，而且相比于现有orbslam中的特征点提取算法能在线段中部、低纹理区域提取出更多的特征点，使得特征点分布更合理，为后续构建地图准确快速的判断关键帧和降低位姿估计累计误差打下了良好基础，能让进行闭环校正的闭环轨迹的精度更高，能提高建图准确度。
[0070]
3、本发明基于直线引导特征提取的视觉slam建图方法，其对三维点云进行z轴方向的直通滤波和整体的体素滤波，去除了噪声点，减少了三维点云的数据量，缩短了八叉树地图的转化时间，保证了算法的实时性。
附图说明
[0071]
图1为原始edlines线特征提取算法提取到直线特征的效果图；
[0072]
图2为基于prosac剔除噪声点的改进edlines直线提取方法提取到直线特征的效果图；
[0073]
图3为投影线段的两个端点同在主线段某一端外侧的线段合并示意图；
[0074]
图4为投影线段的两个端点同在主线段某一端外侧的线段不合并示意图；
[0075]
图5为投影线段的两个端点在主线段的两端外侧的线段合并示意图；
[0076]
图6为投影线段的一个端点在主线段上，另一端点主线段外的线段合并示意图；
[0077]
图7为投影线段的两个端点都在主线段上的线段合并示意图；
[0078]
图8为基于prosac与筛选合并的改进edlines直线提取方法提取到直线特征的效果图；
[0079]
图9为沿线段的长度等距划分预选点的效果图；
[0080]
图10为自适应阈值λ与特征点距线段左端点的距离x的关系图；
[0081]
图11为采用现有orbslam中的特征点提取算法提取图像a中特征点的效果图；
[0082]
图12为采用实施例中方法提取图像a中特征点的效果图；
[0083]
图13为采用现有orbslam中的特征点提取算法提取图像b中特征点的效果图；
[0084]
图14为采用实施例中方法提取图像b中特征点的效果图；
[0085]
图15为闭环轨迹图；
[0086]
图16为由orbslam建立的稀疏点地图；
[0087]
图17为未进行直通滤波的三维点云图；
[0088]
图18为经过直通滤波处理后的三维点云图；
[0089]
图19为现有方法构建的八叉树地图，从图中可以看出其存在参考平面不平的问题；
[0090]
图20为现有方法构建的八叉树地图，从图中可以看出其存在角度与实际不一致的问题；
[0091]
图21为实施例中公开的方法构建的八叉树地图；
[0092]
图22为基于直线引导特征提取的视觉slam建图方法的流程图。
具体实施方式
[0093]
下面结合附图和实施例对本发明作进一步描述。
[0094]
本实施例基于直线引导特征提取的视觉slam建图方法包括步骤：
[0095]
1)通过相机实时拍摄环境图像。
[0096]
2)对图像进行高斯滤波，以平滑图像。
[0097]
3)提取图像中的线段特征，其又包括以下步骤：
[0098]
3.1)计算图像中每个像素的梯度方向和梯度大小；
[0099]
3.2)遍历每个像素，选取在梯度方向上梯度值大于或等于相邻像素梯度值的像素，定义为锚点，
[0100]
3.3)连接锚点，形成边缘像素链；
[0101]
3.4)对每条边缘像素链采用prosac方法剔除噪声点，拟合线段，其又包括以下步骤：
[0102]
a)从边缘像素链中任选2个像素作为线段的起点和终点，并根据所选的2个像素计算出一条直线；
[0103]
b)求取边缘像素链中的其它像素到步骤a)计算所得直线的加权距离，以加权距离在dh范围内的像素作为内点构成内点集；其中dh为给定阈值；
[0104]di
＝αd
i1
+βd
i2
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0105][0106]di2
＝cos(θ
1-θ2)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0107]
其中，di为像素距直线的加权距离，d
i1
为像素(xi,yi)到直线y＝kx+b的欧式距离，k为直线斜率，b为直线截距，d
i2
为像素(xi,yi)梯度方向θ1与直线y＝kx+b的法线方向θ2的单位向量距离，α和β为权值因子；
[0108]
c)以最大化内点数量为指标进行迭代过程，每次迭代过程中用于确定新直线的2个像素从上一次迭代得到的内点集里面选取，直至内点占像素链中所有像素的比例超过比例阈值τ或达到迭代次数num；
[0109]
d)以最后一次迭代得到的线段作为最终结果，并输出提取到的线段方程。
[0110]
图1为原始edlines线特征提取算法提取直线特征的效果图，图2为采用本实施例中方法提取直线特征的效果图；通过对比图1和图2可知，原edlines算法在噪声的干扰下，在非直线特征处会提取出的较短直线较多，而本实施例中方法在非直线特征处会提取出的较短直线很少，本实施例中公开的方法抗噪声干扰能力更强。
[0111]
3.5)对步骤3.4)得到的线段进行筛选和合并，其包括以下步骤：
[0112]
a)任意选择图片中的一条线段作为主线段，再拓展一个长为l+δl,宽为δw的矩形，主线段位于矩形的中心位置，矩形的长边与主线段平行，其中l为主线段的长度，δl和δw为设定的阈值，经过试验，δl设为5个像素效果较好，δw设为3个像素效果较好，当然在具体实施中δl和δw可根据实际情况调整。以向量相似度筛选矩形内及与矩形相交的线段，将满足向量相似度筛选公式的线段放入集合中得到线段集合；向量相似度筛选公式为：
[0113]
[0114][0115][0116]
cosθ≥cosθ
min
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
[0117]
其中为主线段l1的向量表示，为待合并线段l2的向量表示；主线段l1的两个端点坐标分别为(x
1a
,y
1a
)、(x
1b
,y
1b
)，待合并线段l2的两个端点坐标分别为(x
2a
,y
2a
)、(x
2b
,y
2b
)，θ为主线段l1与待合并线段l2的夹角，θ
min
为设定的夹角阈值；
[0118]
b)对线段集合中的线段按照与主线段的距离升序排序，距离度量公式为：
[0119][0120][0121][0122]
其中，为待合并线段的中点p的坐标，d
p
为中点p到主线段y＝k1x+b1的欧式距离，k1为直线斜率，b1为直线截距；
[0123]
c)依次选择步骤b)中升序排序中的线段作为待合并线段，将待合并线段正投影到包含该主线段的直线上，得到待合并线段的投影线段，再分情况合并线段：
[0124]
情况一：当投影线段的两个端点主线段之外，且投影线段的两个端点在主线段的同一侧，则计算主线段和投影线段的两相邻端点的距离，如果两个相邻端点的距离小于设定的阈值则在主线段的两个端点和投影线段的两个端点共四个端点中，取距离最大的两个端点所确定的线段作为主线段和待合并线段合并后的新线段；如果两个相邻端点的距离大于设定的阈值则不进行线段合并；
[0125]
情况二：当投影线段的两个端点在主线段的两端外侧，则取投影线段作为主线段和待合并线段合并后的新线段；
[0126]
情况三：当投影线段的一个端点在主线段上，投影线段的另一端点主线段外，则在主线段的两个端点和投影线段的两个端点共四个端点中，取距离最大的两个端点所确定的线段作为主线段和待合并线段合并后的新线段；
[0127]
情况四：当投影线段的两个端点都在主线段上，则取主线段作为主线段和待合并线段合并后的新线段；
[0128]
d)重复步骤a)-步骤c)，完成图片中线段的合并。
[0129]
图8为基于prosac与筛选合并的改进edlines直线提取方法提取到直线特征的效果图。通过对比图2和图8可知，经本实施例中的线段筛选和合并处理后，断裂的线段被合并为较长的线段，还原出的线段与实际相吻合，提高了线段提取的准确度。并且，将图8和图1做对比，可明显看出本实施例中公开方法相对于现有edlines而言，本实施例中方法提取到的直线特征与实际情况更吻合，直线特征提取精度更高。
[0130]
4)基于步骤3)中提取到的线段特征提取图像中的orb特征点，其又包括以下步骤：
[0131]
4.1)对图像中的线段，沿第i条线段的长度方向对第i条线段进行等距的预选点划
分，确定划分距离的公式为：
[0132][0133]
其中，di为第i条线段对应的划分距离；n为要提取的特征点数；li为第i条线段的长度，lj为第j条线段的长度；m为此幅图像中线段的总数。
[0134]
4.2)对每个预选点进行orb特征提取的自适应阈值计算，阈值计算公式为：
[0135][0136]
其中，λ为每个特征点对应的阈值，λ
max
为设置的阈值最大值，λ
min
为设置的阈值最小值，x为特征点距线段左端点的距离。
[0137]
从图10自适应阈值λ与特征点距线段左端点的距离x的关系图可知，靠近线段两端区域的特征点不仅数量少且容易随着相机运动在下一帧中消失，而靠近线段中间区域的特征点数量较多且在相机运动中较为稳定，因此希望能提出更多靠近线段中间区域的特征点。而阈值越低，特征点提取条件就越宽松，从而将提取出更多的特征点，因此，通过自适应降低靠近线段中间区域的特征点阈值，就可以达到从一条线段中更均衡化地提取orb特征的目的。
[0138]
4.3)根据自适应阈值采用orbslam中的特征点提取算法对每个预选点进行orb特征提取。
[0139]
从图14与图13的对比和图12与图11的对比可知，本实施例中公开的基于直线引导和自适应阈值的orb特征提取方法不仅能在线段的端点、线段间交点、纹理丰富的区域提取处较多的特征点，而且相比于现有orbslam中的特征点提取算法能在线段中部、低纹理区域提取出更多的特征点，使得特征点数量更多且分布更合理，为后续构建地图准确快速的判断关键帧和降低位姿估计累计误差打下了良好基础，能让后续进行闭环校正的闭环轨迹的精度更高，能提高建图准确度。
[0140]
5)构建八叉树地图，其又包括以下步骤：
[0141]
5.1)根据相邻两帧图像提取出的orb特征点进行特征点的匹配，根据匹配后的orb特征点进行位姿估计和优化，确定是否生成关键帧。
[0142]
5.2)根据新加入关键帧进行地图点的生成；本步骤采用orbslam处理关键帧图像建立地图点。
[0143]
5.3)进行闭环检测和闭环校正，优化步骤5.1)和步骤5.2)根据orb特征点生成的位姿和地图点，生成闭环轨迹；
[0144]
闭环轨迹如图15所示，如图16所示orbslam建立的稀疏点地图无法准确清晰看出原三维场景的结构，区分可行区域和障碍物，故不可直接用于导航。
[0145]
5.4)保存图像文件和闭环轨迹文件：将每个关键帧对应的rgb图像保存在rgb文件夹下，rgbd图像保存在depth文件夹下，以图像对应的时间戳命名文件，形成图像文件；将每个关键帧对应的位姿存入轨迹文件，轨迹文件内容格式为时间戳和其对应的位姿。
[0146]
5.5)读取轨迹文件，获取每个关键帧对应的时间戳和位姿。
[0147]
5.6)读取图像文件，根据时间戳获取每个关键帧对应的rgbd图像，从rbgd图像获取地图点的三维点坐标，坐标转换公式如下，生成三维点云；
[0148][0149]
其中，(u,v)为地图点在图像坐标系下的坐标，(x,y,z)为地图点在世界坐标系下的坐标，d从rgbd图像中获取的深度；c
x
图像采集相机的光轴在图像坐标x轴方向上的偏移量，cy图像采集相机的光轴在图像坐标y轴方向上的偏移量，f
x
图像采集相机在x轴方向上的焦距，fy图像采集相机在y轴方向上的焦距。
[0150]
5.7)对三维点云进行滤波处理，其又包括以下步骤：
[0151]
5.7.1)对三维点云进行z方向的直通滤波，其包括步骤：
[0152]
a)将三维点云中所有地图点投影到世界坐标系的z轴，统计每个z坐标出现的次数和z坐标出现的总次数c；
[0153]
b)选取z坐标的最大值z
max
、最小值z
min
为初始值，以k＝0.05m为步长，进行迭代，当然步长还可设定为其它值，统计(z
bottom
,z
top
)内的点数c
ik
，直到点数c
ik
的占比q大于98％，其中i为迭代变量，初始值为0；
[0154][0155]
c)以最后一次迭代的z
bottom
为直通滤波下限值，z
top
为直通滤波上限值，进行直通滤波。
[0156]
对于室内场景，z方向的高度通常在一有限范围内，使用直通滤波器设置一个范围，可较快剪除z方向的离群点。对比图17和图18可知，本实施例中通过对三维点云进行z方向的直通滤波，剪除了z方向的离群点。
[0157]
5.7.2)对三维点云进行体素滤波。由于前述步骤生成的三维点云较为密集，在后续转为八叉树地图中会占据较多内存，花费较多时间，难以保证算法实时性。为了保证建图的实时性，本步骤在三维点云中按一定大小划分三维体素栅格，可以把体素栅格视为微小的空间三维立方体的集合，然后单个体素栅格(即三维立方体)内所有点都由该体素栅格中所有点的重心来显示。如此，通过三维点云进行体素滤波减小了地图点的数量，进而在后续转为八叉树地图中时耗时更短，能更好的保证算法实时性。
[0158]
经多次实验，体素栅格大小为3cmx3cmx3cm时效果最好，在保持三维点云的形状特征的同时减少了点云数据，提高了转化速度，保证算法实时性。以i5-7200u,8gb电脑实测将
一副为640x480像素的rgbd图像转为点云，结果如下：
[0159] 所占内存大小(kb)生成点云耗时(ms)未体素滤波的点云340139体素滤波后的点云3012
[0160]
5.8)将三维点云转为八叉树地图；利用八叉树地图库octomap，创建一颗octree,调用octree的updatenode方法，将三维点云中的点插入octree中。
[0161]
5.9)对新生成的八叉树地图按高度进行染色，同一高度对应同一颜色。
[0162]
生成的八叉树地图如图21所示，通过图21与图20和图19对比可知，本实施例中方法构建的八叉树地图与实际更相符，误差更小，避免了采用现有方法构建八叉树地图存在的参考平面不平、角度与实际不一致的技术问题。
[0163]
最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，则均应涵盖在本发明的权利要求范围当中。
技术特征：1.一种基于直线引导特征提取的视觉slam建图方法，包括步骤：1)通过相机实时拍摄环境图像；2)对图像进行高斯滤波，以平滑图像；其特征在于：3)提取图像中的线段特征，其又包括以下步骤：3.1)计算图像中每个像素的梯度方向和梯度大小；3.2)遍历每个像素，选取在梯度方向上梯度值大于或等于相邻像素梯度值的像素，定义为锚点，3.3)连接锚点，形成边缘像素链；3.4)对每条边缘像素链采用prosac方法剔除噪声点，拟合线段，其又包括以下步骤：a)从边缘像素链中任选2个像素作为线段的起点和终点，并根据所选的2个像素计算出一条直线；b)求取边缘像素链中的其它像素到步骤a)计算所得直线的加权距离，以加权距离在d
h
范围内的像素作为内点构成内点集；其中d
h
为给定阈值；d
i
＝αd
i1
+βd
i2
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)d
i2
＝cos(θ
1-θ2)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)其中，d
i
为像素距直线的加权距离，d
i1
为像素(x
i
,y
i
)到直线y＝kx+b的欧式距离，k为直线斜率，b为直线截距，d
i2
为像素(x
i
,y
i
)梯度方向θ1与直线y＝kx+b的法线方向θ2的单位向量距离，α和β为权值因子；c)以最大化内点数量为指标进行迭代过程，每次迭代过程中用于确定新直线的2个像素从上一次迭代得到的内点集里面选取，直至内点占像素链中所有像素的比例超过比例阈值τ或达到迭代次数num；d)以最后一次迭代得到的线段作为最终结果，并输出提取到的线段方程；3.5)对步骤3.4)得到的线段进行筛选和合并，其包括以下步骤：a)任意选择图片中的一条线段作为主线段，再拓展一个长为l+δl,宽为δw的矩形，主线段位于矩形的中心位置，矩形的长边与主线段平行，其中l为主线段的长度，δl和δw为设定的阈值，以向量相似度筛选矩形内及与矩形相交的线段，将满足向量相似度筛选公式的线段放入集合中得到线段集合；向量相似度筛选公式为：的线段放入集合中得到线段集合；向量相似度筛选公式为：的线段放入集合中得到线段集合；向量相似度筛选公式为：cosθ≥cosθ
min
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)其中为主线段l1的向量表示，为待合并线段l2的向量表示；主线段l1的两个端点坐标分别为(x
1a
,y
1a
)、(x
1b
,y
1b
)，待合并线段l2的两个端点坐标分别为(x
2a
,y
2a
)、(x
2b
,y
2b
)，θ为
主线段l1与待合并线段l2的夹角，θ
min
为设定的夹角阈值；b)对线段集合中的线段按照与主线段的距离升序排序，距离度量公式为：b)对线段集合中的线段按照与主线段的距离升序排序，距离度量公式为：b)对线段集合中的线段按照与主线段的距离升序排序，距离度量公式为：其中，为待合并线段的中点p的坐标，d
p
为中点p到主线段y＝k1x+b1的欧式距离，k1为直线斜率，b1为直线截距；c)依次选择步骤b)中升序排序中的线段作为待合并线段，将待合并线段正投影到包含该主线段的直线上，得到待合并线段的投影线段，再分情况合并线段：情况一：当投影线段的两个端点主线段之外，且投影线段的两个端点在主线段的同一侧，则计算主线段和投影线段的两相邻端点的距离，如果两个相邻端点的距离小于设定的阈值则在主线段的两个端点和投影线段的两个端点共四个端点中，取距离最大的两个端点所确定的线段作为主线段和待合并线段合并后的新线段；如果两个相邻端点的距离大于设定的阈值则不进行线段合并；情况二：当投影线段的两个端点在主线段的两端外侧，则取投影线段作为主线段和待合并线段合并后的新线段；情况三：当投影线段的一个端点在主线段上，投影线段的另一端点主线段外，则在主线段的两个端点和投影线段的两个端点共四个端点中，取距离最大的两个端点所确定的线段作为主线段和待合并线段合并后的新线段；情况四：当投影线段的两个端点都在主线段上，则取主线段作为主线段和待合并线段合并后的新线段；d)重复步骤a)-步骤c)，完成图片中线段的合并；4)基于步骤3)中提取到的线段特征提取图像中的orb特征点，其又包括以下步骤：4.1)对图像中的线段，沿第i条线段的长度方向对第i条线段进行等距的预选点划分，确定划分距离的公式为：其中，d
i
为第i条线段对应的划分距离；n为要提取的特征点数；l
i
为第i条线段的长度，l
j
为第j条线段的长度；m为此幅图像中线段的总数；4.2)对每个预选点进行orb特征提取的自适应阈值计算，阈值计算公式为：其中，λ为每个特征点对应的阈值，λ
max
为设置的阈值最大值，λ
min
为设置的阈值最小值，
x为特征点距线段左端点的距离；4.3)根据自适应阈值采用orbslam中的特征点提取算法对每个预选点进行orb特征提取；5)构建八叉树地图，其又包括以下步骤：5.1)根据相邻两帧图像提取出的orb特征点进行特征点的匹配，根据匹配后的orb特征点进行位姿估计和优化，确定是否生成关键帧；5.2)根据新加入关键帧进行地图点的生成；5.3)进行闭环检测和闭环校正，优化步骤5.1)和步骤5.2)根据orb特征点生成的位姿和地图点，生成闭环轨迹；5.4)保存图像文件和闭环轨迹文件：将每个关键帧对应的rgb图像保存在rgb文件夹下，rgbd图像保存在depth文件夹下，以图像对应的时间戳命名文件，形成图像文件；将每个关键帧对应的位姿存入轨迹文件，轨迹文件内容格式为时间戳和其对应的位姿；5.5)读取轨迹文件，获取每个关键帧对应的时间戳和位姿；5.6)读取图像文件，根据时间戳获取每个关键帧对应的rgbd图像，从rbgd图像获取地图点的三维点坐标，坐标转换公式如下，生成三维点云；其中，(u,v)为地图点在图像坐标系下的坐标，(x,y,z)为地图点在世界坐标系下的坐标，d从rgbd图像中获取的深度；c
x
图像采集相机的光轴在图像坐标x轴方向上的偏移量，c
y
图像采集相机的光轴在图像坐标y轴方向上的偏移量，f
x
图像采集相机在x轴方向上的焦距，f
y
图像采集相机在y轴方向上的焦距。5.7)对三维点云进行滤波处理，其又包括以下步骤：5.7.1)对三维点云进行z方向的直通滤波，其包括步骤：a)将三维点云中所有地图点投影到世界坐标系的z轴，统计每个z坐标出现的次数和z坐标出现的总次数c；b)选取z坐标的最大值z
max
、最小值z
min
为初始值，以k为步长，进行迭代，统计(z
bottom
,z
top
)内的点数c
ik
，直到点数c
ik
的占比q大于98％，其中i为迭代变量，初始值为0；c)以最后一次迭代的z
bottom
为直通滤波下限值，z
top
为直通滤波上限值，进行直通滤波；5.7.2)对三维点云进行体素滤波：在三维点云中按设定大小划分三维体素栅格，每个
体素栅格内的所有点都由该体素栅格内所有点的重心来显示；5.8)将三维点云转为八叉树地图；5.9)对新生成的八叉树地图按高度进行染色，同一高度对应同一颜色。
技术总结本发明公开了一种基于直线引导特征提取的视觉SLAM建图方法，其包括1)通过相机实时拍摄环境图像；2)对图像进行高斯滤波；3)提取图像中的线段特征；4)基于步骤3)中提取到的线段特征提取图像中的ORB特征点；5)构建八叉树地图。本发明方法中提取的线段特征与实际吻合度相对于现有方法更好，在提取出的线段特征引导下再结合自适应阈值算法，使得提取的ORB特征点分布更合理，为后续构建地图准确快速的判断关键帧和降低位姿估计累计误差打下了良好基础，能让进行闭环校正的闭环轨迹的精度更高，能提高建图准确度；且通过对三维点云进行Z轴方向的直通滤波和整体的体素滤波，缩短了八叉树地图的转化时间，保证了算法的实时性。保证了算法的实时性。保证了算法的实时性。
技术研发人员：宋永端 沈志熙 陈宇栋
受保护的技术使用者：重庆大学
技术研发日：2022.03.22
技术公布日：2022/7/5
