1.一种动态场景下基于目标检测和几何概率的视觉SLAM方法,其特征在于,具体步骤如下:
1)采集待处理图像,使用目标检测算法将待处理图像划分为静态区域和动态区域;
2)使用VSAC算法计算静态区域匹配点的基础矩阵F;
3)使用基础矩阵F和对极约束计算动态区域匹配点的几何概率;
4)将动态区域连续两帧匹配点的几何概率融合,作为当前帧匹配点的最终几何概率;
5)对动态区域未匹配的点进行概率拓展,得到动态区域所有特征点的几何概率,并剔除不满足概率要求的特征点,将剩余的点传入后续跟踪和建图步骤;
步骤2)中使用VSAC算法计算静态区域匹配点基础矩阵F的具体步骤为:
2-1)对静态区域的特征点进行特征匹配,匹配点对数记为N,根据匹配点的质量进行排序,并从高质量匹配点中随机抽样出最小样本集,使用数值法计算其基础矩阵Fi;
2-2)使用自适应SPRT算法判断基础矩阵Fi是否满足成为最优基础矩阵Fbest的条件,满足则转至步骤2-3),不满足则返回步骤2-1);
2-3)判断当前基础矩阵的最小样本集中是否满足有Q个点共面的判定条件,若不满足判定条件,则当前基础矩阵不退化,并转至步骤2-4);
若满足判定条件,则随机在最小样本集外抽取样本,对当前的基础矩阵Fi通过对极约束验证,判断其是否有满足有P个非主导平面外的点的限定条件,若满足限定条件,则当前基础矩阵不退化,若不满足限定条件,并转至步骤2-4),则当前基础矩阵退化,舍弃当前的基础矩阵,返回步骤2-1);
2-4)计算基础矩阵Fi对应的内点个数,若比当前最优Fbest的内点数多,则计算独立内点个数,若独立内点个数大于阈值将Fi记为最优Fbest,并转至步骤2-5),反之则返回步骤2-1);
2-5)判断当前最优Fbest是否满足局部优化条件,若满足局部优化条件,则使用Fbest的内点集对Fbest进行局部优化,若不满足局部优化条件返回步骤2-1);
所述局部优化的判断条件为:若独立内点个数大于阈值且只有当前的最优基础矩阵和上一个最优基础矩阵的最小样本集低于0.95Jaccard指数,则满足进行局部优化的条件,反之则不满足进行局部优化的条件;
所述局部优化的过程为:在最优基础矩阵对应的内点中随机选取大于最小样本集个数的点,使用解析法迭代计算出更好的基础矩阵;
2-6)判断算法是否达到循环终止条件,若满足循环终止条件则转至步骤2-7),若不满足循环终止条件则返回步骤2-1),所述循环终止的判断条件为:η大于预设阈值其中η表示当前所有随机抽取的最小样本集中至少包含一个全部为内点的最小样本集的概率,其计算方式如下:η=(1-Pg(1-α))k
其中k为当前迭代次数,Pg代表最小样本集中的m个点全为内点的概率,α代表拒绝一个好模型的概率,计算方式如下:Pg=εm
α≈1/A
2-7)对最优Fbest使用迭代最小二乘方法做最后的优化,得到最终的最优Fbest,并将最终的最优Fbest作为静态区域的基础矩阵F;
步骤3)中使用基础矩阵F和对极约束计算动态区域匹配点的几何概率的具体步骤为:使用基础矩阵F计算动态区域内匹配点的极线再计算动态区域内匹配点到极线的距离式中,p′i和pi为一对匹配点,x,y,z为极线的向量参数表示;
将动态区域内匹配点到极线的距离使用二值Sigmod函数转换为几何概率式中,Dth为静态点和动态点到极线距离的阈值;
步骤4)中计算当前帧匹配点的最终几何概率的具体步骤为:根据上一帧点pi′状态概率的概率值预测当前帧点pi状态概率的概率值根据上一帧点pi′状态概率的方差预测当前帧点pi状态概率的方差式中,δ0为匹配点之间的状态概率转移的标准差;
计算卡尔曼增益k:
式中,δ1为关键点的观测状态概率标准差;
根据点pi的观测状态概率更新点pi的状态概率:
式中,表示点pi的状态概率在当前帧的预测结果,表示点pi的状态概率通过卡尔曼滤波后的最终结果;
更新点pi状态概率的方差:
式中,为当前帧点pi状态概率方差的预测值;
步骤5)中对动态区域未匹配的点进行概率拓展,得到动态区域所有特征点的几何概率,并剔除不满足概率要求特征点的具体步骤为:搜索动态区域内高置信度特征点周围与前一帧不匹配的相邻特征点pi,更新相邻特征点pi的状态概率式中,pj为与点pi相邻的高置信度特征点,为点pj的状态概率,为点pi观测状态概率;C是常数,λ(d)代表距离因子;
若小于预设的阈值,则将pi划分为动态点;
若大于阈值则划分为静态点,并剔除动态点,将动态区域中的静态点加入到静态区域的点集中,用于后续的定位和建图。
2.如权利要求1所述的一种动态场景下基于目标检测和几何概率的视觉SLAM方法,其特征在于,步骤1)中使用目标检测算法将待处理图像划分为静态区域和动态区域的具体步骤为:
1-1)使用Crowdhuman数据集对Yolov3 tiny目标检测网络进行训练,得到训练好的Yolov3 tiny网络;
1-2)使用训练好的Yolov3 tiny网络将待处理图像分为动态区域和静态区域,并提取待处理图像的特征点。
3.如权利要求1所述的一种动态场景下基于目标检测和几何概率的视觉SLAM方法,其特征在于,步骤2-2)中使用自适应SPRT算法判断基础矩阵Fi是否满足成为最优基础矩阵Fbest的条件的具体步骤为:
2-2-1)构建基础矩阵Fi使用自适应SPRT算法的限定条件:式中,和tv分别代表在使用和不使用自适应SPRT时测试一个点是否为当前基础矩阵Fi内点的时间,α是拒绝好模型的概率,Ew(T)是已测试点的平均数;
若满足,则转至步骤2-2-2),若不满足,则转至步骤2-3);
2-2-2)计算自适应SPRT的参数初始值:坏模型内点概率δ0和好模型内点概率ε0:式中,T为匹配点的总数,前n次迭代中非最优基础矩阵对应的内点个数的平均值是当前最优基础矩阵F对应的内点个数,Iδ是通过δ0和估计得到的非最优基础矩阵F对应的最大内点个数;
令初始时刻j=1,在最小样本集外随机抽取一个匹配点,计算基于Wald的SPRT似然比λj:式中,Hb与Hg分别代表一个坏模型和一个好模型,p(xr|Hb)为一个点是坏模型内点概率,p(xr|Hg)为一个点是好模型内点概率;若数据点r为模型的内点,则xr=1,否则xr=0;
若λj>A,则当前的基础矩阵为坏模型,返回步骤2-1),其中A为决策阈值;
若λj≤A,且j>N,则当前的基础矩阵是一个好模型,即满足成为最优基础矩阵的条件,转至步骤2-3);
若λj≤A,且j≤N,则重新抽取一个点,并将j自增1,继续计算基于Wald的SPRT似然比,直到j>N。