一种基于多帧的单目SLAM鲁棒初始化方法

一种基于多帧的单目slam鲁棒初始化方法
技术领域
1.本发明属于单目slam初始化技术领域，尤其涉及一种基于多帧的单目slam鲁棒初始化方法。


背景技术：

2.同步定位与建图技术(simultaneous localization and mapping，slam)的目标是在估计摄像机的运动轨迹的同时重建未知环境。该技术目前已广泛应用于增强现实和自动驾驶等领域，能够在不依赖于外部基础设施的背景下实时运行。
3.初始化是单目slam技术的关键，通过初始化能够获得摄像机的初始姿态并且生成初始地图，为后续的跟踪阶段提供支撑。
4.目前学界主要利用类似于增量式运动恢复结构技术(structure from motion，sfm)来进行单目slam系统的初始化，主要利用两帧之间的极线几何约束或者平面结构约束，构建基础矩阵或单应矩阵，通过分解基础矩阵或单应矩阵来获取摄像机的初始姿态，并利用三角测量技术获取初始地图。这类技术对摄像机的初始姿态和特征点的匹配要求较高，初始化的过程依赖于摄像机的初始运动，无法较快地收敛。在初始化的后期，将进行集束调整 (bundle adjustment，ba)，用以进一步优化初始姿态和初始地图，但优化后仍存在“散点”，这些“散点”与其最近邻三维特征点的距离远超普通三维特征点之间的平均距离，在跟踪过程中将造成误差，尤其是在图像观测质量差的情况下，将影响后续的姿态追踪。


技术实现要素：

5.本发明的目的在于克服上述现有技术中无法快速收敛、以及存在“散点”的不足，提供了一种能够提高收敛速度、减少“散点”出现，从而提高初始地图的精度的slam初始化方法，具体为一种基于多帧的单目slam鲁棒初始化方法。
6.本发明提供了一种基于多帧的单目slam鲁棒初始化方法，该方法包括步骤：
7.s1：提取初始视频流中每一图像帧的特征点，并根据特征点对图像帧进行两两相互匹配，筛选出匹配点，获取初始匹配点对，匹配点为匹配的特征点；
8.s2：根据初始匹配点对筛选出三视图对，即拥有足够多共视特征点的三个图像帧，进而基于三焦张量的随机抽样一致算法筛选出三视图对中的匹配点，构建三帧匹配图，即描述各图像帧之间的共视关系的拓扑图；
9.s3：根据双视几何原理，求解各图像帧之间的相对旋转；
10.s4：根据各图像帧之间的相对旋转，采用迭代加权最小二乘法求解全局旋转；
11.s5：基于各图像帧的全局旋转以及场景结构同全局位移之间的线性约束关系求解全局位移；
12.s6：综合全局旋转和全局位移，获得各帧的初始位姿，以此为基础，利用仅位姿非线性优化调整策略，优化各帧的位姿；
13.s7：计算特征点景深，恢复特征点三维坐标。
14.优选的，s1中，提取初始视频流中每一图像帧的特征点，并根据特征点对图像帧进行两两相互匹配，通过随机抽样一致算法筛选匹配点，获取初始匹配点对，匹配点为匹配的特征点。
15.优选的，筛选出三视图对中的匹配点的具体步骤为：
16.s2.1：设置最小样本数为n的样本集p，从样本集p中抽取包含n个样本形成样本子集 s，利用视图间的本质矩阵计算初始三焦张量作为初始化模型m；
17.s2.2：根据初始三焦张量得到三视图的投影矩阵p1、p2、p3，并利用最小二乘法计算特征点坐标，再通过p1、p2、p3投影矩阵分别获得特征点的三个估计值，即：
[0018][0019]
其中，表示p1投影矩阵作用下特征点的估计值，表示p2投影矩阵作用下特征点的估计值，表示p3投影矩阵作用下特征点的估计值，x表示特征点三维坐标；
[0020]
根据三视图对计算重投影误差，即：
[0021][0022]
其中，ω表示重投影误差，x1表示特征点在视图1中的测量值，x2表示特征点在视图2中的测量值，x3表示特征点在视图3中的测量值，d2(
·
，
·
)表示两个元素之间欧氏距离的平方；
[0023]
将重投影误差ω作为初始化模型m的误差衡量，将样本集p中与初始化模型m的误差小于设定阈值th的样本集以及样本子集s构成内点集s*；
[0024]
s2.3：根据内点集s*采用最小二乘法计算新的模型m*；
[0025]
s2.4：重复s2.1、s2.2、s2.3，直至得到最大一致集，并剔除外点，记录内点和当前循环的三焦张量，内点即为匹配点。
[0026]
优选的，s5中，基于各图像帧的全局旋转以及场景结构同全局位移之间的线性约束关系求解全局位移；线性关系表达式为：
[0027]
bt
l
cti dtr＝0
[0028]
其中，
[0029]
b[xi]
×rr，i
xr([r
r，l
xr]
x
x
l
)
t
[x
l
]
xrl
[0030]
c＝|[x
l
]
×rr，l
xr||2[xi]
×ri
[0031]
d＝-(b c)
[0032]rr，i
＝r
irrt
[0033]
其中，t
l
表示视图l的全局位移，ti表示视图i的全局位移，tr表示视图r的全局位移，xi表示特征点在视图i中的归一化图像坐标，[
·
]
×
表示向量对应的反对称矩阵，r
r，i
表示视图r与i之间的相对旋转，xr表示特征点在视图r中的归一化图像坐标，r
l
表示视图l的全局旋转，ri表示视图i的全局旋转，rr表示视图r的全局旋转，x
l
表示特征点特征点在视图l中的归一化图像坐标，t表示矩阵的转置，r
r，l
表示视图r与l之间的相对旋转；
[0034]
考虑所有特征点，综合所有线性约束，获得下式：
[0035]f·
t＝0
[0036]
其中f为由b、c、d构成的系数矩阵，t＝(t
1t
，t
2t
，...，t
nt
)
t
表示所有n个视图的全局
位移；
[0037]
求解上述线性齐次方程即可获得全局位移的最优值
[0038]
优选的，s6中，进行仅位姿的非线性优化调整的步骤为：
[0039]
s6.1：基于各三视图对，计算重投影向量，即：
[0040][0041]
其中，表示重投影向量，表示向量(0,0,1)，是由基准视图计算获得的特征点景深，xr表示特征点在视图r中的归一化图像坐标，t
r，i
表示视图r，i之间的相对位移，表示为：
[0042]
t
r，i
＝rj(t
r-ti)
[0043]
其中，rj表示视图j的全局旋转，tr表示视图r的全局位移，ti表示视图i的全局位移；
[0044]
s6.2：对所有特征点计算重投影误差并相加，获得误差项ε，误差项ε的表达式为：
[0045][0046]
其中，表示重投影向量，xi表示特征点在视图i中的测量值，t表示矩阵的转置；
[0047]
s6.3：通过通用图优化库进行优化，将各帧的位姿作为图优化的节点，将各特征点的重投影误差作为图优化的边。
[0048]
优选的，s7中，将θ
(r，j)
作为特征点恢复质量的判断标准，根据θ
(r，j)
计算视图r的加权深度zr，加权深度zr的表达式为：
[0049][0050]
ω
(r，j)
＝θ
(r，j)
/∑
1≤j≤n
θ
(r，j)
[0051]
θ
(r，j)
＝||[xj]
×rr，j
xr||
[0052]
其中，j表示第j个视图，表示景深，ω
(r，j)
表示加权值,θ
(r，j)
表示特征点恢复质量，r
r，j
表示视图r，j之间的相对旋转，xr表示特征点在视图r中的归一化图像坐标，xj表示特征点在视图j下的坐标，[
·
]
×
表示向量的反对称矩阵；
[0053]
根据加权深度zr对初始地图进行加权重建，恢复特征点三维坐标，恢复后特征点三维坐标表示为：
[0054]
xw＝zrrrxr tr[0055]
其中，xw表示恢复后的特征点三维坐标，zr表示视图r的加权深度，rr表示视图r的全局旋转，xr表示特征点在视图r中的归一化图像坐标，tr表示视图r的全局位移。
[0056]
优选的，s6.3中，还包括通过设置核函数提升优化器的鲁棒性。
[0057]
有益效果：
[0058]
1.本发明通过引入全局式运动恢复结构的技术，相比于传统的使用两帧信息进行初始化的系统，在初始位姿的求解过程中能够更多地利用视频流信息，通过平均的方法获得高精度的初始位姿。
[0059]
2.在初始地图的三角测量过程中，利用了加权重建技术，针对一个特征点能够综合利用多个观测信息，提高了特征点的精度。
[0060]
3.在优化过程中采取以初始位姿作为优化变量的非线性优化策略，相比于传统的非线性优化策略，减少了初始地图中“散点”的数目，进一步提高了初始地图的精度。
附图说明
[0061]
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0062]
图1为本发明实施中一种基于多帧的单目slam鲁棒初始化方法的流程图。
具体实施方式
[0063]
下面将结合本发明的实施例中的附图，对本发明的实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0064]
如图1所示，本实施例提供了一种基于多帧的单目slam鲁棒初始化方法，该方法包括步骤：
[0065]
s1：提取初始视频流中每一图像帧的特征点，并根据特征点对图像帧进行两两相互匹配，通过随机抽样一致(random sample consensus，ransac)算法筛选匹配点，获取初始匹配点对，匹配点为匹配的特征点；判断是否拥有足够的特征点，是，则进行s2；否，则重复 s1。
[0066]
s2：根据初始匹配点对筛选出三视图对，即拥有足够多共视特征点的三个图像帧，进而基于三焦张量的随机抽样一致算法筛选出三视图对中的匹配点，构建三帧匹配图，即描述各图像帧之间的共视关系的拓扑图。
[0067]
其中，在两帧之间以基础矩阵e为初始化模型m，利用随机抽样一致算法无法完全剔除匹配点，在三帧之间通过以三焦张量为初始化模型m的随机抽样一致算法可以进一步剔除误匹配点，提高匹配精度；故筛选出三视图对中的匹配点的具体步骤为：
[0068]
s2.1：设置最小样本数为n的样本集p，从样本集p中抽取包含n个样本形成样本子集 s，利用两两视图间的本质矩阵计算初始三焦张量作为初始化模型m。
[0069]
s2.2：根据初始三焦张量得到三视图的投影矩阵p1、p2、p3，并利用最小二乘法计算特征点坐标，再通过p1、p2、p3投影矩阵分别获得特征点的三个估计值，即：
[0070]
[0071]
其中，表示p1投影矩阵作用下特征点的估计值，表示p2投影矩阵作用下特征点的估计值，表示p3投影矩阵作用下特征点的估计值，x表示特征点三维坐标；
[0072]
根据三视图对计算重投影误差，即：
[0073][0074]
其中，ω表示重投影误差，x1表示特征点在视图1中的测量值，x2表示特征点在视图2中的测量值，x3表示特征点在视图3中的测量值，d2(
·
，
·
)表示两个元素之间欧氏距离的平方；
[0075]
将重投影误差ω作为初始化模型m的误差衡量，将样本集p中与初始化模型m的误差小于设定阈值th的样本集以及样本子集s构成内点集s*。
[0076]
s2.3：若内点集占样本集75％以上，则认为得到正确的模型参数，并根据内点集s*采用最小二乘法计算新的模型m*。
[0077]
s2.4：重复s2.1、s2.2、s2.3，直至得到最大一致集，并剔除外点，记录内点和当前循环的三焦张量，内点即为匹配点。
[0078]
s3：根据双视几何原理，求解各图像帧之间的相对旋转。
[0079]
s4：根据各图像帧之间的相对旋转，通过迭代加权最小二乘法(iterativelyreweighted least square，irls)计算全局旋转。
[0080]
s5：由于在精确的全局旋转背景下，场景结构和全局位移之间存在线性关系，可构建线性全局平移约束，故而基于各图像帧的全局旋转，以及场景结构和全局位移之间的线性约束关系可直接求解全局位移；
[0081]
其中，线性关系表达式为：
[0082]
bt
l
cti dtr＝0
[0083]
其中，
[0084]
b＝[xi]
×rr，i
xr([r
r，l
xr]
x
x
l
)
t
[x
l
]
xrl
[0085]
c＝||[x
l
]
×rr，l
xr||2[xi]
×ri
[0086]
d＝-(b c)
[0087]rr，i
＝r
irrt
[0088]
其中，t
l
表示视图l的全局位移，ti表示视图i的全局位移，tr表示视图r的全局位移，xi表示特征点在视图i中的归一化图像坐标，[
·
]
×
表示向量对应的反对称矩阵，r
r，i
表示视图r与i之间的相对旋转，xr表示特征点在视图r中的归一化图像坐标，r
l
表示视图l的全局旋转，ri表示视图i的全局旋转，rr表示视图r的全局旋转，x
l
表示特征点特征点在视图l中的归一化图像坐标，t表示矩阵的转置，r
r，l
表示视图r与l之间的相对旋转；
[0089]
考虑所有特征点，综合所有线性约束，可获得下式：
[0090]f·
t＝0
[0091]
其中f为由b、c、d构成的系数矩阵，t＝(t
1t
，t
2t
，...，t
nt
)
t
表示所有n个视图的全局位移。
[0092]
求解上述线性齐次方程即可获得全局位移的最优值
[0093]
s6：综合全局旋转和全局位移，获得各帧的初始位姿，以此为基础，利用仅位姿非
线性优化调整策略，优化各帧的位姿，使得重投影误差最小；
[0094]
其中，进行仅位姿的非线性优化调整的步骤为：
[0095]
s6.1：基于各三视图对，计算重投影向量，即：
[0096][0097]
其中，表示重投影向量，表示向量(0,0,1)，是由基准视图计算获得的特征点景深，xr表示特征点在视图r中的归一化图像坐标，t
r，i
表示视图r，i之间的相对位移，表示为：
[0098]
t
r，i
＝rj(t
r-ti)
[0099]
其中，rj表示视图j的全局旋转，tr表示视图r的全局位移，ti表示视图i的全局位移；
[0100]
s6.2：对所有特征点计算重投影误差并相加，获得误差项ε，误差项ε的表达式为：
[0101][0102]
其中，表示重投影向量，xi表示特征点在视图i中的测量值，t表示矩阵的转置；
[0103]
s6.3：通过通用图优化库进行优化，将各帧的位姿作为图优化的节点，将各特征点的重投影误差作为图优化的边，且通过设置核函数提升优化器的鲁棒性。
[0104]
s7：利用三角测量技术，计算特征点景深，恢复特征点三维坐标；
[0105]
具体为：将θ
(r，j)
作为特征点恢复质量的判断标准，根据θ
(r，j)
计算视图r的加权深度 zr，加权深度zr的表达式为：
[0106][0107]
ω
(r，j)
＝θ
(r，j)
/∑
1≤j≤n
θ
(r，j)
[0108]
θ
(r，j)
＝||[xj]
×rr，j
xr||
[0109]
其中，j表示第j个视图，表示景深，ω
(r，j)
表示加权值,θ
(r，j)
表示特征点恢复质量，r
r，j
表示视图r，j之间的相对旋转，xr表示特征点在视图r中的归一化图像坐标，xj表示特征点在视图j下的坐标，[
·
]
×
表示向量的反对称矩阵；
[0110]
根据加权深度zr对初始地图进行加权重建，恢复特征点三维坐标，恢复后特征点三维坐标表示为：
[0111]
xw＝zrrrxr tr[0112]
其中，xw表示恢复后的特征点三维坐标，zr表示视图r的加权深度，rr表示视图r的全局旋转，xr表示特征点在视图r中的归一化图像坐标，tr表示视图r的全局位移。
[0113]
本实施例提供的这种基于多帧的单目slam鲁棒初始化方法具有以下有益效果：
[0114]
1.实施例通过在单目初始化系统中引入全局式运动恢复结构的技术，相比于传统
的使用两帧信息进行初始化的系统，在初始位姿的求解过程中能够更多地利用视频流信息，通过平均的方法获得高精度的初始位姿。
[0115]
2.在初始地图的三角测量过程中，利用了加权重建技术，针对一个特征点能够综合利用多个观测信息，提高了初始地图的精度。
[0116]
3.在优化过程中采取以初始位姿作为优化变量的非线性优化策略，相比于传统的非线性优化策略，减少了初始地图中“散点”的数目，进一步提高了初始地图的精度。
[0117]
以上仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换或改进等，均应包含在本发明的保护范围之内。