A General Incremental Bundle Adjustment Framework For Visual-Inertial SLAM

With the rise of artificial intelligence concepts and the development of industries such as augmented reality,drones,mobile robots,and self-driving cars,there is a growing demand on industry and academia on robust SLAM algorithms with high efficiency and high precision.In the SLAM applications,the speed and accuracy of the state estimation methods greatly limit the performance of the SLAM algorithms.Most SLAM systems employ bundle adjust?ment(BA)for state estimation.Some systems use the open source general nonlinear least squares(NLS)solvers.In order to adapt to different types of NLS problems,those solvers are usually based on batch NLS algorithms,which sacrifice efficiency;Existing incremental solvers are very efficient,but only able to solve BA problems with certain cost functions and local parameterizations or tightly coupled with certain SLAM systems,and thus difficult to integrate into other SLAM systems.Upon these analyses,we proposed a general incremental BA framework for visual-inertial SLAM(VISLAM).On the one hand,incremental algorithms are used to improve efficiency:1)A factor graph is used t.o encode as well as to help the construction of the incremental Schur complement,in order to reduce the redundant computations at the linearization and elimination steps.And in the meanwhile,a Bayes tree is generated simultaneously;2)We also addressed the rank-deficient issue while solving the Schur equation,by proposing a Hybrid LM-DL method,which enhances the numerical stability of the linear solutions without destroying the incremental constructions of the Schur complement;3)In the back-substitution part,a partial Bayes tree(PBT)algorithm is used to improve the efficiency and variable consistency.On the other hand,the BA framework retains versatility while guaranteeing efficiency and precision.Besides BA problems,general NLS problems are supported:1)We imple-mented IMU preintegration cost function and re-projection error cost function,which are commonly required by VISLAM.Users can also implement their own cost functions of ar-bitrary types;2)Local parameterizations like rotation matrix and quaternion are built-in.Users can also define their own local parameterization methods;3)At the linear solving step,a variety of linear strategies are provided,such as sparse/dense Cholesky solver,QR solver and incremental preconditioned conjugate gradient(I-PCG)solver.It is also possible to implement customized linear algorithms that meet different numerical requirements.The BA framework proposed is fully based on the block sparse matrix which is more cache-friendly.We tested it on both simulated datasets and real BA datasets.The efficiency and accuracy are verified by experiment,s.