基于雅克比域零空间边缘化的视觉SLAM

doi:10.19562/j.chinasae.qcgc.2023.08.016

摘要/Abstract

摘要：

为降低系统求解大规模线性方程时的计算资源占用率、提高系统运行速度，现有基于非线性优化的视觉SLAM框架大多利用增量方程中海森矩阵稀疏性与边缘化策略对问题进行降阶。然而，这些方法仍须占用大量内存以显式构建超高维度的海森矩阵，且由于该方法对数值变化的敏感性，在实际部署时常依赖双精度浮点数进行求解以降低数值误差，限制了其在低算力平台中的应用。为解决这一问题，本文提出基于雅克比域零空间边缘化的视觉SLAM方法，该方法在后端优化模块中将路标雅克比矩阵投影至其左零空间，在避免构建海森矩阵前提下达到降阶效果，提升求解效率，并在代数上证明两种边缘化方法的等价性。从数值分析角度证明本文提出的边缘化方法具备更好的数值稳定性，可支持单精度浮点数求解，进一步提升效率。公开数据集和实车试验表明本文方法相较于基于舒尔消元边缘化的通用优化器具备更好的求解速度和精度。

关键词: 自动驾驶, 视觉SLAM, 光束法平差, 边缘化

Abstract:

The existing visual SLAM frameworks based on nonlinear optimization， in order to reduce the computational resource occupation and improve the system operation speed when solving large-scale linear equations， mostly use the Hession matrix sparsity and marginalization strategy in incremental equations to reduce the order of the problem. However， these methods still need a large amount of memory to explicitly construct the ultra-high dimensional Hessian matrix. Moreover， due to the sensitivity of the method to numerical changes， in order to reduce numerical errors， in actual deployment， they often rely on double precision floating point numbers to solve， which limits the application in low computing power platforms. To solve this problem， this paper proposes a visual SLAM method based on Jacobian domain null-space marginalization， which projects the landmark Jacobian matrix to its left null-space in the back-end optimization module， achieves the effect of reduction and improves the solution efficiency on the premise of avoiding the construction of Hessian matrix， and proves the equivalence of the two marginalization methods algebraically. From the perspective of numerical analysis， it is proved that the marginalization method proposed in this paper has better numerical stability and can support the single precision floating point solution， with further improvement of the efficiency. The open dataset and real vehicle test show that the method in this paper has better solution speed and accuracy than the general optimizer based on Schur complement marginalization.

Key words: automatic driving, visual SLAM, bundle adjustment, marginalization

芦涛,金馨,廖毅霏,黄圣杰,杨依琳,谢国涛,秦晓辉. 基于雅克比域零空间边缘化的视觉SLAM[J]. 汽车工程, 2023, 45(8): 1457-1467.

Tao Lu,Xin Jin,Yifei Liao,Shengjie Huang,Yilin Yang,Guotao Xie,Xiaohui Qin. Visual SLAM Based on Jacobian Null-space Marginalization[J]. Automotive Engineering, 2023, 45(8): 1457-1467.

图/表 13

图1

图2

图3

图4

图5

图6

表1

KITTI及实车数据集试验APE对比结果"

数据集	g2o （SM）		Ours-double（NM）		Ours-float（NM）		ORB-SLAM3
数据集	mean	RMSE	mean	RMSE	mean	RMSE	mean	RMSE
00	7.580	9.387	5.734（24% $↓$ ）	6.335（32% $↓$ ）	6.188（18% $↓$ ）	7.208（23% $↓$ ）	7.513（1% $↓$ ）	9.879（5% $↑$ ）
02	19.570	26.900	19.30（1% $↓$ ）	25.47（5% $↓$ ）	19.77（1% $↑$ ）	27.20（1% $↑$ ）	21.02（7% $↑$ ）	26.43（2% $↓$ ）
03	1.384	1.552	0.556（59% $↓$ ）	0.818（47% $↓$ ）	1.009（27% $↓$ ）	1.164（25% $↓$ ）	1.378（1% $↓$ ）	1.613（3% $↑$ ）
04	1.592	1.865	0.700（56% $↓$ ）	0.841（54% $↓$ ）	1.423（10% $↓$ ）	1.635（12% $↓$ ）	1.086（31% $↓$ ）	1.279（31% $↓$ ）
05	6.830	7.361	4.530（33% $↓$ ）	4.840（34% $↓$ ）	4.665（31% $↓$ ）	4.979（32% $↓$ ）	6.915（1% $↑$ ）	7.708（5% $↑$ ）
06	15.230	17.470	10.68（29% $↓$ ）	12.51（28% $↓$ ）	13.10（14% $↓$ ）	15.00（14% $↓$ ）	15.84（4% $↑$ ）	18.54（6% $↑$ ）
07	3.955	4.501	3.799（4% $↓$ ）	4.220（6% $↓$ ）	4.200（6% $↑$ ）	4.632（2% $↑$ ）	3.516（11% $↓$ ）	3.872（14% $↓$ ）
09	8.017	9.170	7.330（8% $↓$ ）	8.010（12% $↓$ ）	8.360（4% $↑$ ）	9.457（3% $↑$ ）	9.090（13% $↑$ ）	10.33（13% $↑$ ）
10	8.070	9.390	6.605（18% $↓$ ）	7.724（17% $↓$ ）	7.725（4% $↓$ ）	8.917（5% $↓$ ）	9.835（21% $↑$ ）	11.32（20% $↑$ ）
实车	3.557	3.902	2.454（31% $↓$ ）	2.782（26% $↓$ ）	3.673（3% $↑$ ）	4.075（4% $↑$ ）	4.425（24% $↑$ ）	4.961（27% $↑$ ）

表1

表2

KITTI及实车数据集后端优化耗时统计 ms"

数据集	g2o-baseline	Ours-double	Ours-float
00	89	85（4% $↓$ ）	75（15% $↓$ ）
02	76	69（9% $↓$ ）	58（23% $↓$ ）
03	108	99（8% $↓$ ）	82（24% $↓$ ）
04	92	86（6% $↓$ ）	77（16% $↓$ ）
05	93	91（2% $↓$ ）	80（13% $↓$ ）
06	85	77（9% $↓$ ）	73（25% $↓$ ）
07	95	89（6% $↓$ ）	82（13% $↓$ ）
09	80	73（9% $↓$ ）	62（23% $↓$ ）
10	81	78（3% $↓$ ）	74（9% $↓$ ）
实车	128	117（9% $↓$ ）	104（19% $↓$ ）

表2

图7

图8

图9

图10

图11

参考文献 24

1	王金强，黄航，郅朋，等.自动驾驶发展与关键技术综述［J］.电子技术应用，2019，45（6）：28-36.DOI：10.16157/j. issn.0258-7998.199062.
	WANG Jinqiang， HUANG Hang， ZHI Peng， et al. Overview of autonomous driving development and key technologies ［J］. Electronic Technology Application， 2019， 45 （6）： 28-36. DOI： 10.16157/j. issn.0258-7998.199062.
2	秦晓辉，王哲文，庞涛，等.基于车辆模型紧耦合的封闭园区车辆定位方法［J］.汽车工程，2021，43（9）：1328-1335.DOI：10.19562/j.chinasae.qcgc.2021.09.009.
	QIN Xiaohui， WANG Zhewen， PANG Tao，et al. Vehicle positioning method for closed parks based on tight coupling of vehicle models ［J］. Automotive Engineering， 2021，43 （9）： 1328-1335. DOI： 10.19562/j.chinasae.qcgc.2021.09.009.
3	CADENA C， CARLONE L， CARRILLO H， et al. Past， present， and future of simultaneous localization and mapping： toward the robust-perception age［J］. IEEE Transactions on Robotics， 2016， 32（6）： 1309-1332.
4	MACARIO BARROS A， MICHEL M， MOLINE Y， et al. A comprehensive survey of visual slam algorithms［J］. Robotics， 2022， 11（1）： 24.
5	PIRE T， FISCHER T， CASTRO G， et al. S-PTAM： stereo parallel tracking and mapping［J］. Robotics and Autonomous Systems， 2017， 93： 27-42.
6	黄圣杰，胡满江，周云水，等.动态场景下基于语义分割与运动一致性约束的车辆视觉SLAM［J］.汽车工程，2022，44（10）：1503-1510.DOI：10.19562/j.chinasae.qcgc.2022.10.004.
	HUANG Shengjie， HU Manjiang， ZHOU Yunshui，et al. Vehicle vision SLAM based on semantic segmentation and motion consistency constraint in dynamic scenes ［J］. Automotive Engineering， 2022，44 （10）： 1503-1510. DOI： 10.19562/j.chinasae.qc gc.2022.10.004.
7	HUANG G P， MOURIKIS A I， ROUMELIOTIS S I. Analysis and improvement of the consistency of extended Kalman filter based SLAM［C］.2008 IEEE International Conference on Robotics and Automation. IEEE， 2008： 473-479.
8	MOURIKIS A I， ROUMELIOTIS S I. A multi-state constraint Kalman filter for vision-aided inertial navigation［C］.ICRA. 2007， 2： 6.
9	MUR-ARTAL R， MONTIEL J M M， TARDOS J D. ORB-SLAM： a versatile and accurate monocular SLAM system［J］. IEEE Transactions on Robotics， 2015， 31（5）： 1147-1163.
10	SIBLEY G， MATTHIES L， SUKHATME G. Sliding window filter with application to planetary landing［J］. Journal of Field Robotics， 2010， 27（5）： 587-608.
11	MCCORD T B， SOTIN C. Ceres： evolution and current state［J］. Journal of Geophysical Research： Planets， 2005， 110（E5）.
12	GRISETTI G， KÜMMERLE R， STRASDAT H， et al. g2o： a general framework for （hyper） graph optimization［C］.Proceedings of the IEEE international conference on robotics and automation （ICRA）， Shanghai， China. 2011： 9-13.
13	DEMMEL N， SOMMER C， CREMERS D， et al. Square root bundle adjustment for large-scale reconstruction［C］.Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition. 2021： 11723-11732.
14	LIU H， CHEN M， ZHANG G， et al. Ice-ba： incremental， consistent and efficient bundle adjustment for visual-inertial slam［C］.Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2018： 1974-1982.
15	REN J， LIANG W， YAN R， et al. MegBA： A GPU-based distributed library for large-scale bundle adjustment［C］.European Conference on Computer Vision. Springer， Cham， 2022： 715-731.
16	DEMMEL N， SCHUBERT D， SOMMER C， et al. Square root marginalization for sliding-window bundle adjustment［C］.Proceedings of the IEEE/CVF International Conference on Computer Vision. 2021： 13260-13268.
17	蔡英凤，陆子恒，李祎承，等.基于多传感器融合的紧耦合SLAM系统［J］.汽车工程，2022，44（3）：350-361.DOI：10.1956 2/j.chinasae.qcgc.2022.03.006.
	CAI Yingfeng， LU Ziheng， LI Yicheng， et al. Tight coupling SLAM system based on multi-sensor fusion ［J］. Automotive Engineering， 2022，44 （3）： 350-361. DOI： 10.1956 2/j.chinasae.qcgc.2022.03.006.
18	GENEVA P， ECKENHOFF K， LEE W， et al. Openvins： a research platform for visual-inertial estimation［C］.2020 IEEE International Conference on Robotics and Automation （ICRA）. IEEE， 2020： 4666-4672.
19	GEIGER A， LENZ P， STILLER C， et al. Vision meets robotics： the KITTI dataset［J］. The International Journal of Robotics Research， 2013， 32（11）： 1231-1237.
20	GONG X， LV Y， XU X， et al. Pose estimation of omnidirectional camera with improved EPnP algorithm［J］. Sensors， 2021， 21（12）： 4008.
21	ARSHAD S， KIM G W. Role of deep learning in loop closure detection for visual and lidar SLAM： a survey［J］. Sensors， 2021， 21（4）： 1243.
22	MUR-ARTAL R， TARDÓS J D. ORB-SLAM2： an open-source SLAM system for monocular， stereo， and RGB-D cameras［J］. IEEE Transactions on Robotics， 2017， 33（5）： 1255-1262.
23	TRANSTRUM M K， SETHNA J P. Improvements to the levenberg-marquardt algorithm for nonlinear least-squares minimization［J］. arXiv preprint arXiv：， 2012.
24	USENKO V， DEMMEL N， SCHUBERT D， et al. Visual-inertial mapping with non-linear factor recovery［J］. IEEE Robotics and Automation Letters， 2019， 5（2）： 422-429.

[1]	付新科,蔡英凤,陈龙,王海,刘擎超. 不确定性环境下的自动驾驶汽车行为决策方法[J]. 汽车工程, 2024, 46(2): 211-221.
[2]	赵晓聪,房世玉,李子睿,孙剑. 社会性驾驶交互关键效用析取与应用[J]. 汽车工程, 2024, 46(2): 230-240.
[3]	马艳丽, 秦钦, 董方琦, 娄艺苧. 基于风险场的不同认知次任务下接管风险评估模型[J]. 汽车工程, 2024, 46(1): 9-17.
[4]	刘卫国,项志宇,刘伟平,齐道新,王子旭. 基于分布式强化学习的车辆控制算法研究[J]. 汽车工程, 2023, 45(9): 1637-1645.
[5]	王明,唐小林,杨凯,李国法,胡晓松. 考虑预测风险的自动驾驶车辆运动规划方法[J]. 汽车工程, 2023, 45(8): 1362-1372.
[6]	朱向雷,吴志新,张宇飞,赵帅,李克秋,孙博华. 基于场景降维及采样方法的场景库优化方法研究[J]. 汽车工程, 2023, 45(8): 1408-1416.
[7]	吴新政,邢星宇,刘力豪,沈勇,陈君毅. 基于错误注入的决策规划系统抗扰性测试与分析[J]. 汽车工程, 2023, 45(8): 1428-1437.
[8]	高锋,冯德福,胡秋霞. 面向NMPC运动规划系统的数值优化加速技术[J]. 汽车工程, 2023, 45(8): 1438-1447.
[9]	伍文广,田双岳,张志勇,张斌. 非铺装道路凹凸不平特征语义分割方法研究[J]. 汽车工程, 2023, 45(8): 1468-1478.
[10]	林程, 汪博文, 吕沛原, 宫新乐, 于潇. 面向变曲率道路的自动驾驶汽车换道博弈运动规划与协同控制研究[J]. 汽车工程, 2023, 45(7): 1099-1111.
[11]	赵东宇, 赵树恩. 基于级联YOLOv7的自动驾驶三维目标检测[J]. 汽车工程, 2023, 45(7): 1112-1122.
[12]	李军, 周伟, 唐爽. 基于自适应拟合的智能车换道避障轨迹规划[J]. 汽车工程, 2023, 45(7): 1174-1183.
[13]	赵嘉豪,齐志权,齐智峰,王皓,何磊. 基于轮胎特征点的并行大型车辆朝向角计算[J]. 汽车工程, 2023, 45(6): 1031-1039.
[14]	金立生,韩广德,谢宪毅,郭柏苍,刘国峰,朱文涛. 基于强化学习的自动驾驶决策研究综述[J]. 汽车工程, 2023, 45(4): 527-540.
[15]	陈妍妍,王海,蔡英凤,陈龙,李祎承. 基于检测的高效自动驾驶实例分割方法[J]. 汽车工程, 2023, 45(4): 541-550.