CN114202579B - 一种面向动态场景的实时多体slam系统 - Google Patents
一种面向动态场景的实时多体slam系统 Download PDFInfo
- Publication number
- CN114202579B CN114202579B CN202111279429.5A CN202111279429A CN114202579B CN 114202579 B CN114202579 B CN 114202579B CN 202111279429 A CN202111279429 A CN 202111279429A CN 114202579 B CN114202579 B CN 114202579B
- Authority
- CN
- China
- Prior art keywords
- pose
- model
- expressed
- tracking
- probability
- 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.)
- Active
Links
- 230000004927 fusion Effects 0.000 claims description 14
- 238000000034 method Methods 0.000 claims description 14
- 230000003068 static effect Effects 0.000 claims description 11
- 238000001514 detection method Methods 0.000 claims description 10
- 230000003287 optical effect Effects 0.000 claims description 8
- 238000005457 optimization Methods 0.000 claims description 8
- 238000001914 filtration Methods 0.000 claims description 7
- 238000013179 statistical model Methods 0.000 claims description 7
- 239000011159 matrix material Substances 0.000 claims description 6
- 230000002457 bidirectional effect Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000008447 perception Effects 0.000 abstract description 3
- 230000009286 beneficial effect Effects 0.000 abstract description 2
- 230000000007 visual effect Effects 0.000 abstract 5
- 239000000284 extract Substances 0.000 abstract 1
- 238000012360 testing method Methods 0.000 description 4
- 230000003190 augmentative effect Effects 0.000 description 3
- 238000013461 design Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 238000011156 evaluation Methods 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000002372 labelling Methods 0.000 description 1
- 238000011158 quantitative evaluation Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Classifications
-
- 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
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
- G06T7/85—Stereo camera calibration
-
- 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/30—Subject of image; Context of image processing
- G06T2207/30244—Camera pose
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Image Analysis (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
一种面向动态场景的实时多体SLAM系统,该发明提取先验地图的结构信息和语义信息作为视觉里程计的先验约束。通过提出一种融合先验地图语义信息和结构信息的混合约束,将先验地图因子引入到视觉定位中。随后,利用视觉路标与先验地图之间的数据关联,使用EM算法同时优化数据关联和相机位姿。该算法能够有效限制视觉里程计的漂移误差并提高视觉定位精度,以服务于导航等应用场景。本发明构建的系统极大地提升了SLAM系统在动态场景中的位姿估计精度并同时提升了系统对周围物体的运动感知能力,有助于开展更智能化的智能决策与规划等工作。
Description
技术领域
本发明涉及计算机视觉及机器人领域,涉及一种面向动态场景的实时多体SLAM系统。
背景技术
具备同时估计自身位姿和周围环境中的物体的运动及位姿能力的多体SLAM系统有助于提升机器人对环境的感知能力。随着自动驾驶和增强现实等领域技术的发展,具备同时估计自身位姿和周围环境中的物体的运动及位姿能力的多体SLAM系统受到了广泛的关注,尤其是以视觉为主的方案以低成本且可扩展性强的优势备受关注,具备对环境的准确感知能力为后续的决策规划和主动交互任务具有重要的研究价值。《Proceedings ofthe European Conference on Computer Vision(ECCV).2018:646-661.》提出了一个紧耦合语义观测、特征观测的图优化模型对环境中运动物体的位姿进行优化,实现了精确鲁棒的定位效果。《IEEE Transactions on Robotics,2019,35(4):925-938》采用消失点法通过二维目标检测恢复三维立方框,并对运动物体的位姿估计设计了结合运动模型和几何特征的优化模型,在低动态场景中能够实现较为准确的位姿估计结果。《Proceedings of theIEEE/CVF Conference on Computer Vision and Pattern Recognition.2020:2168-2177》针对几何特征和语义特征的数据关联问题,通过在概率框架下构建基于异构条件随机场的以及多层概率数据关联方法,实现基于运动一致性判断的非先验模型多体运动估计里程计设计,从而降低由于引入错误数据关联对物体位姿估计的影响。
因此,面向动态场景的多体SLAM技术的关键问题在于,针对结构信息和时空一致性信息下的约束项设计,以及基于语义信息和几何结构信息数据关联。
发明内容
针对现有多体SLAM系统在实时性、鲁棒性以及定位的准确性方面的不足。本发明提出一种基于多特征融合以及静态率加权的多体SLAM位姿估计方法。遵循连续且多源的观测能够获取更精确估计的原则,该方法提出一种基于重投影残差统计模型的静态率加权自身位姿估计模型,以及多特征融合观测的运动物体位姿估计因子图模型。通过结合端到端的三维物体检测技术、SLAM过程对被连续跟踪上的物体提供的额外结构信息约束以及物体自身在时空内的运动一致性,使得该系统使用了低精度的三维物体检测算法,但仍能获得较好的运动物体位姿估计结果。提升系统在的环境感知能力,以服务于自动驾驶、增强现实等应用领域。
本发明的技术方案如下:
一种面向动态场景的实时多体SLAM系统,包括如下步骤:
步骤一,通过基于自适应贝叶斯决策融合的多模态跟踪器及基于多帧投票机制的特征数据关联方法,获取物体级和特征级数据关联结果;
步骤二,通过基于多源信息融合以及刚体运动一致性的因子图位姿优化模型对场景中的运动物体位姿进行优化;
步骤三,通过基于重投影残差统计模型的静态率加权自身位姿估计模型以及基于多视图几何的物体运动状态检测方法,提升相机位姿估计精度及鲁棒性。
进一步特征,步骤一:为了兼顾跟踪精度及跟踪实时性的需求,系统融合卡尔曼滤波(Kalman Filter,KF)、双向光流(Bi-directional optical flow,BDOF)以及KCF(KernelCorrelation Filter,KCF)跟踪器的跟踪结果,构建混合特征的亲和力矩阵,实现帧间物体级数据关联。
基于卡尔曼滤波跟踪结果的空间关联距离表示为:
上式中为第i-1时刻对第k个物体通过卡尔曼滤波预测得到的三维立方框,其中通过结合Sutherland-Hodgman和Shoelace计算得到。
基于双向光流跟踪结果的内点关联距离表示为:
其中size{·}用于计算集合大小,In{·}用于获取通过第i-1帧预测得到,处于第l个2D检测框内特征点。
基于贝叶斯决策级融合的概率关联距离表示为:
其中DKL(Pi,k||Qi,l),表示第i时刻,第k个物体中的像素集合属于该物体的概率分布Pi,k和第l个物体中的像素集合属于该物体的概率分布Qi,l之间的差异,通过KL散度(Kullback-Leibler divergence,KL)进行计算,表示第r个像素在KCF、KF观测的条件下属于第k个物体的类条件概率,其中αi,k表示贝叶斯决策级融合的加法准则中二者间的权重。
其中αi,k,l表示通过多模态跟踪器获取的混合亲和力矩阵中各个元素的数值加权,其中λ、β、γ分别表示三种不同的关联距离所占的权重。
进一步特征,步骤二:基于重投影残差统计模型的静态率加权自身位姿估计模型表示为:
wTc为相机自身在世界坐标系的位姿,Xb路标点的位姿,w(·)为基于重投影残差的统计概率模型所构建的静态权重。
在w(·)的计算中,δ2,n分别表示统计点的方差及个数,w(·)在给定初值下通过Gauss Newton法迭代求解。
进一步特征,步骤三:对于第i时刻跟踪上的物体k所构建的因子图位姿优化模型表示为:
分别表示第i时刻第k个物体的位姿以及属于其特征观测的位姿,其位姿通过融合eZ(·),eM(·),eS(·)残差项的最大后验概率(Maximum a posterioriestimation,MAP)模型求解。
eZ(·),表示基于特征点的重投影残差,πs(·)针孔相机投影模型。
eM(·)运动模型残差项。公式为基于non-holonomic的运动模型。
eS(·)为基于RTM3D语义观测的残差模型。
本发明构建的系统充分考虑SLAM过程提供的结构信息与目标检测提供的语义信息之间的互补性,通过构建多特征融合观测的运动物体位姿估计因子图模型,并结合重投影残差统计模型构建静态率加权自身位姿估计模型,以双目图像作为输入,结合RTM3D目标检测方法,构建动态场景下实时多体SLAM系统,实现对相机自身位姿及周围动态物体位姿的精确鲁棒估计。极大提升系统在的环境感知能力,以服务于自动驾驶、增强现实等应用领域。
附图说明
图1基于自适应贝叶斯融合的多模态跟踪器;
图2实际场景数据集下相机位姿估计与传统SLAM算法对比;
图3物体跟踪长度以及速度估计与真值对比;
图4多模态跟踪器各组分耗时分析;
图5整体系统流程图。
具体实施方式
步骤一:首先为了兼顾跟踪精度及跟踪实时性的需求,系统融合卡尔曼滤波(Kalman Filter,KF)、双向光流(Bi-directional optical flow,BDOF)以及KCF(KernelCorrelation Filter,KCF)跟踪器的跟踪结果,构建混合特征的亲和力矩阵,实现帧间物体级数据关联。
基于卡尔曼滤波跟踪结果的空间关联距离表示为:
上式中为第i-1时刻对第k个物体通过卡尔曼滤波预测得到的三维立方框,其中通过结合Sutherland-Hodgman和Shoelace计算得到。
基于双向光流跟踪结果的内点关联距离表示为:
其中size{·}用于计算集合大小,In{·}用于获取通过第i-1帧预测得到,处于第l个2D检测框内特征点。
基于贝叶斯决策级融合的概率关联距离表示为:
其中DKL(Pi,k||Qi,l),表示第i时刻,第k个物体中的像素集合属于该物体的概率分布Pi,k和第l个物体中的像素集合属于该物体的概率分布Qi,l之间的差异,通过KL散度(Kullback-Leibler divergence,KL)进行计算,表示第r个像素在KCF、KF观测的条件下属于第k个物体的概率,其中αi,k表示贝叶斯决策级融合的加法准则中二者间的权重,融合过程如图1所示。
其中αi,k,l表示通过多模态跟踪器获取的混合亲和力矩阵中各个元素的数值加权,其中λ、β、γ分别表示三种不同的关联距离所占的权重。
步骤二:基于重投影残差统计模型的静态率加权自身位姿估计模型表示为:
wTc为相机自身在世界坐标系的位姿,Xb路标点的位姿,w(·)为基于重投影残差的统计概率模型所构建的静态权重。
在w(·)的计算中,δ2,n分别表示统计点的方差及个数,w(·)在给定初值下通过Gauss Newton法迭代求解。
步骤三:对于第i时刻跟踪上的物体k所构建的因子图位姿优化模型表示为:
分别表示第i时刻第k个物体的位姿以及属于其特征观测的位姿,其位姿通过融合eZ(·),eM(·),eS(·)残差项的最大后验概率(Maximum a posterioriestimation,MAP)模型求解。
eZ(·),表示基于特征点的重投影残差,πs(·)针孔相机投影模型。
eM(·)运动模型残差项。公式为基于non-holonomic的运动模型。
eS(·)为基于RTM3D语义观测的残差模型。
本发明所构建的系统在KITTI数据集和实际场景数据集上进行了定量和定性的评估。
在KITTI Raw数据集上,我们通过绝对平移误差(Absolute Translation Error,ATE)、相对平移和旋转误差(Translation Relative Pose Error,T.RPE)(RotationRelative Pose Error,R.RPE)进行量化评估,实验结果如表1所示,在里程计精度方面,本发明所构建的系统在动态场景下显著提升定位精度,与目前最先进的系统相比达到接近甚至是更好的结果。此外,本发明所构建的系统还在我们所采集的两个实际场景数据集上进行了测试,测试结果如图2所示,可以看出,在动态场景下,本系统相较于传统系统显著提升了定位精度。
表1 KITTI Raw数据集下相机位姿估计与同类型最先进算法量化对比
另外,为评估本发明所构建的系统对场景中的运动物体位姿估计以及跟踪能力,我们在KITTI Raw数据集上进行了定量评估。表2为优化前后对场景中物体的正确关联率(True Association,TA)以及平均三维交并比(Average Three DimensionalIntersection-over-Union,A3D_IoU)的量化评估结果,本发明所构建的系统在此指标上平均分别提升20%和10%以上,对物体速度的估计以及跟踪帧数如图3所示,基本与数据集提供的标注真值一致。
表2物体优化前后位姿估计量化对比
此外,为评估本系统的实时运行能力,我们对系统的各组分耗时以及最为耗时的多模态跟踪器部分进行了测试,测试结果如图4和表3所示,可以看出,本系统在KITTI Raw数据集中动态车辆最多的1003_0047序列中仍能保持9FPS左右的的性能,基本满足KITTI数据集图像采集帧率10FPS,达到同类方法中较为高效的性能。
表3整体系统各组分耗时分析
Claims (1)
1.一种面向动态场景的实时多体SLAM系统,其特征在于,包括如下步骤:
步骤一,通过基于自适应贝叶斯决策融合的多模态跟踪器及基于多帧投票机制的特征数据关联方法,获取物体级和特征级数据关联结果;
步骤二,通过基于多源信息融合以及刚体运动一致性的因子图位姿优化模型对场景中的运动物体位姿进行优化;
步骤三,通过基于重投影残差统计模型的静态率加权自身位姿估计模型以及基于多视图几何的物体运动状态检测方法,提升相机位姿估计精度及鲁棒性;所述步骤一具体如下:
为了兼顾跟踪精度及跟踪实时性的需求,系统融合卡尔曼滤波、双向光流以及KCF跟踪器的跟踪结果,构建混合特征的亲和力矩阵,实现帧间物体级数据关联;
基于卡尔曼滤波跟踪结果的空间关联距离表示为:
上式中为第i-1时刻对第k个物体通过卡尔曼滤波预测得到的三维立方框,其中通过结合Sutherland-Hodgman和Shoelace计算得到;
基于双向光流跟踪结果的内点关联距离表示为:
其中size{·}用于计算集合大小,In{·}用于获取通过第i-1帧预测得到,处于第l个2D检测框内特征点;
基于贝叶斯决策级融合的概率关联距离表示为:
其中DKL(Pi,k||Qi,l),表示第i时刻,第k个物体中的像素集合属于该物体的概率分布Pi,k和第l个物体中的像素集合属于该物体的概率分布Qi,l之间的差异,通过KL散度进行计算,表示第r个像素在KCF、KF观测的条件下属于第k个物体的类条件概率,其中αi,k表示贝叶斯决策级融合的加法准则中二者间的权重;
其中αi,k,l表示通过多模态跟踪器获取的混合亲和力矩阵中各个元素的数值加权,其中λ、β、γ分别表示三种不同的关联距离所占的权重;
所述步骤二具体如下:
基于重投影残差统计模型的静态率加权自身位姿估计模型表示为:
wTc为相机自身在世界坐标系的位姿,Xb路标点的位姿,w(·)为基于重投影残差的统计概率模型所构建的静态权重;
在w(·)的计算中,δ2,n分别表示统计点的方差及个数,w(·)在给定初值下通过GaussNewton法迭代求解;
所述步骤三具体如下:
对于第i时刻跟踪上的物体k所构建的因子图位姿优化模型表示为:
分别表示第i时刻第k个物体的位姿以及属于其特征观测的位姿,其位姿通过融合ez(·),eM(·),eS(·)残差项的最大后验概率模型求解;
ez(·),表示基于特征点的重投影残差,πs(·)针孔相机投影模型;
eM(·)运动模型残差项;公式为基于non-holonomic的运动模型;
eS(·)为基于RTM3D语义观测的残差模型。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111279429.5A CN114202579B (zh) | 2021-11-01 | 2021-11-01 | 一种面向动态场景的实时多体slam系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111279429.5A CN114202579B (zh) | 2021-11-01 | 2021-11-01 | 一种面向动态场景的实时多体slam系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114202579A CN114202579A (zh) | 2022-03-18 |
CN114202579B true CN114202579B (zh) | 2024-07-16 |
Family
ID=80646708
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111279429.5A Active CN114202579B (zh) | 2021-11-01 | 2021-11-01 | 一种面向动态场景的实时多体slam系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114202579B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114972501B (zh) * | 2022-04-21 | 2024-07-02 | 东北大学 | 一种基于先验语义地图结构信息和语义信息的视觉定位方法 |
CN114964276B (zh) * | 2022-05-26 | 2024-09-13 | 哈尔滨工业大学 | 一种融合惯导的动态视觉slam方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111462135A (zh) * | 2020-03-31 | 2020-07-28 | 华东理工大学 | 基于视觉slam与二维语义分割的语义建图方法 |
CN111814683A (zh) * | 2020-07-09 | 2020-10-23 | 北京航空航天大学 | 一种基于语义先验和深度学习特征的鲁棒视觉slam方法 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110349213B (zh) * | 2019-06-28 | 2023-12-12 | Oppo广东移动通信有限公司 | 基于深度信息的位姿确定方法、装置、介质与电子设备 |
CN112446882A (zh) * | 2020-10-28 | 2021-03-05 | 北京工业大学 | 一种动态场景下基于深度学习的鲁棒视觉slam方法 |
CN112902953B (zh) * | 2021-01-26 | 2022-10-04 | 中国科学院国家空间科学中心 | 一种基于slam技术的自主位姿测量方法 |
-
2021
- 2021-11-01 CN CN202111279429.5A patent/CN114202579B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111462135A (zh) * | 2020-03-31 | 2020-07-28 | 华东理工大学 | 基于视觉slam与二维语义分割的语义建图方法 |
CN111814683A (zh) * | 2020-07-09 | 2020-10-23 | 北京航空航天大学 | 一种基于语义先验和深度学习特征的鲁棒视觉slam方法 |
Also Published As
Publication number | Publication date |
---|---|
CN114202579A (zh) | 2022-03-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111862126B (zh) | 深度学习与几何算法结合的非合作目标相对位姿估计方法 | |
CN110688905B (zh) | 一种基于关键帧的三维物体检测与跟踪方法 | |
CN114202579B (zh) | 一种面向动态场景的实时多体slam系统 | |
CN104537709A (zh) | 一种基于位姿变化的实时三维重建关键帧确定方法 | |
CN109974743A (zh) | 一种基于gms特征匹配及滑动窗口位姿图优化的rgb-d视觉里程计 | |
Wen et al. | Hybrid semi-dense 3D semantic-topological mapping from stereo visual-inertial odometry SLAM with loop closure detection | |
CN112233179A (zh) | 一种视觉里程计测量方法 | |
CN111882602B (zh) | 基于orb特征点和gms匹配过滤器的视觉里程计实现方法 | |
CN112446882A (zh) | 一种动态场景下基于深度学习的鲁棒视觉slam方法 | |
CN110070578B (zh) | 一种回环检测方法 | |
CN115619826A (zh) | 一种基于重投影误差和深度估计的动态slam方法 | |
CN114707611B (zh) | 基于图神经网络特征提取与匹配的移动机器人地图构建方法、存储介质及设备 | |
Jin et al. | Beyond learning: Back to geometric essence of visual odometry via fusion-based paradigm | |
Min et al. | Coeb-slam: A robust vslam in dynamic environments combined object detection, epipolar geometry constraint, and blur filtering | |
Zhu et al. | Fusing panoptic segmentation and geometry information for robust visual slam in dynamic environments | |
Miao et al. | Ds-depth: Dynamic and static depth estimation via a fusion cost volume | |
CN113888603A (zh) | 基于光流跟踪和特征匹配的回环检测及视觉slam方法 | |
CN113362377A (zh) | 一种基于单目相机的vo加权优化方法 | |
CN116452654B (zh) | 一种基于bev感知的相对位姿估计方法、神经网络及其训练方法 | |
CN116972874A (zh) | 一种基于光流全局感知的无监督单目视觉里程计 | |
CN112348854A (zh) | 一种基于深度学习视觉惯性里程检测方法 | |
CN114037759A (zh) | 一种室内环境下的动态特征点滤除与重定位方法 | |
CN113837243A (zh) | 基于边缘信息的rgb-d相机动态视觉里程计方法 | |
Zhou et al. | Svo-pl: Stereo visual odometry with fusion of points and line segments | |
Yi et al. | Real-time Estimation of Road Surfaces using Fast Monocular Depth Estimation and Normal Vector Clustering |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |