A Block Coordinate Descent approach for solving Graph SLAM

Abstract

Simultaneous Localisation and Mapping (SLAM) refers to the problem of estimating the position of a mobile robot navigating in an unknown environment while simultaneously constructing a map of it, using measurements collected by sensors mounted on the robot, such as cameras, lasers, radars, or inertial sensors. SLAM is of particular interest when there is no prior knowledge of the environment nor external sources of localisation (compass, GPS). In this sense, SLAM aims for autonomy of robot motion and environment discovery.

The graph-based formulation of the SLAM problem, also commonly referred to as Graph SLAM, maximum a posteriori estimation, factor graph optimisation or smoothing and mapping (SAM), is considered the current de-facto standard formulation for SLAM. This approach defines the SLAM problem as a nonlinear least squares minimisation problem, commonly solved via successive linearisation methods such as Gauss-Newton. However, iterative line search methods have limitations in terms of convergence guarantees and scalability, which suggest the research potential for alternative optimisation algorithms.

In our research, we study an alternative numerical method for solving the Graph SLAM problem: the Block Coordinate Descent method. By partitioning the problem into a series of optimisation subproblems, this approach may offer comparatively better performance than iterative linearisation algorithms, such as lower per-iteration computational complexity, scalability and parallel processing capabilities. Importantly, this method is not dependant on linearisation, and under certain conditions, may offer convergence guarantees towards stationary points.

We present our Block Coordinate Descent approach by systematically analysing the attributes of the optimisation subproblems originating from the use of this numerical method on a Graph SLAM problem formulation based on particular inertial, bearing and range measurement models: the Affine Motion Model, the Affine Bearing Model and the Squared Range Model. We verify the resulting optimisation subproblems satisfy conditions that offer convergence guarantees and scalability properties. Additionally, we evaluate our Block Coordinate Descent approach by implementing the resulting algorithm in a simulated environment using real-world datasets, comparing its performance to the Gauss-Newton line search method.