Sparse Local Submap Joining Filter for Building Large-Scale Maps

Abstract

This paper presents a novel local submap joining algorithm for building large-scale feature-based maps: sparse local submap joining filter (SLSJF). The input to the filter is a sequence of local submaps. Each local submap is represented in a coordinate frame defined by the robot pose at which the map is initiated. The local submap state vector consists of the positions of all the local features and the final robot pose within the submap. The output of the filter is a global map containing the global positions of all the features as well as all the robot start/end poses of the local submaps. Use of an extended information filter (EIF) for fusing submaps makes the information matrix associated with SLSJF exactly sparse. The sparse structure together with a novel state vector and covariance submatrix recovery technique makes the SLSJF computationally very efficient. The SLSJF is a canonical and efficient submap joining solution for large-scale simultaneous localization and mapping (SLAM) problems that makes use of consistent local submaps generated by any reliable SLAM algorithm. The effectiveness and efficiency of the new algorithm is verified through computer simulations and experiments.