一种动态场景下基于目标检测和几何概率的视觉SLAM方法

一种动态场景下基于目标检测和几何概率的视觉slam方法
技术领域
1.本发明涉及一种视觉slam方法，特别是一种动态场景下基于目标检测和几何概率的视觉slam方法。


背景技术：

2.slam(simultaneous localization and mapping，即时定位与地图构建)技术是智能机器人应用的核心技术，它可以在未知的环境下同时估计自身位置和构建环境地图。视觉slam以相机作为感知传感器，因其成本低、高精度、信息丰富，成为近年来重要的研究课题。
3.在过去的几十年里，出现了许多优秀的视觉slam，如orb-salm2、kinectfusion、svo、dso；然而，传统的视觉slam系统通常在静态环境中工作得很好，但当环境中有动态对象时仅仅使用几何约束的方法剔除动态点，所以效果很差，甚至会失败。随着深度学习技术的发展，我们目前可以通过语义分割或目标检测网络标记动态物体，然后剔除动态物体的全部特征。但是语义分割网络无法在保证实时性的前提下，同时保证精准的语义分割；目标检测网络能满足实时要求，但会导致部分静态物体被剔除而降低定位精度和建图质量。


技术实现要素：

4.本发明的目的就是提供一种动态场景下基于目标检测和几何概率的视觉slam方法，该方法能有效提高动态场景下的slam系统的定位精度。
5.本发明的目的是通过这样的技术方案实现的，具体步骤如下：
6.1)采集待处理图像，使用目标检测算法将待处理图像划分为静态区域和动态区域；
7.2)使用vsac算法计算静态区域匹配点的基础矩阵f；
8.3)使用基础矩阵f和对极约束计算动态区域匹配点的几何概率；
9.4)将动态区域连续两帧匹配点的几何概率融合，作为当前帧匹配点的最终几何概率；
10.5)对动态区域未匹配的点进行概率拓展，得到动态区域所有特征点的几何概率，并剔除不满足概率要求的特征点，将剩余的点传入后续跟踪和建图步骤。
11.进一步，步骤1)中使用目标检测算法将待处理图像划分为静态区域和动态区域的具体步骤为：
12.1-1)使用crowdhuman数据集对yolov3 tiny目标检测网络进行训练，得到训练好的yolov3 tiny网络；
13.1-2)使用训练好的yolov3 tiny网络将待处理图像分为动态区域和静态区域，并提取待处理图像的特征点。
14.进一步，步骤2)中使用vsac算法计算静态区域匹配点基础矩阵f的具体步骤为：
15.2-1)对静态区域的特征点进行特征匹配，匹配点对数记为n，根据匹配点的质量进
行排序，并从高质量匹配点中随机抽样出最小样本集，使用数值法计算其基础矩阵fi；
16.2-2)使用自适应sprt算法判断基础矩阵fi是否满足成为最优基础矩阵f
best
的条件，满足则转至步骤2-3)，不满足则返回步骤2-1)；
17.2-3)判断当前基础矩阵的最小样本集中是否满足有q个点共面的判定条件，若不满足判定条件，则当前基础矩阵不退化，并转至步骤2-4)；
18.若满足判定条件，则随机在最小样本集外抽取样本，对当前的基础矩阵fi通过对极约束验证，判断其是否有满足有p个非主导平面外的点的限定条件，若满足限定条件，则当前基础矩阵不退化，若不满足限定条件，并转至步骤2-4)，则当前基础矩阵退化，舍弃当前的基础矩阵，返回步骤2-1)；
19.2-4)计算基础矩阵fi对应的内点个数，若比当前最优f
best
的内点数多，则计算独立内点个数，若独立内点个数大于阈值将fi记为最优f
best
，并转至步骤2-5)，反之则返回步骤2-1)；
20.2-5)判断当前最优f
best
是否满足局部优化条件，若满足局部优化条件，则使用f
best
的内点集对f
best
进行局部优化，若不满足局部优化条件返回步骤2-1)；
21.所述局部优化的判断条件为：若独立内点个数大于阈值且只有当前的最优基础矩阵和上一个最优基础矩阵的最小样本集低于0.95jaccard指数，则满足进行局部优化的条件，反之则不满足进行局部优化的条件；
22.所述局部优化的过程为：在最优基础矩阵对应的内点中随机选取大于最小样本集个数的点，使用解析法迭代计算出更好的基础矩阵；
23.2-6)判断算法是否达到循环终止条件，若满足循环终止条件则转至步骤2-7)，若不满足循环终止条件则返回步骤2-1)，所述循环终止的判断条件为：η大于预设阈值其中η表示当前所有随机抽取的最小样本集中至少包含一个全部为内点的最小样本集的概率，其计算方式如下：
24.η＝(1-pg(1-α))k25.其中k为当前迭代次数，pg代表最小样本集中的m个点全为内点的概率，α代表拒绝一个好模型的概率，计算方式如下：
26.pg＝εm27.α≈1/a
[0028][0029]
2-7)对最优f
best
使用迭代最小二乘方法做最后的优化，得到最终的最优f
best
，并将最终的最优f
best
作为静态区域的基础矩阵f。
[0030]
进一步，步骤2-2)中使用自适应sprt算法判断基础矩阵fi是否满足成为最优基础矩阵f
best
的条件的具体步骤为：
[0031]
2-2-1)构建基础矩阵fi使用自适应sprt算法的限定条件：
[0032][0033]
式中，和tv分别代表在使用和不使用自适应sprt时测试一个点是否为当前基础矩阵fi内点的时间，α是拒绝好模型的概率，ew(t)是已测试点的平均数；
[0034]
若满足，则转至步骤2-2-2)，若不满足，则转至步骤2-3)；
[0035]
2-2-2)计算自适应sprt的参数初始值：坏模型内点概率δ0和好模型内点概率ε0：
[0036][0037][0038][0039]
式中，t为匹配点的总数，前n次迭代中非最优基础矩阵对应的内点个数的平均值是当前最优基础矩阵f对应的内点个数，i
δ
是通过δ0和估计得到的非最优基础矩阵f对应的最大内点个数；
[0040]
令初始时刻j＝1，在最小样本集外随机抽取一个匹配点，计算基于wald的sprt似然比λj：
[0041][0042]
式中，hb与hg分别代表一个坏模型和一个好模型，p(xr|hb)为一个点是坏模型内点概率，p(xr|hg)为一个点是好模型内点概率；若数据点r为模型的内点，则xr＝1，否则xr＝0；
[0043]
若λj》a，则当前的基础矩阵为坏模型，返回步骤2-1)，其中a为决策阈值；
[0044]
若λj≤a，且j》n，则当前的基础矩阵是一个好模型，即满足成为最优基础矩阵的条件，转至步骤2-3)；
[0045]
若λj≤a，且j≤n，则重新抽取一个点，并将j自增1，继续计算基于wald的sprt似然比，直到j》n。
[0046]
进一步，步骤3)中使用基础矩阵f和对极约束计算动态区域匹配点的几何概率的具体步骤为：
[0047]
使用基础矩阵f计算动态区域内匹配点的极线再计算动态区域内匹配点到极线的距离
[0048][0049][0050]
式中，p
′i和pi为一对匹配点，x,y,z为极线的向量参数表示；
[0051]
将动态区域内匹配点到极线的距离使用二值sigmod函数转换为几何概率
[0052][0053]
式中，dth为静态点和动态点到极线距离的阈值。
[0054]
进一步，步骤4)中计算当前帧匹配点的最终几何概率的具体步骤为：
[0055]
根据上一帧点p
′i状态概率的概率值预测当前帧点pi状态概率的概率值
[0056][0057]
根据上一帧点p
′i状态概率的方差预测当前帧点pi状态概率的方差
[0058][0059]
式中，δ0为匹配点之间的状态概率转移的标准差；
[0060]
计算卡尔曼增益k：
[0061][0062]
式中，δ1为关键点的观测状态概率标准差；
[0063]
根据点pi的观测状态概率更新点pi的状态概率：
[0064][0065]
式中，表示点pi的状态概率在当前帧的预测结果，表示点pi的状态概率通过卡尔曼滤波后的最终结果；
[0066]
更新点pi状态概率的方差：
[0067][0068]
式中，为当前帧点pi状态概率方差的预测值。
[0069]
进一步，步骤5)中对动态区域未匹配的点进行概率拓展，得到动态区域所有特征点的几何概率，并剔除不满足概率要求特征点的具体步骤为：
[0070]
搜索动态区域内高置信度特征点周围与前一帧不匹配的相邻特征点pi，更新相邻特征点pi的状态概率
[0071][0072][0073]
式中，pj为与点pi相邻的高置信度特征点，为点pj的状态概率，为点pi观测状态概率；c是常数，λ(d)代表距离因子；
[0074]
若小于预设的阈值，则将pi划分为动态点；
[0075]
若大于阈值则划分为静态点，并剔除动态点，将动态区域中的静态点加入到静态区域的点集中，用于后续的定位和建图。
[0076]
由于采用了上述技术方案，本发明具有如下的优点：
[0077]
1、本技术将目标检测网络与传统的几何方法相结合能够同时满足视觉slam的精度和实时性要求，既保证了实时性又提高了定位精度和建图质量。
[0078]
2、本技术使用目标检测算法对图像动态区域和静态区域进行标记，将vsac算法与对极约束结合，计算动态区域匹配点的几何概率，提高动态场景下的图像特征点匹配精度，提高了定位精度。
[0079]
本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书和权利要求书来实现和获得。
附图说明
[0080]
本发明的附图说明如下。
[0081]
图1为本发明的方法流程图。
[0082]
图2为本发明使用vsac算法计算静态区域匹配点的基础矩阵的方法流程图。
具体实施方式
[0083]
下面结合附图和实施例对本发明作进一步说明。
[0084]
实施例1：
[0085]
1.一种动态场景下基于目标检测和几何概率的视觉slam方法，具体步骤如下：
[0086]
1)采集待处理图像，使用目标检测算法将待处理图像划分为静态区域和动态区域；具体步骤为：
[0087]
1-1)使用crowdhuman数据集对yolov3 tiny目标检测网络进行训练，得到训练好的yolov3 tiny网络；
[0088]
1-2)使用训练好的yolov3 tiny网络将待处理图像分为动态区域和静态区域，并提取待处理图像的特征点。
[0089]
2)使用vsac算法计算静态区域匹配点的基础矩阵f，具体步骤为：
[0090]
2-1)对静态区域的特征点进行特征匹配，匹配点对数记为n，根据匹配点的质量进行排序，并从高质量匹配点中随机抽样出最小样本集，使用数值法计算其基础矩阵fi。
[0091]
在本发明实例中，所述匹配点的质量为特征点到极线的距离大小，第一次的质量为匹配点经过比率测试的结果。
[0092]
2-2)使用自适应sprt算法判断基础矩阵fi是否满足成为最优基础矩阵f
best
的条件，满足则转至步骤2-3)，不满足则返回步骤2-1)，具体步骤为：
[0093]
2-2-1)构建基础矩阵fi使用自适应sprt算法的限定条件：
[0094][0095]
式中，和tv分别代表在使用和不使用自适应sprt时测试一个点是否为当前基础矩阵fi内点的时间，α是拒绝好模型的概率，ew(t)是已测试点的平均数；
[0096]
若满足，则转至步骤2-2-2)，若不满足，则转至步骤2-3)；
[0097]
2-2-2)计算自适应sprt的参数初始值：坏模型内点概率δ0和好模型内点概率ε0：
[0098][0099][0100]
[0101]
式中，t为匹配点的总数，前n次迭代中非最优基础矩阵对应的内点个数的平均值是当前最优基础矩阵f对应的内点个数，i
δ
是通过δ0和估计得到的非最优基础矩阵f对应的最大内点个数；
[0102]
令初始时刻j＝1，在最小样本集外随机抽取一个匹配点，计算基于wald的sprt似然比λj：
[0103][0104]
式中，hb与hg分别代表一个坏模型和一个好模型，p(xr|hb)为一个点是坏模型内点概率，p(xr|hg)为一个点是好模型内点概率；若数据点r为模型的内点，则xr＝1，否则xr＝0；
[0105]
若λj》a，则当前的基础矩阵为坏模型，返回步骤2-1)，其中a为决策阈值；
[0106]
若λj≤a，且j》n，则当前的基础矩阵是一个好模型，即满足成为最优基础矩阵的条件，转至步骤2-3)；
[0107]
若λj≤a，且j≤n，则重新抽取一个点，并将j自增1，继续计算基于wald的sprt似然比，直到j》n。
[0108]
在本发明实例中，模型估计时间tm通过前n次迭代中计算基础矩阵和相应内点数的时间均值，模型平均数量通过前n次迭代中每个特征点对应的有效基础矩阵个数的平均数，其中n＜＜n。决策阈值a的计算方式为：
[0109]an+1
＝k1/k2+1+log(an)
[0110]
其中，初值a0为k1/k2+1
[0111]
k1＝tm/pg[0112][0113][0114]
2-3)判断当前基础矩阵的最小样本集中是否满足有q个点共面的判定条件，若不满足判定条件，则当前基础矩阵不退化，并转至步骤2-4)；
[0115]
若满足判定条件，则随机在最小样本集外抽取样本，对当前的基础矩阵fi通过对极约束验证，判断其是否有满足有p个非主导平面外的点的限定条件，若满足限定条件，则当前基础矩阵不退化，若不满足限定条件，并转至步骤2-4)，则当前基础矩阵退化，舍弃当前的基础矩阵，返回步骤2-1)；
[0116]
在本发明实例中，q为最小样本集中共面点个数的阈值，取q＝5。
[0117]
2-4)计算基础矩阵fi对应的内点个数，若比当前最优f
best
的内点数多，则计算独立内点个数，若独立内点个数大于阈值将fi记为最优f
best
，并转至步骤2-5)，反之则返回步骤2-1)。
[0118]
在本发明实例中，独立内点的定义为：除去在最小样本集里的点、在独立内点附近的点、在一对匹配点极线附近的点，且满足点到极线距离小于阈值的点称为独立内点。
[0119]
独立内点的阈值计算方式：计算前n(n＜＜n)次迭代中得到的有效基础矩阵对应的独立内点个数(除去最优基础矩阵和最小样本集与最优矩阵重叠度较高的基础矩阵)的
中位数，记为
[0120]
将作为泊松分布的λ，计算泊松分布的95％百分位数，计算前n(n＜＜n)次迭代中得到的有效基础矩阵对应的独立内点个数小于95％百分位数的平均值，记为独立内点阈值
[0121]
2-5)判断当前最优f
best
是否满足局部优化条件，若满足局部优化条件，则使用f
best
的内点集对f
best
进行局部优化，若不满足局部优化条件返回步骤2-1)；
[0122]
所述局部优化的判断条件为：若独立内点个数大于阈值且只有当前的最优基础矩阵和上一个最优基础矩阵的最小样本集低于0.95jaccard指数，则满足进行局部优化的条件，反之则不满足进行局部优化的条件；
[0123]
所述局部优化的过程为：在最优基础矩阵对应的内点中随机选取大于最小样本集个数的点，使用解析法迭代计算出更好的基础矩阵；
[0124]
2-6)判断算法是否达到循环终止条件，若满足循环终止条件则转至步骤2-7)，若不满足循环终止条件则返回步骤2-1)，所述循环终止的判断条件为：η大于预设阈值其中η表示当前所有随机抽取的最小样本集中至少包含一个全部为内点的最小样本集的概率，其计算方式如下：
[0125]
η＝(1-pg(1-α))k[0126]
其中k为当前迭代次数，pg代表最小样本集中的m个点全为内点的概率，α代表拒绝一个好模型的概率，计算方式如下：
[0127]
pg＝εm[0128]
α≈1/a
[0129][0130]
2-7)对最优f
best
使用迭代最小二乘方法做最后的优化，得到最终的最优f
best
，并将最终的最优f
best
作为静态区域的基础矩阵f。
[0131]
3)使用基础矩阵f和对极约束计算动态区域匹配点的几何概率，具体步骤为：
[0132]
使用基础矩阵f计算动态区域内匹配点的极线再计算动态区域内匹配点到极线的距离
[0133][0134][0135]
式中，p
′i和pi为一对匹配点，x,y,z为极线的向量参数表示；
[0136]
将动态区域内匹配点到极线的距离使用二值sigmod函数转换为几何概率
[0137][0138]
式中，dth为静态点和动态点到极线距离的阈值。
[0139]
4)将动态区域连续两帧匹配点的几何概率融合，作为当前帧匹配点的最终几何概
率，具体步骤为：
[0140]
根据上一帧点p
′i状态概率的概率值预测当前帧点pi状态概率的概率值
[0141][0142]
根据上一帧点p
′i状态概率的方差预测当前帧点pi状态概率的方差
[0143][0144]
式中，δ0为匹配点之间的状态概率转移的标准差；
[0145]
计算卡尔曼增益k：
[0146][0147]
式中，δ1为关键点的观测状态概率标准差；
[0148]
根据点pi的观测状态概率更新点pi的状态概率：
[0149][0150]
式中，表示点pi的状态概率在当前帧的预测结果，表示点pi的状态概率通过卡尔曼滤波后的最终结果；
[0151]
更新点pi状态概率的方差：
[0152][0153]
式中，为当前帧点pi状态概率方差的预测值。
[0154]
在本发明实例中，定义连续两帧之间匹配点的概率传递满足马尔科夫性，即当前状态只和上一状态有关，基于卡尔曼滤波可得到滤波后的状态概率。
[0155]
5)对动态区域未匹配的点进行概率拓展，得到动态区域所有特征点的几何概率，并剔除不满足概率要求的特征点，将剩余的点传入后续跟踪和建图步骤，具体步骤为：
[0156]
根据相邻特征点的状态在大多数情况下是相似的思想，搜索动态区域内高置信度特征点周围与前一帧不匹配的相邻特征点pi，更新相邻特征点pi的状态概率
[0157][0158][0159]
式中，pj为与点pi相邻的高置信度特征点，为点pj的状态概率，为点pi观测状态概率；c是常数，λ(d)代表距离因子；
[0160]
若小于预设的阈值，则将pi划分为动态点；
[0161]
若大于阈值则划分为静态点，并剔除动态点，将动态区域中的静态点加入到静态区域的点集中，用于后续的定位和建图。
[0162]
本领域内的技术人员应明白，本技术的实施例可提供为方法、系统、或计算机程序
产品。因此，本技术可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本技术可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0163]
本技术是参照根据本技术实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0164]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0165]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0166]
最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。