MASAT: A fast and robust algorithm for pose-graph initialization

Abstract

In this paper, we propose a novel algorithm to compute the initial structure of pose-graph based Simultaneous Localization and Mapping (SLAM) systems. We perform a Breadth-First Search (BFS) on the graph in order to obtain multiple votes regarding the location of a certain robot position from all of its previously processed neighbors. Next, we define the initial location of a pose as the average of the multiple alternatives. By adopting the proposed initialization approach, the number of iterations needed for optimization is significantly reduced while the computational complexity remains lightweight. We perform quantitative evaluation on various 2D and 3D benchmark datasets to demonstrate the advantages of the proposed method.