CN115471748A - 一种面向动态环境的单目视觉slam方法 - Google Patents

一种面向动态环境的单目视觉slam方法 Download PDF

Info

Publication number
CN115471748A
CN115471748A CN202211059723.XA CN202211059723A CN115471748A CN 115471748 A CN115471748 A CN 115471748A CN 202211059723 A CN202211059723 A CN 202211059723A CN 115471748 A CN115471748 A CN 115471748A
Authority
CN
China
Prior art keywords
frame
map
points
key frame
map points
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211059723.XA
Other languages
English (en)
Inventor
田联房
刘海林
杜启亮
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN202211059723.XA priority Critical patent/CN115471748A/zh
Publication of CN115471748A publication Critical patent/CN115471748A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/10—Terrestrial scenes
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/44—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
    • G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20016—Hierarchical, coarse-to-fine, multiscale or multiresolution image processing; Pyramid transform
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20112—Image segmentation details
    • G06T2207/20164—Salient point detection; Corner detection

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Multimedia (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computing Systems (AREA)
  • Databases & Information Systems (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种面向动态环境的单目视觉SLAM方法，包括：1)初始化，包括读取图像序列的前两帧、提取并匹配ORB特征点、建立世界坐标系、设置单目尺度信息、建立初始地图、构建关键帧序列以及关键帧滑动窗口；2)追踪参考帧并估计初始位姿；3)剔除动态特征点并优化位姿；4)根据追踪结果，***关键帧并追踪参考关键帧，然后构造地图点并***地图；5)剔除滑动窗口上动态地图点，并执行局部光束平差优化；6)剔除冗余关键帧和地图点；7)若设备算力充足，执行全局光束平差优化；8)重复步骤2)‑7)，直至序列中图像帧全部处理完。本发明解决了视觉SLAM方法应用于动态环境中定位与建图存在鲁棒性弱及精度差的问题。

Description

一种面向动态环境的单目视觉SLAM方法
技术领域
本发明涉及视觉同时定位与建图的技术领域，尤其是指一种面向动态环境的单目视觉SLAM方法。
背景技术
视觉同时定位与建图(Visual Simultaneous Localization and Mapping，简称视觉SLAM)是无人自主***的关键技术之一，其作用是使得智能机器人等在陌生环境中通过可见光相机感知周边环境信息而不依赖于外接设备，即可完成自身位姿的精确测量和环境地图的准确构建。除了智能机器人，涉及视觉SLAM的无人自主***还包括自动驾驶、智能无人机、AR/VR等先进技术领域。
当前，关于视觉SLAM研究的主要成果是基于静态环境的假设，这些方法在静态环境中可以获取良好的工作效果。但是，实际应用中不可避免地会出现运动物体，例如无人驾驶汽车不可能行驶在完全静态的高速公路上，智能机器人在执行任务过程中经常遇到行人、动物、车辆等动态物体。环境中出现的动态物体会导致现有基于静态假设的主流视觉SLAM方法性能下降，甚至无法工作。因此，缺乏对运动物体的考虑限制了当前主流基于静态环境假设视觉SLAM方法的实际应用。
综合以上论述，发明一种可工作于动态环境下的视觉SLAM方法，具有较高的实际应用价值。
发明内容
本发明的目的在于克服现有技术的缺点与不足，提出了一种面向动态环境的单目视觉SLAM方法，根据运动一致性的原理进行动态特征点识别与剔除，进而通过静态特征点估计相机位姿并构建稀疏地图。该方法分为两个线程执行：追踪线程和建图线程。在追踪线程中，主要完成ORB特征提取与匹配、动态特征点过滤、初始位姿估计和关键帧决策，其中的核心环节是动态特征点过滤，通过比较当前帧与上一帧匹配特征点的几何信息和光度信息一致性进行快速运动物体的特征点识别与剔除。建图线程是在追踪线程***关键帧之后执行，主要完成动态地图点剔除、静态地图构建和全局光束平差优化，其中的核心环节是动态地图点剔除，通过比较滑动窗口中关键帧上地图点的几何信息一致性进行缓慢运动物体上的动态地图点识别与剔除。通过以上两处的动态点识别与剔除操作，有效减少了动态特征点对视觉SLAM方法的影响，并且仅利用静态特征点信息完成了位姿估计和稀疏地图构建，提升了视觉SLAM方法在动态环境中的鲁棒性及精度。
为实现上述目的，本发明所提供的技术方案为：一种面向动态环境的单目视觉SLAM方法，包括以下步骤：
1)初始化，包括读取图像序列的前两帧、提取并匹配ORB特征点、建立世界坐标系、设置单目尺度信息、建立初始地图、构建关键帧序列以及关键帧滑动窗口；
2)读取图像序列作为当前帧，将上一帧设为当前帧的参考帧，完成当前帧与参考帧的特征匹配并估计相对位姿Tk-1,k，其中，当前帧为第k帧，参考帧为第k-1帧；
3)根据步骤2)中的相对位姿，比较当前帧与参考帧匹配特征点对的几何信息一致性和光度信息一致性，剔除不一致的匹配特征点对以及对应的地图点，并根据剩余的匹配特征点对，通过最小化重投影误差函数优化当前帧与参考帧的相对位姿；
4)若步骤3)中剩余的匹配特征点对数小于设置的阈值Tm，或者当前帧与当前关键帧间隔帧数超过阈值Tn，则根据当前帧构造新的关键帧；
5)若步骤4)构造了新的关键帧，则将新的关键帧***关键帧序列和滑动窗口，将新的关键帧设为当前关键帧，将上一关键帧设为当前关键帧的参考关键帧；然后根据步骤3)优化后的相机位姿，将当前关键帧与参考关键帧进行特征点匹配，其中，对于未初始化地图点的匹配特征点对，则通过三角化方法构造新的地图点并***地图M；
6)根据几何信息一致性，识别并剔除滑动窗口上的动态地图点，然后在滑动窗上进行局部光束平差优化；
7)若滑动窗口中关键帧个数达到最大值TK，并且当前关键帧位姿Tn,w与参考关键帧位姿Tn-1,w满足
Figure BDA0003823761050000031
则剔除滑动窗口中的参考关键帧，若
Figure BDA0003823761050000032
则剔除滑动窗口上时间最早的关键帧，其中，运算符||·||表示取矩阵范数，δt为设置的阈值；随后遍历全部关键帧及地图点，若关键帧所观测到地图点的90％以上都被至少三个其它关键帧所观测，则剔除该关键帧，若地图点被观测次数少于三次，则剔除该地图点；
8)完成步骤7)后，新开启一条线程进行一次全局光束平差优化；重复步骤2)至步骤8)，直到序列中的图像全部处理完。
进一步，在步骤1)中，将读取的RGB图像转化为灰度图，将灰度图连续降采样构造图像金字塔，根据每层图像分辨率大小提取相应数量的FAST角点，并计算全部角点的BRIEF描述子，根据描述子之间的汉明距离，在两帧之间进行特征点匹配；随后将世界坐标系设为第一帧的位姿，并通过随机采样一致性计算第二帧的位姿，具体包括以下步骤：
1.1)随机选取8对匹配的特征点，利用八点法计算本质矩阵E12；
1.2)根据步骤1.1)的结果，检验全部匹配特征点对(qi1,qi2)的对极约束偏差
Figure BDA0003823761050000033
若ed＞Td，则将匹配特征点对标记为离群值(outlier)，否则标记为内围值(inlier)，其中，qi1和qi2分别是第一帧和第二帧特征点的齐次坐标，K是相机内参矩阵，Td是设置的阈值；
1.3)根据步骤1.2)的结果，若内围值匹配特征点对数占总匹配特征点对数比例超过阈值Tr，0.7＜Tr≤1，则对步骤1.1)的本质矩阵进行奇异值分解(SVD)，并得到第一帧与第二帧的相对位姿T12，其中，令位姿中的平移向量为单位向量，即确定了尺度信息；若内围值比例低于Tr，则重复步骤1.1)至1.3)直到内围值比例不低于Tr；若全部组合选取完，仍未找到满足内围值比例大于Tr的组合，则选择内围值比例最大的组合对应的本质矩阵进行奇异值分解，并得到T12；
1.4)根据步骤1.3)的结果，将全部内围值进行三角化(Triangulation)计算得到三维地图点，并构建初始地图M；根据第一帧构造关键帧KF1，***到关键帧序列和滑动窗口，将该关键帧设为当前关键帧。
进一步，在步骤2)中，首先将读取的RGB图转换为灰度图，连续降采样构造图像金字塔，根据每层图像的分辨率大小提取相应数量的FAST角点，并计算全部角点的BRIEF描述子；然后将提取的每个特征点通过BRIEF描述子在参考帧中寻找最佳匹配，若最佳匹配的汉明距离小于阈值To，则保留该特征点匹配，否则舍弃该特征点匹配；最后根据随机采样一致性策略，利用Epnp算法计算当前帧与参考帧之间的相对位姿Tk-1,k，具体包括以下步骤：
2.1)随机选取4对匹配特征点，通过Epnp算法求解出位姿Tk-1,k；
2.2)计算当前帧特征点重投影误差ei：
Figure BDA0003823761050000041
式中，i表示地图点的索引号，N表示三维地图点数，Pi k-1为参考帧坐标系地图点，
Figure BDA0003823761050000042
为当前帧特征点，π(·)为去齐次运算；
2.3)若||ei||＞Tl，则该匹配特征点对标记为离群值，否则标记为内围值；计算内围值的总占比，若大于Tn，则保留步骤2.1)的位姿，否则，重复步骤2.1)至2.3)直至内围值总占比大于Tn；若选取完全部组合，仍无法满足条件，则选择内围值比例最高的位姿作为估计位姿，其中，Tl和Tn是设置的阈值。
进一步，在步骤3)中，为了消除快速运动物体上特征点的影响，通过光度信息一致性和几何信息一致性进行动态特征点识别与剔除，并优化当前帧与参考帧的相对位姿，具体包括以下步骤：
3.1)根据步骤2)中标记为内围值的特征点对，计算特征点附近图像块的像素值，若特征点对的区域像素值差值大于阈值Tg，则将相应的匹配特征点对标记为动态特征点并移除该匹配特征点对及其对应的地图点；
3.2)对于经过步骤3.1)检验并具有三维地图点的特征点对，将对应的地图点投影到当前帧和参考帧中并构造重投影误差函数，最后通过最小化重投影误差函数来迭代优化地图点的空间坐标，具体表达式如下：
Figure BDA0003823761050000051
上式通过高斯牛顿法迭代求解；在优化过程中，若偏差满足
Figure BDA0003823761050000052
则将Pl k-1标记为动态地图点，进而移除该地图点以及相应匹配特征点对，式中，l表示检验之后地图点的索引号，l＝1,2,...,N′，arg min(·)表示函数取最小值时的参数取值，π(·)表示去齐次变换，Tk-1,k是当前帧与参考帧的相对位姿，
Figure BDA0003823761050000053
和
Figure BDA0003823761050000054
分别是参考帧和当前帧的特征点，Pl k是第k帧坐标系地图点，N′是检验之后剩余的地图点个数，ΔPl k-1表示优化前后地图点Pl k-1位置的变化量，Ωl为地图点Pl k-1的协方差矩阵，Th为设置的阈值，K是相机内参；
3.3)根据步骤3.2)过滤后的匹配特征点对，通过求解重投影误差函数优化相对位姿，具体表示为：
Figure BDA0003823761050000061
上式通过高斯牛顿法迭代求解；式中，u表示过滤后地图点的索引号，N”为过滤后的地图点个数。
进一步，在步骤5)中，将当前关键帧的特征点与参考关键帧的特征点利用BRIEF描述子进行匹配，具体包括以下步骤：
5.1)根据步骤3)优化后的相对位姿，通过累乘的方式得到当前关键帧与参考关键帧的相对位姿，进而计算两个关键帧之间的本质矩阵E＝t^R，其中，t是平移向量，R是旋转矩阵，符号∧表示反对称变换；
5.2)根据步骤5.1)的本质矩阵，将当前关键帧中的特征点二维像素坐标通过对极约束计算得到参考关键帧图像上对应的二维坐标点，并在计算得到的二维坐标点附近通过BRIEF描述子寻找最佳的匹配特征点对，若最佳匹配特征点对的BRIEF描述子汉明距离小于阈值Tk，则保留该匹配结果，否则舍弃该匹配结果。
进一步，在步骤6)中，通过非线性优化的方式检验三维地图点的空间坐标一致性，识别并剔除运动慢的动态地图点，并进行局部光束平差优化，具体包括以下分步骤：
6.1)将滑动窗口上的全部地图点投影到被观测到的关键帧上获得二维投影坐标，构造重投影误差函数e：
Figure BDA0003823761050000062
式中，m表示滑动窗口上地图点的索引号，m＝1,2,...,Na，j表示滑动窗口上关键帧的索引号，j＝1,2,...,Ns，Ns为滑动窗口中关键帧数量，Na为滑动窗口中地图点数量，δmj表示第m个地图点与第j个关键帧构造的重投影误差，具体的表达式为
Figure BDA0003823761050000071
Tj,w为世界坐标系到第j个关键帧的相对位姿，
Figure BDA0003823761050000072
为世界坐标系地图点，
Figure BDA0003823761050000073
表示第j个关键帧上的特征点，π(·)表示去齐次变换，K是相机内参；
6.2)将步骤6.1)的误差函数作为最小化的目标函数，迭代优化三维地图点坐标，具体表达式为：
Figure BDA0003823761050000074
上式通过高斯牛顿法迭代求解；在优化过程中，若偏差满足
Figure BDA0003823761050000075
则将
Figure BDA0003823761050000076
标记为动态地图点，进而移除该地图点及相应特征点，其中，argmin(·)表示函数取最小值时的参数取值，
Figure BDA0003823761050000077
是世界坐标系地图点，
Figure BDA0003823761050000078
表示优化前后地图点
Figure BDA0003823761050000079
位置的变化量，Ωmw为地图点
Figure BDA00038237610500000710
的协方差矩阵，Thw为设置的阈值大小；
6.3)通过最小化重投影误差函数来优化滑动窗口上的关键帧位姿和地图点坐标，具体表示为：
Figure BDA00038237610500000711
上式通过高斯牛顿法求解。
进一步，在步骤8)中，开启一条新的线程对全部关键帧位姿和地图点位置进行优化，具体表达式如下：
Figure BDA00038237610500000712
上式通过高斯牛顿法迭代求解；式中，g表示地图中关键帧的索引号，g＝1,2,...,Nsw，b表示地图中地图点的索引号，b＝1,2,...,Naw，Tg,w表示世界坐标系到第g个关键帧的相对位姿，
Figure BDA0003823761050000081
表示地图点坐标，argmin(·)表示函数取最小值时的参数取值，e′是待优化的重投影误差函数，其具体表达式为
Figure BDA0003823761050000082
δbg是地图点
Figure BDA0003823761050000083
与关键帧Tg,w构造的重投影误差，具体表达式为
Figure BDA0003823761050000084
if关键帧g观测到地图点
Figure BDA0003823761050000085
π(·)为去齐次变换，K是相机内参，
Figure BDA0003823761050000086
为第g个关键帧的特征点，Nsw为关键帧数，Naw为地图点数，
Figure BDA0003823761050000087
为世界坐标系地图点。
本发明与现有技术相比，具有如下优点与有益效果：
1、本发明是适用于动态环境下的单目视觉SLAM方法。当前主流的视觉SLAM方法都是基于静态环境的假设，较少关注动态物体造成的影响。
2、本发明使用单目相机作为环境感知设备，在室内外环境都能良好工作。目前很多针对于动态环境的视觉SLAM方法的技术发明都是基于RGB-D相机，因为这些技术必须用到相机提供的深度图像。与这类技术相比，本发明的新颖之处在于动态特征点的识别策略不同。本发明通过比较特征点对的光度值差异以及对应地图点在优化过程中的较大波动性，来识别动态特征点，而其它的方法经常需要直接比较深度值的绝对差异。
3、本发明使用的单目相机具有价格低、体积小、质量轻的优点，同时本发明也可以推广到双目相机、RGB-D相机中。
4、本发明在追踪线程和建图线程设计了不同的动态地图点识别和剔除策略，综合保证了视觉SLAM的鲁棒性和运行效率。
附图说明
图1为本发明方法实施例的流程框图。
具体实施方式
下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。
本实施例公开了一种面向动态环境的单目视觉SLAM方法，如图1所示，该方法分为两个线程执行：追踪线程和建图线程，在追踪线程中，主要完成单目视觉SLAM初始化、ORB特征提取与匹配、动态特征点过滤、初始位姿估计和关键帧决策，其中的核心环节是动态特征点过滤，通过比较当前帧与上一帧匹配特征点的几何信息和光度信息一致性进行快速运动物体的特征点识别与剔除。建图线程是在追踪线程***关键帧之后执行，主要完成动态地图点剔除、静态地图构建和全局光束平差优化，其中的核心环节是动态地图点剔除，通过比较滑动窗口中关键帧上地图点的几何信息一致性进行缓慢运动物体上的动态地图点识别与剔除。具体流程如下：
一、追踪线程：
1)初始化，包括读取图像序列的前两帧、提取并匹配ORB特征点、建立世界坐标系、设置单目尺度信息、建立初始地图、构建关键帧序列以及关键帧滑动窗口，具体情况如下：
将读取的RGB图像转化为灰度图，将灰度图连续降采样构造图像金字塔，根据每层图像分辨率大小提取相应数量的FAST角点，并计算全部角点的BRIEF描述子，根据描述子之间的汉明距离，在两帧之间进行特征点匹配；随后将世界坐标系设为第一帧的位姿，并通过随机采样一致性计算第二帧的位姿，具体包括以下步骤：
1.1)随机选取8对匹配的特征点，利用八点法计算本质矩阵E12；
1.2)根据步骤1.1)的结果，检验全部匹配特征点对(qi1,qi2)的对极约束偏差
Figure BDA0003823761050000101
若ed＞Td，则将匹配特征点对标记为离群值(outlier)，否则标记为内围值(inlier)，其中，qi1和qi2分别是第一帧和第二帧特征点的齐次坐标，K是相机内参矩阵，Td是设置的阈值；
1.3)根据步骤1.2)的结果，若内围值匹配特征点对数占总匹配特征点对数比例超过阈值Tr，0.7＜Tr≤1(优选0.8)，则对步骤1.1)的本质矩阵进行奇异值分解(SVD)，并得到第一帧与第二帧的相对位姿T12，其中，令位姿中的平移向量为单位向量，即确定了尺度信息；若内围值比例低于Tr，则重复步骤1.1)至1.3)直到内围值比例不低于Tr；若全部组合选取完，仍未找到满足内围值比例大于Tr的组合，则选择内围值比例最大的组合对应的本质矩阵进行奇异值分解，并得到T12；
1.4)根据步骤1.3)的结果，将全部内围值进行三角化(Triangulation)计算得到三维地图点，并构建初始地图M；根据第一帧构造关键帧KF1，***到关键帧序列和滑动窗口(滑动窗口长度设置为20)，将该关键帧设为当前关键帧。
2)读取图像序列作为当前帧，将上一帧设为当前帧的参考帧，完成当前帧与参考帧的特征匹配并估计相对位姿Tk-1,k，其中，当前帧为第k帧，参考帧为第k-1帧，具体如下：
首先将读取的RGB图转换为灰度图，采用1.2的缩放因子，连续降采样7次，构造8层图像金字塔；根据每层图像的分辨率大小提取相应数量的FAST角点，总共提取1000个FAST角点，并计算全部角点的BRIEF描述子；然后将提取的每个特征点通过BRIEF描述子在参考帧中寻找最佳匹配，若最佳匹配的汉明距离小于阈值To，则保留该特征点匹配，否则舍弃该特征点匹配；最后根据随机采样一致性策略，利用Epnp算法计算当前帧与参考帧之间的相对位姿Tk-1,k，具体包括以下步骤：
2.1)随机选取4对匹配特征点，通过Epnp算法求解出位姿Tk-1,k；
2.2)计算当前帧特征点重投影误差ei：
Figure BDA0003823761050000111
式中，i表示地图点的索引号，N表示三维地图点数，Pi k-1为参考帧坐标系地图点，
Figure BDA0003823761050000112
为当前帧特征点，π(·)为去齐次运算；
2.3)根据步骤2.2)的结果，若||ei||＞Tl(优选5)，则该匹配特征点对标记为离群值，否则标记为内围值；计算内围值的总占比，若大于Tn(优选0.8)，则保留步骤2.1)的位姿，否则，重复步骤2.1)至2.3)直至内围值总占比大于Tn；若选取完全部组合，仍无法满足条件，则选择内围值比例最高的位姿作为估计位姿，其中，Tl和Tn是设置的阈值。
3)根据步骤2)中的相对位姿，比较当前帧与参考帧匹配特征点对的几何信息一致性和光度信息一致性，剔除不一致的匹配特征点对以及对应的地图点，并根据剩余的匹配特征点对，通过最小化重投影误差函数优化当前帧与参考帧的相对位姿，具体步骤如下：
3.1)根据步骤2)中标记为内围值的特征点对，计算以特征点为中心的5×5像素块的区域灰度值总和，若特征点对的区域像素值差值大于阈值Tg，则将相应的匹配特征点对标记为动态特征点并移除该匹配特征点对及其对应的地图点；
3.2)对于经过步骤3.1)检验并具有三维地图点的特征点对，将对应的地图点投影到当前帧和参考帧中并构造重投影误差函数，最后通过最小化重投影误差函数来迭代优化地图点的空间坐标，具体表达式如下：
Figure BDA0003823761050000121
上式通过高斯牛顿法迭代求解；在优化过程中，若偏差满足
Figure BDA0003823761050000122
则将Pl k-1标记为动态地图点，进而移除该地图点以及相应匹配特征点对，式中，l表示检验之后地图点的索引号，l＝1,2,...,N′，arg min(·)表示函数取最小值时的参数取值，π(·)表示去齐次变换，Tk-1,k是当前帧与参考帧的相对位姿，
Figure BDA0003823761050000123
和
Figure BDA0003823761050000124
分别是参考帧和当前帧的特征点，Pl k是第k帧坐标系地图点，N′是检验之后剩余的地图点个数，ΔPl k-1表示优化前后地图点Pl k-1位置的变化量，Ωl为地图点Pl k-1的协方差矩阵，Th为设置的阈值，K是相机内参；
3.3)根据步骤3.2)过滤后的匹配特征点对，通过求解重投影误差函数优化相对位姿，具体表示为：
Figure BDA0003823761050000125
上式通过高斯牛顿法迭代求解；式中，u表示过滤后地图点的索引号，N”为过滤后的地图点个数。
4)若步骤3)中剩余的静态匹配特征点对数少于50个，或者当前帧与当前关键帧间隔帧数超过10，则根据当前帧构造新的关键帧。
二、建图线程：
5)若步骤4)构造了新的关键帧，则将新的关键帧***关键帧序列和滑动窗口，将新的关键帧设为当前关键帧，将上一关键帧设为当前关键帧的参考关键帧；根据步骤3)优化后的相对位姿，通过累乘的方式得到当前关键帧与参考关键帧的相对位姿，进而计算两个关键帧之间的本质矩阵E＝t^R，其中，t是平移向量，R是旋转矩阵，符号∧表示反对称变换；根据本质矩阵，将当前关键帧中的特征点二维像素坐标通过对极约束计算得到参考关键帧图像上对应的二维坐标点，并在计算得到的二维坐标点附近通过BRIEF描述子寻找最佳的匹配特征点对，若最佳匹配特征点对的BRIEF描述子汉明距离小于阈值Tk，则保留该匹配结果，否则舍弃该匹配结果；对于未初始化地图点的匹配对，通过三角化方法构造新的地图点，并***地图M。
6)根据几何信息一致性，通过非线性优化的方式检验三维地图点的空间坐标一致性，识别并剔除运动慢的动态地图点，并进行局部光束平差优化，具体步骤如下：
6.1)将滑动窗口上的全部地图点投影到被观测到的关键帧上获得二维投影坐标，构造重投影误差函数e：
Figure BDA0003823761050000131
式中，m表示滑动窗口上地图点的索引号，m＝1,2,...,Na，j表示滑动窗口上关键帧的索引号，j＝1,2,...,Ns，Ns为滑动窗口中关键帧数量，Na为滑动窗口中地图点数量，δmj表示第m个地图点与第j个关键帧构造的重投影误差，具体的表达式为
Figure BDA0003823761050000132
if关键帧j观测到地图点Pi w，Tj,w为世界坐标系到第j个关键帧的相对位姿，
Figure BDA0003823761050000133
为世界坐标系地图点，
Figure BDA0003823761050000134
表示第j个关键帧上的特征点，π(·)表示去齐次变换，K是相机内参；
6.2)将步骤6.1)的误差函数作为最小化的目标函数，迭代优化三维地图点坐标，具体表达式为：
Figure BDA0003823761050000135
上式通过高斯牛顿法迭代求解；在优化过程中，若偏差满足
Figure BDA0003823761050000141
则将
Figure BDA0003823761050000142
标记为动态地图点，进而移除该地图点及相应特征点，其中，arg min(·)表示函数取最小值时的参数取值，
Figure BDA0003823761050000143
是世界坐标系地图点，
Figure BDA0003823761050000144
表示优化前后地图点
Figure BDA0003823761050000145
位置的变化量，Ωmw为地图点
Figure BDA0003823761050000146
的协方差矩阵，Thw为设置的阈值大小；
6.3)通过最小化重投影误差函数来优化滑动窗口上的关键帧位姿和地图点坐标，具体表示为：
Figure BDA0003823761050000147
上式通过高斯牛顿法求解。
7)若滑动窗口中关键帧个数达到20，并且当前关键帧位姿Tn,w接近参考关键帧位姿Tn-1,w，即
Figure BDA0003823761050000148
则剔除滑动窗口中的参考关键帧；若滑动窗口中关键帧个数达到20，且当前关键帧位与参考关键帧位姿差距较大，即
Figure BDA0003823761050000149
则剔除滑动窗口上时间最早的关键帧；遍历关键帧序列中的全部关键帧，若关键帧所观测到地图点的90％以上都被至少三个其它关键帧所观测，则剔除该关键帧；遍历地图，若地图点观测次数少于三次，则剔除该地图点。
8)完成步骤7)后，新开启一条线程进行一次全局光束平差优化，具体表达式为：
Figure BDA00038237610500001410
上式通过高斯牛顿法迭代求解；式中，g表示地图中关键帧的索引号，g＝1,2,...,Nsw，b表示地图中地图点的索引号，b＝1,2,...,Naw，Tg,w表示世界坐标系到第g个关键帧的相对位姿，
Figure BDA00038237610500001411
表示地图点坐标，arg min(·)表示函数取最小值时的参数取值，e′是待优化的重投影误差函数，其具体表达式为
Figure BDA0003823761050000151
δbg是地图点
Figure BDA0003823761050000152
与关键帧Tg,w构造的重投影误差，具体表达式为
Figure BDA0003823761050000153
if关键帧g观测到地图点
Figure BDA0003823761050000154
π(·)为去齐次变换，K是相机内参，
Figure BDA0003823761050000155
为第g个关键帧的特征点，Nsw为关键帧数，Naw为地图点数，
Figure BDA0003823761050000156
为世界坐标系地图点；重复步骤2)至步骤8)，直到序列中的图像帧全部处理完。
最终，本实施例上述面向动态环境的单目视觉SLAM方法在动态环境中获得了全局一致的相机位姿估计，并构建了稀疏地图。
综上所述，本发明通过以上两处的动态点识别与剔除操作，有效减少了动态特征点对视觉SLAM方法的影响，并且仅利用静态特征点信息完成了位姿估计和稀疏地图构建，提升了视觉SLAM方法在动态环境中的鲁棒性及精度，解决了视觉SLAM方法应用于动态环境中定位与建图存在鲁棒性弱及精度差的问题，值得推广。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (7)

1.一种面向动态环境的单目视觉SLAM方法，其特征在于，包括以下步骤：
1)初始化，包括读取图像序列的前两帧、提取并匹配ORB特征点、建立世界坐标系、设置单目尺度信息、建立初始地图、构建关键帧序列以及关键帧滑动窗口；
2)读取图像序列作为当前帧，将上一帧设为当前帧的参考帧，完成当前帧与参考帧的特征匹配并估计相对位姿Tk-1,k，其中，当前帧为第k帧，参考帧为第k-1帧；
3)根据步骤2)中的相对位姿，比较当前帧与参考帧匹配特征点对的几何信息一致性和光度信息一致性，剔除不一致的匹配特征点对以及对应的地图点，并根据剩余的匹配特征点对，通过最小化重投影误差函数优化当前帧与参考帧的相对位姿；
4)若步骤3)中剩余的匹配特征点对数小于设置的阈值Tm，或者当前帧与当前关键帧间隔帧数超过阈值Tn，则根据当前帧构造新的关键帧；
5)若步骤4)构造了新的关键帧，则将新的关键帧***关键帧序列和滑动窗口，将新的关键帧设为当前关键帧，将上一关键帧设为当前关键帧的参考关键帧；然后根据步骤3)优化后的相机位姿，将当前关键帧与参考关键帧进行特征点匹配，其中，对于未初始化地图点的匹配特征点对，则通过三角化方法构造新的地图点并***地图M；
6)根据几何信息一致性，识别并剔除滑动窗口上的动态地图点，然后在滑动窗上进行局部光束平差优化；
7)若滑动窗口中关键帧个数达到最大值TK，并且当前关键帧位姿Tn,w与参考关键帧位姿Tn-1,w满足
Figure FDA0003823761040000011
则剔除滑动窗口中的参考关键帧，若
Figure FDA0003823761040000021
则剔除滑动窗口上时间最早的关键帧，其中，运算符||·||表示取矩阵范数，δt为设置的阈值；随后遍历全部关键帧及地图点，若关键帧所观测到地图点的90％以上都被至少三个其它关键帧所观测，则剔除该关键帧，若地图点被观测次数少于三次，则剔除该地图点；
8)完成步骤7)后，新开启一条线程进行一次全局光束平差优化；重复步骤2)至步骤8)，直到序列中的图像全部处理完。
2.根据权利要求1所述的一种面向动态环境的单目视觉SLAM方法，其特征在于，在步骤1)中，将读取的RGB图像转化为灰度图，将灰度图连续降采样构造图像金字塔，根据每层图像分辨率大小提取相应数量的FAST角点，并计算全部角点的BRIEF描述子，根据描述子之间的汉明距离，在两帧之间进行特征点匹配；随后将世界坐标系设为第一帧的位姿，并通过随机采样一致性计算第二帧的位姿，具体包括以下步骤：
1.1)随机选取8对匹配的特征点，利用八点法计算本质矩阵E12；
1.2)根据步骤1.1)的结果，检验全部匹配特征点对(qi1,qi2)的对极约束偏差
Figure FDA0003823761040000022
若ed＞Td，则将匹配特征点对标记为离群值，否则标记为内围值，其中，qi1和qi2分别是第一帧和第二帧特征点的齐次坐标，K是相机内参矩阵，Td是设置的阈值；
1.3)根据步骤1.2)的结果，若内围值匹配特征点对数占总匹配特征点对数比例超过阈值Tr，0.7＜Tr≤1，则对步骤1.1)的本质矩阵进行奇异值分解，并得到第一帧与第二帧的相对位姿T12，其中，令位姿中的平移向量为单位向量，即确定了尺度信息；若内围值比例低于Tr，则重复步骤1.1)至1.3)直到内围值比例不低于Tr；若全部组合选取完，仍未找到满足内围值比例大于Tr的组合，则选择内围值比例最大的组合对应的本质矩阵进行奇异值分解，并得到T12；
1.4)根据步骤1.3)的结果，将全部内围值进行三角化计算得到三维地图点，并构建初始地图M；根据第一帧构造关键帧KF1，***到关键帧序列和滑动窗口，将该关键帧设为当前关键帧。
3.根据权利要求2所述的一种面向动态环境的单目视觉SLAM方法，其特征在于，在步骤2)中，首先将读取的RGB图转换为灰度图，连续降采样构造图像金字塔，根据每层图像的分辨率大小提取相应数量的FAST角点，并计算全部角点的BRIEF描述子；然后将提取的每个特征点通过BRIEF描述子在参考帧中寻找最佳匹配，若最佳匹配的汉明距离小于阈值To，则保留该特征点匹配，否则舍弃该特征点匹配；最后根据随机采样一致性策略，利用Epnp算法计算当前帧与参考帧之间的相对位姿Tk-1,k，具体包括以下步骤：
2.1)随机选取4对匹配特征点，通过Epnp算法求解出位姿Tk-1,k；
2.2)计算当前帧特征点重投影误差ei：
Figure FDA0003823761040000031
式中，i表示地图点的索引号，N表示三维地图点数，Pi k-1为参考帧坐标系地图点，
Figure FDA0003823761040000032
为当前帧特征点，π(·)为去齐次运算；
2.3)若||ei||＞Tl，则该匹配特征点对标记为离群值，否则标记为内围值；计算内围值的总占比，若大于Tn，则保留步骤2.1)的位姿，否则，重复步骤2.1)至2.3)直至内围值总占比大于Tn；若选取完全部组合，仍无法满足条件，则选择内围值比例最高的位姿作为估计位姿，其中，Tl和Tn是设置的阈值。
4.根据权利要求3所述的一种面向动态环境的单目视觉SLAM方法，其特征在于，在步骤3)中，为了消除快速运动物体上特征点的影响，通过光度信息一致性和几何信息一致性进行动态特征点识别与剔除，并优化当前帧与参考帧的相对位姿，具体包括以下步骤：
3.1)根据步骤2)中标记为内围值的特征点对，计算特征点附近图像块的像素值，若特征点对的区域像素值差值大于阈值Tg，则将相应的匹配特征点对标记为动态特征点并移除该匹配特征点对及其对应的地图点；
3.2)对于经过步骤3.1)检验并具有三维地图点的特征点对，将对应的地图点投影到当前帧和参考帧中并构造重投影误差函数，最后通过最小化重投影误差函数来迭代优化地图点的空间坐标，具体表达式如下：
Figure FDA0003823761040000041
上式通过高斯牛顿法迭代求解；在优化过程中，若偏差满足
Figure FDA0003823761040000042
则将Pl k-1标记为动态地图点，进而移除该地图点以及相应匹配特征点对，式中，l表示检验之后地图点的索引号，l＝1,2,...,N′，arg min(·)表示函数取最小值时的参数取值，π(·)表示去齐次变换，Tk-1,k是当前帧与参考帧的相对位姿，
Figure FDA0003823761040000043
和
Figure FDA0003823761040000044
分别是参考帧和当前帧的特征点，Pl k是第k帧坐标系地图点，N′是检验之后剩余的地图点个数，ΔPl k-1表示优化前后地图点Pl k-1位置的变化量，Ωl为地图点Pl k-1的协方差矩阵，Th为设置的阈值，K是相机内参；
3.3)根据步骤3.2)过滤后的匹配特征点对，通过求解重投影误差函数优化相对位姿，具体表示为：
Figure FDA0003823761040000045
上式通过高斯牛顿法迭代求解；式中，u表示过滤后地图点的索引号，N”为过滤后的地图点个数。
5.根据权利要求4所述的一种面向动态环境的单目视觉SLAM方法，其特征在于，在步骤5)中，将当前关键帧的特征点与参考关键帧的特征点利用BRIEF描述子进行匹配，具体包括以下步骤：
5.1)根据步骤3)优化后的相对位姿，通过累乘的方式得到当前关键帧与参考关键帧的相对位姿，进而计算两个关键帧之间的本质矩阵E＝t^R，其中，t是平移向量，R是旋转矩阵，符号^表示反对称变换；
5.2)根据步骤5.1)的本质矩阵，将当前关键帧中的特征点二维像素坐标通过对极约束计算得到参考关键帧图像上对应的二维坐标点，并在计算得到的二维坐标点附近通过BRIEF描述子寻找最佳的匹配特征点对，若最佳匹配特征点对的BRIEF描述子汉明距离小于阈值Tk，则保留该匹配结果，否则舍弃该匹配结果。
6.根据权利要求5所述的一种面向动态环境的单目视觉SLAM方法，其特征在于，在步骤6)中，通过非线性优化的方式检验三维地图点的空间坐标一致性，识别并剔除运动慢的动态地图点，并进行局部光束平差优化，具体包括以下分步骤：
6.1)将滑动窗口上的全部地图点投影到被观测到的关键帧上获得二维投影坐标，构造重投影误差函数e：
Figure FDA0003823761040000051
式中，m表示滑动窗口上地图点的索引号，m＝1,2,...,Na，j表示滑动窗口上关键帧的索引号，j＝1,2,...,Ns，Ns为滑动窗口中关键帧数量，Na为滑动窗口中地图点数量，δmj表示第m个地图点与第j个关键帧构造的重投影误差，具体的表达式为
Figure FDA0003823761040000052
if关键帧j观测到地图点Pi w，Tj,w为世界坐标系到第j个关键帧的相对位姿，
Figure FDA0003823761040000061
为世界坐标系地图点，
Figure FDA0003823761040000062
表示第j个关键帧上的特征点，π(·)表示去齐次变换，K是相机内参；
6.2)将步骤6.1)的误差函数作为最小化的目标函数，迭代优化三维地图点坐标，具体表达式为：
Figure FDA0003823761040000063
上式通过高斯牛顿法迭代求解；在优化过程中，若偏差满足
Figure FDA0003823761040000064
则将
Figure FDA0003823761040000065
标记为动态地图点，进而移除该地图点及相应特征点，其中，argmin(·)表示函数取最小值时的参数取值，
Figure FDA0003823761040000066
是世界坐标系地图点，
Figure FDA0003823761040000067
表示优化前后地图点
Figure FDA0003823761040000068
位置的变化量，Ωmw为地图点
Figure FDA0003823761040000069
的协方差矩阵，Thw为设置的阈值大小；
6.3)通过最小化重投影误差函数来优化滑动窗口上的关键帧位姿和地图点坐标，具体表示为：
Figure FDA00038237610400000610
上式通过高斯牛顿法求解。
7.根据权利要求6所述的一种面向动态环境的单目视觉SLAM方法，其特征在于，在步骤8)中，开启一条新的线程对全部关键帧位姿和地图点位置进行优化，具体表达式如下：
Figure FDA00038237610400000611
上式通过高斯牛顿法迭代求解；式中，g表示地图中关键帧的索引号，g＝1,2,...,Nsw，b表示地图中地图点的索引号，b＝1,2,...,Naw，Tg,w表示世界坐标系到第g个关键帧的相对位姿，
Figure FDA0003823761040000071
表示地图点坐标，argmin(·)表示函数取最小值时的参数取值，e′是待优化的重投影误差函数，其具体表达式为
Figure FDA0003823761040000072
δbg是地图点
Figure FDA0003823761040000073
与关键帧Tg,w构造的重投影误差，具体表达式为
Figure FDA0003823761040000074
if关键帧g观测到地图点
Figure FDA0003823761040000075
π(·)为去齐次变换，K是相机内参，
Figure FDA0003823761040000076
为第g个关键帧的特征点，Nsw为关键帧数，Naw为地图点数，
Figure FDA0003823761040000077
为世界坐标系地图点。
CN202211059723.XA 2022-08-30 2022-08-30 一种面向动态环境的单目视觉slam方法 Pending CN115471748A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211059723.XA CN115471748A (zh) 2022-08-30 2022-08-30 一种面向动态环境的单目视觉slam方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211059723.XA CN115471748A (zh) 2022-08-30 2022-08-30 一种面向动态环境的单目视觉slam方法

Publications (1)

Publication Number Publication Date
CN115471748A true CN115471748A (zh) 2022-12-13

Family

ID=84368848

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211059723.XA Pending CN115471748A (zh) 2022-08-30 2022-08-30 一种面向动态环境的单目视觉slam方法

Country Status (1)

Country Link
CN (1) CN115471748A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116051815A (zh) * 2023-03-31 2023-05-02 四川省寰宇众恒科技有限公司 一种基于单目摄像头的目标定位方法及***
CN117765084A (zh) * 2024-02-21 2024-03-26 电子科技大学 基于动态分支预测的迭代求解的面向视觉定位方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116051815A (zh) * 2023-03-31 2023-05-02 四川省寰宇众恒科技有限公司 一种基于单目摄像头的目标定位方法及***
CN117765084A (zh) * 2024-02-21 2024-03-26 电子科技大学 基于动态分支预测的迭代求解的面向视觉定位方法
CN117765084B (zh) * 2024-02-21 2024-05-03 电子科技大学 基于动态分支预测的迭代求解的面向视觉定位方法

Similar Documents

Publication Publication Date Title
CN109345588B (zh) 一种基于Tag的六自由度姿态估计方法
CN110223348B (zh) 基于rgb-d相机的机器人场景自适应位姿估计方法
CN109544636B (zh) 一种融合特征点法和直接法的快速单目视觉里程计导航定位方法
CN108242079B (zh) 一种基于多特征视觉里程计和图优化模型的vslam方法
CN108537848B (zh) 一种面向室内场景重建的两级位姿优化估计方法
CN111652934B (zh) 定位方法及地图构建方法、装置、设备、存储介质
US9420265B2 (en) Tracking poses of 3D camera using points and planes
CN111445526B (zh) 一种图像帧之间位姿的估计方法、估计装置和存储介质
CN109472828B (zh) 一种定位方法、装置、电子设备及计算机可读存储介质
CN115471748A (zh) 一种面向动态环境的单目视觉slam方法
CN107917710B (zh) 一种基于单线激光的室内实时定位与三维地图构建方法
CN111462210B (zh) 一种基于极线约束的单目线特征地图构建方法
CN108519102B (zh) 一种基于二次投影的双目视觉里程计算方法
CN112734841B (zh) 一种用轮式里程计-imu和单目相机实现定位的方法
CN111368759B (zh) 基于单目视觉的移动机器人语义地图构建***
CN112734765A (zh) 基于实例分割与多传感器融合的移动机器人定位方法、***及介质
CN112419497A (zh) 基于单目视觉的特征法与直接法相融合的slam方法
CN112767546B (zh) 移动机器人基于双目图像的视觉地图生成方法
CN111998862A (zh) 一种基于bnn的稠密双目slam方法
CN114494150A (zh) 一种基于半直接法的单目视觉里程计的设计方法
Majdik et al. New approach in solving the kidnapped robot problem
CN115008454A (zh) 一种基于多帧伪标签数据增强的机器人在线手眼标定方法
US20210304411A1 (en) Map construction method, apparatus, storage medium and electronic device
Zhang et al. A visual-inertial dynamic object tracking SLAM tightly coupled system
Wang et al. Relative pose estimation and planar reconstruction via superpixel-driven multiple homographies

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination