一种基于曼哈顿世界假设的结构化征筛选方法

1.本发明属于图像图形领域，涉及一种同时定位与建图技术，具体涉及一种基于曼哈顿世界假设的结构化征筛选方法。


背景技术：

2.近年来，智能机器人已成为学术界和工业界的一个重要的研究方向。要使机器人更加智能、更加自主，机器人必须提高对未知环境的认知能力。同时定位与地图构建技术(slam，simultaneously positioning and mapping)是构建三维场景地图以及机器自身定位的关键技术，已经成功应用到无人车、无人机、快递机器人、智能清洁机器人等产品上。
3.目前稀疏纹理场景下基于结构化特征的slam系统已经能够稳定运行，且可以提取准确度较高的结构特征，然而这些系统对所提取的线面特征缺乏高效的筛选机制，使得特征冗余，如果对所有的线面特征进行跟踪，会导致计算资源的浪费。由于真实世界中存在大量结构化场景，基于曼哈顿世界假设，结构化场景中存在着平行和垂直关系，因此，基于能量函数对提取的线面特征进行筛选，设定不规则的线特征角度(除平行和垂直之外)会提升其能量值，从而保留互相垂直或平行的线面特征，可以更好的约束slam系统进行定位，同时可以节省计算资源，使得slam系统高效、稳定的运行。


技术实现要素：

4.为解决上述问题，本发明公开了一种基于曼哈顿世界假设的结构化征筛选方法,筛选线面特征，以便基于结构化特征的slam系统能够高效、稳定的运行。
5.为达到上述目的，本发明的技术方案如下：
6.一种基于曼哈顿世界假设的结构化征筛选方法，可提高弱纹理场景下slam系统的精度和稳健性，包括如下步骤：
7.(1)分别根据平面集合中平面特征点云的拟合程度和结构化场景中平面特征之间的几何约束关系构建平面特征能量函数的一阶数据项和二阶几何结构约束项；
8.(2)分别根据线集合中线特征点云的拟合程度和线特征之间的结几何约束关系构建线特征能量函数的一阶数据项和二阶几何结构约束项；
9.(3)基于迭代的置信区域优化算法对能量函数进行求解。
10.其中步骤(1)中，分别根据平面集合中平面特征点云的拟合程度和结构化场景中平面特征之间的几何约束关系构建平面特征能量函数的一阶数据项和二阶几何结构约束项，具体步骤为：
11.(1.1)对点云进行分割后得到平面候选集p；
12.(1.2)计算平面特征能量函数的一阶数据项e1，首先计算平面质心与平面的点云的距离，判断平面候选集合与点云的拟合程度，拟合程度越高，能量越低：
[0013][0014]
其中，其中πn是候选平面，λ是平面候选集合与点云的拟合度阈值，设置为30～50，n
p
为场景中候选平面对应的点云集合中点的数量；
[0015]
(1.3)计算平面特征能量函数的二阶几何结构约束相关项e2，基于曼哈顿世界假设，环境中存在垂直/正交的信息，不规则的平面角度会提升其能量，从而剔除不规则角度的平面特征：
[0016][0017][0018]
其中，为候选平面集合中平面πm和平面πn之间的平面夹角，为两平面与应当匹配的平行或垂直的几何约束所相差的角度，σ2是设定的角度增长步长；
[0019]
(1.4)得到平面特征的能量函数e(p)＝e1+e2。
[0020]
步骤(2)中，分别根据线集合中线特征点云的拟合程度和线特征之间的结几何约束关系构建线特征能量函数的一阶数据项和二阶几何结构约束项，具体步骤为：
[0021]
(2.1)提取平面边缘得到线候选集q；
[0022]
(2.2)计算线特征能量函数的一阶数据项e1，首先计算线的几何中心与分类到该线上的结构化场景点云的距离，判断线特征与点云的拟合程度，拟合程度越高，能量越低：
[0023][0024]
其中，ln是候选线，λ是线候选集合与点云的拟合度阈值，设置为20～30，n
p
为场景中候选线对应的点云集合中点的数量；
[0025]
(2.3)计算线特征能量函数的二阶几何结构数据项e2，基于曼哈顿世界假设，不规则的线特征角度会提升其能量，从而剔除不规则角度的线特征：
[0026][0027][0028]
其中，为候选线集合中线lm和线ln之间的向量夹角，是为两条候选线与应当匹配的平行或垂直的几何约束相差的角度，ε2为设定的角度增长步长；
[0029]
(2.4)得到线特征的能量函数为e(q)＝e1+e2。
[0030]
步骤(3)中，基于迭代的置信区域优化算法对能量函数进行求解，具体步骤为：
[0031]
(3.1)根据步骤(1)和步骤(2)获得平面和线的候选集p和总的能量函数e(p)；
[0032]
(3.2)在置信区域内近似的能量函数表达式，并将其转化为无约束的拉格朗日优化问题，通过最小化无约束拉格朗日函数求解b
*
；
[0033]
(3.3)根据候选解b
*
和当前解b
t
计算预测的能量减小值p和实际的能量减小值r，根据r/p与阈值σ1＝2和阈值σ2＝0.25的关系选择下一次迭代的b
t+1
和λ
t+1
，迭代执行直到r＞0，获得使能量函数最小的解b和过滤后的线面集合。
[0034]
本发明的有益效果是：
[0035]
针对目前基于结构化特征的slam系统提取的线面特征缺乏高效的筛选机制问题，本发明提出了基于曼哈顿世界假设的结构化征筛选方法。首先根据点云集合与平面和线的拟合程度构造一阶数据项，再根据结构化场景中的几何约束关系构造二阶几何结构约束项，然后迭代的置信区域优化算法对能量函数进行求解对平面集合和线集合进行筛选。
[0036]
本发明针对纹理稀疏场景下slam系统效率和鲁棒性低的问题，提出一种基于曼哈顿世界假设的结构化征筛选方法，提高了基于结构化特征的slam系统的效率和鲁棒性，能够广泛应用于自动驾驶、室内导航等场景。
附图说明
[0037]
图1为一种基于曼哈顿世界假设的结构化征筛选方法流程图；
[0038]
图2为基于迭代的置信区域优化算法对能量函数进行求解流程图。
具体实施方式
[0039]
下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
[0040]
图1为本发明的方法流程示意图。
[0041]
本发明所述的一种基于曼哈顿世界假设的结构化征筛选方法，
[0042]
步骤s1：根据平面集合中点云的拟合程度和结构化场景中平面之间的几何约束关系(垂直或平行)构建平面集合能量函数的一阶数据项和二阶几何结构约束项。具体包括：
[0043]
s1.1、对点云进行分割后得到平面候选集p{π1，π2，...πn}；
[0044]
s1.2、计算平面特征能量函数的一阶数据项e1，候选平面对应的点云集合可以表示为：
[0045][0046]
其中，是点云pi到某一候选平面质心的距离，σ1为距离阈值。计算候选平面质心与归类到该平面的结构化场景点云的距离，判断平面候选集合与点云的拟合程度，符合判定条件的点云数量越多则拟合程度越高，能量函数的值越小，因此平面特征能量函数的一阶数据项e1表示为：
[0047]
[0048]
其中，λ是平面候选集合与点云的拟合度阈值，n
p
场景中候选平面对应的点云集合中点的数量；
[0049]
s1.3、计算平面特征能量函数的二阶几何结构约束相关项e2，由于结构化场景中墙面、地面和书桌等存在较多的互相垂直或平行的平面，因此基于曼哈顿世界假设添加平面间的几何约束项，对于不规则的平面角度(即除了平行和垂直)会提升其能量，可表示为：
[0050][0051][0052]
其中，为候选平面集合中πm和πn之间的平面夹角，为两平面与应当匹配的平行或垂直的集合约束所相差的角度，当两个平面平行或垂直时为0，σ2是设定的角度增长步长，取π/18；
[0053]
s1.4、得到平面特征的能量函数：
[0054][0055]
其中，e(p)表示整体能量函数，e1表示一阶数据项，e2表示二阶几何结构约束项。
[0056]
步骤s2：根据点云集合距离边的几何中心以及边之间方向向量的结构性角度误差构建边集合能量函数的一阶数据项和二阶几何结构约束项。具体包括：
[0057]
s2.1、提取平面边缘得到线候选集q{l1，l2，...，ln}；
[0058]
s2.2、计算线特征能量函数的一阶数据项e1，候选线对应的点云集合可以表示为：
[0059][0060]
其中，是点云中的点pi到候选线的几何中心，ε1为距离阈值，首先计算候选线的几何中心与归类到该线的结构化场景点云的距离，判断候选线与点云的拟合程度，符合判定条件的点云数量越多则拟合程度越高，因此线特征能量函数的一阶数据项为：
[0061][0062]
其中，λ是线候选集合与点云的拟合度阈值，n
p
为场景中候选线对应的点云集合中点的数量；
[0063]
s2.3、计算线特征能量函数的二阶几何结构数据项，基于曼哈顿世界假设添加线的几何约束项，对于不规则的线角度(即除了平行和垂直)将提升其能量，可表示为：
[0064][0065]
[0066]
其中，为候选线集合中lm和ln之间的向量夹角，是为两条候选线与应当匹配的平行或垂直时相差的角度，当两条线平行或垂直时为0，ε2为角度增长步长，取π/18；
[0067]
s2.4、得到线特征的能量函数为：
[0068][0069]
其中，e(q)表示整体能量函数，e1表示一阶数据项，e2示二阶几何结构约束项。
[0070]
步骤s3：基于迭代的置信区域优化算法对能量函数进行求解。具体步骤为：
[0071]
s3.1、根据步骤(1)和步骤(2)获得平面和线的候选集p{i1，i2，...，in}和总的能量函数为：
[0072][0073]
其中e1，e2分别为候选平面或线的一阶数据项和二阶几何结构约束项，bn，bn∈{0，1}表示平面或线是否被选中，用矩阵乘法表示为：
[0074]
e(b)＝b
t
u+b
t
mb
[0075]
其中b＝[b1，b2，...，bn]，b1，b2，...，bn∈{0，1}，u为能量方程的一阶数据项，m＝{m
ij
∈r}为两个候选成员之间相关系数；
[0076]
s3.2、将总体能量函数e表达为一个正相关项en和负相关项e
p
：
[0077][0078]
其中m
+
表示m矩阵相关系数为正的部分m-表示m矩阵相关系数为负的部分在置信区域内能量方程可以近似为：
[0079]et
(b)＝en(b)+b
tut
+c，u
t
＝2m
+bt
[0080]
其中u
t
+c是e
p
(b)的一阶泰勒展开项，近似解将其转化为无约束的拉格朗日优化问题：
[0081]
l
t
(b)＝e
t
(b)+λ
t
·
||b-b
t
||
[0082]
其中λ
t
为置信区域大小的控制系数，通过最小化上述无约束拉格朗日函数求解b
*
；
[0083]
s3.3、根据候选解b
*
和当前解b
t
计算预测的能量减小值p：
[0084]
p＝e
t
(b
t
)-e
t
(b
*
)
[0085]
然后计算实际的能量减小值r：
[0086]
r＝e(b
t
)-e(b
*
)
[0087]
根据r/p与阈值σ1，σ2的关系选择下一次迭代的b
t+1
和λ
t+1
：
[0088][0089]
[0090]
反复执行上述两步直到r＞0，获得最终使能量函数最小的解b，流程图如图2所示。
[0091]
需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰均落入本发明权利要求书的保护范围之内。