State Estimation Method Based on Improved VINS-Mono Algorithm

doi:10.12068/j.issn.1005-3026.2024.09.009

Abstract

Abstract:

Aiming at the requirement of precision in pose recognition process of traditional SLAM（simultaneous localization and mapping）， the reverse optical flow algorithm is added to the front?end part of VINS-Mono. To meet the real?time requirement of SLAM， a marginal optimization algorithm is integrated into the back?end sliding window optimization method of VINS-Mono， the parts excluding camera pose are marginalized firstly， and then pose parts of cameras are marginalized， to accelerate marginalization process. And experiments are carried out using EuRoc（European robotics challenge）datasets. The results show that accuracy is improved slightly by improvement strategy of the front end， and the possible reasons are analyzed. For the improvement strategy of the back end， it is found that compared with the source code， the time used for the marginalization of the improved algorithm is reduced by 25.9% on average， and then compare their trajectory accuracy. It is found that the error is controllable. Finally， it is verified that the improved strategy for the back?end of VINS-Mono is superior in real?time performance.

Key words: state estimation method, sliding window, marginalization, real?time performance, ROS robot simulation platform

CLC Number:

TP 301

Hai-fang WANG, Ding-jie QIAO, Tian-hao WU, Peng HUANG. State Estimation Method Based on Improved VINS-Mono Algorithm[J]. Journal of Northeastern University(Natural Science), 2024, 45(9): 1287-1293.

Figures/Tables 10

References 16

1	Davison A J， Reid I D， Molton N D，et al.MonoSLAM：real‑time single camera SLAM［J］.IEEE Transactions on Pattern Analysis and Machine Intelligence，2007，29（6）：1052-1067.
2	Klein G， Murray D.Parallel tracking and mapping for small AR workspaces［C］//IEEE and ACM International Symposium on Mixed and Augmented Reality.Nara，2007：225-234.
3	Engel J， Schöps T， Cremers D.LSD-SLAM：large‑scale direct monocular SLAM［C］//European Conference on Computer Vision.Cham：Springer，2014：834-849.
4	Engel J， Sturm J， Cremers D.Semi‑dense visual odometry for a monocular camera［C］//IEEE International Conference on Computer Vision.Sydney，2013：1449-1456.
5	Forster C， Pizzoli M， Scaramuzza D.SVO：fast semi‑direct monocular visual odometry［C］//IEEE International Conference on Robotics and Automation.Hong Kong，2014：15-22.
6	Newcombe R A， Lovegrove S J， Davison A J.DTAM：dense tracking and mapping in real‑time［C］//2011 International Conference on Computer Vision.Barcelona，2011：2320-2327.
7	Engel J， Koltun V， Cremers D.Direct sparse odometry［J］.IEEE Transactions on Pattern Analysis and Machine Intelligence，2018，40（3）：611-625.
8	Kerl C， Sturm J， Cremers D.Dense visual SLAM for RGB-D cameras［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems.Tokyo，2013：2100-2106.
9	Mur‑Artal R， Montiel J M， Tardós J D.ORB-SLAM：a versatile and accurate monocular SLAM system［J］.IEEE Transactions on Robotics，2015，31（5）：1147-1163.
10	Mourikis A I， Roumeliotis S I.A multi‑state constraint Kalman filter for vision‑aided inertial navigation［C］//IEEE International Conference on Robotics and Automation.Rome，2007：3565-3572.
11	Bloesch M， Omari S， Hutter M，et al.Robust visual inertial odometry using a direct EKF‑based approach［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）.Hamburg，2015：298-304.
12	Lynen S， Achtelik M W， Weiss S，et al.A robust and modular multi‑sensor fusion approach applied to MAV navigation［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems.Tokyo，2013：3923-3929.
13	Leutenegger S， Lynen S， Bosse M，et al.Keyframe‑based visual‑inertial odometry using nonlinear optimization［J］.The International Journal of Robotics Research，2015，34（3）：314-334.
14	Qin T， Li P L， Shen S J.VINS‑Mono：a robust and versatile monocular visual‑inertial state estimator［J］.IEEE Transactions on Robotics，2018，34（4）：1004-1020.
15	Martinelli A.Closed‑form solution of visual‑inertial structure from motion［J］.International Journal of Computer Vision，2014，106（2）：138-152.
16	Lucas B D， Kanade T.An iterative image registration technique with an application to stereo vision［C］//International Joint Conference on Artificial Intelligence.Vancouver，1981：674-679.

数据集	RMSE		对比结果/%
数据集	源码	改进算法	对比结果/%
MH_01	0.181 302	0.179 712	0.88
MH_02	0.182 778	0.180 029	1.50
MH_03	0.405 599	0.404 163	0.35
MH_04	0.393 492	0.402 433	-2.27

数据集	RMSE		对比结果/%
数据集	源码	改进算法	对比结果/%
MH_01	0.181 302	0.179 712	0.88
MH_02	0.182 778	0.180 029	1.50
MH_03	0.405 599	0.404 163	0.35
MH_04	0.393 492	0.402 433	-2.27

步骤	边缘化优化
1	localSize（parameter_block_size[it.first]）>4
2	marg_pose_index_size<-pos,localSize （parameter_block_size[it.first]） marg_pose_size<-marg_pose_size+localSize （parameter_block_size[it.first]） //对要边缘化的变量参数根据维度赋予id
3	m₁<-m-marg_pose_size; n₁<-n+marg_pose_size;
4	for （auto iter: marg_pose_index_size） //遍历marg_pose_index_size的所有元素
5	swap（A_reorder.block（idx,0,size,pos）, A_reorder.block（idx+size,0,m-idx-size,pos）） //将row i移到矩阵的最下面 swap（A_reorder.block（0,idx,pos,size）,A_reorder.block（0,idx+size,pos,m-idx-size）） //将col i移到矩阵的最右面 swap（b_reorder.segment（idx,size）,b_reorder. segment（idx+size,m?idx?size）） //将b i移到向量的最下面
6	tempA<-Arm1 * Amm_inv1; A<-Arr1-tempA * Amr1; b<-brr1-tempA * bmm1; //用Schur消元进行边缘化
7	m₂<-m-m₁;n₂<-n;
8	MatrixXd tempB=Arm * Amm_inv; A=Arr-tempB * Amr; b=brr-tempB * bmm;
	//对剩余的部分进行边缘化

步骤	边缘化优化
1	localSize（parameter_block_size[it.first]）>4
2	marg_pose_index_size<-pos,localSize （parameter_block_size[it.first]） marg_pose_size<-marg_pose_size+localSize （parameter_block_size[it.first]） //对要边缘化的变量参数根据维度赋予id
3	m₁<-m-marg_pose_size; n₁<-n+marg_pose_size;
4	for （auto iter: marg_pose_index_size） //遍历marg_pose_index_size的所有元素
5	swap（A_reorder.block（idx,0,size,pos）, A_reorder.block（idx+size,0,m-idx-size,pos）） //将row i移到矩阵的最下面 swap（A_reorder.block（0,idx,pos,size）,A_reorder.block（0,idx+size,pos,m-idx-size）） //将col i移到矩阵的最右面 swap（b_reorder.segment（idx,size）,b_reorder. segment（idx+size,m?idx?size）） //将b i移到向量的最下面
6	tempA<-Arm1 * Amm_inv1; A<-Arr1-tempA * Amr1; b<-brr1-tempA * bmm1; //用Schur消元进行边缘化
7	m₂<-m-m₁;n₂<-n;
8	MatrixXd tempB=Arm * Amm_inv; A=Arr-tempB * Amr; b=brr-tempB * bmm;
	//对剩余的部分进行边缘化

数据集	源码用时				改进算法用时
数据集	中位数	平均数	最小值	最大值	中位数	平均数	最小值	最大值
MH_01	11 918.92	11 877.99	11 706.68	11 977.77	8 550.97	8 578.00	8 481.91	8 677.89
MH_02	9 715.99	9 738.60	9 567.80	9 931.28	7 105.28	7 059.44	6 643.53	7 436.98
MH_03	9 405.12	9 427.55	9 289.16	9 649.91	7 257.35	7 129.18	6 726.81	7 448.28
MH_04	6 812.63	6 904.28	6 658.04	7 311.03	5 227.38	5 251.68	5 204.86	5 318.54