3D I-SLSJF: A Consistent Sparse Local Submap Joining Algorithm for Building Large-Scale 3D Map

This paper presents an efficient and reliable algorithm for autonomous robots to build large-scale three dimensional maps by combining small local submaps. The algorithm is a generalization of our recent work on two dimensional map joining algorithm - Iterated Sparse Local Submap Joining Filter (I-SLSJF). The 3D local submap joining problem is formulated as a least squares optimization problem and solved by Extended Information Filter (EIF) together with smoothing and iterations. The resulting information matrix is exactly sparse and this makes the algorithm efficient. The smoothing and iteration steps improve the accuracy and consistency of the estimate. The consistency and efficiency of 3D I-SLSJF is demonstrated by comparing the algorithm with some existing algorithms using computer simulations.