Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Overlapping Schwarz Preconditioners for Pose-Graph SLAM in Robotics

Stephan Köhler, Oliver Rheinbach, Yue Xiang Tee, Sebastian Zug

TL;DR

It is shown that a simplified SLAM problem can be interpreted as a finite element problem using linear elastic bars, highlighting the structural analogy to PDE discretizations and motivating the use of PDE-based preconditioners such as scalable domain decomposition preconditioners.

Abstract

We investigate the application of the additive overlapping Schwarz domain decomposition method as a preconditioner for the large sparse linear systems arising in graph-based nonlinear least-squares problems, specifically the pose-graph optimization back-end in Simultaneous Localization and Mapping (SLAM) in robotics. A brief introduction to both SLAM and domain decomposition preconditioners is given, followed by a description of the nonlinear least-squares formulation, its linearization, and the resulting matrix structure, making the paper accessible to readers without prior knowledge of either field. Numerical experiments for a simple model problem demonstrate the numerical scalability of the preconditioned conjugate gradient method to solve the linear systems resulting from Gauss--Newton linearization: Using the additive overlapping Schwarz preconditioner, the number of conjugate gradient iterations remains bounded independently of the problem size. We also show that a simplified SLAM problem can be interpreted as a finite element problem using linear elastic bars, highlighting the structural analogy to PDE discretizations and motivating the use of PDE-based preconditioners such as scalable domain decomposition preconditioners.

Overlapping Schwarz Preconditioners for Pose-Graph SLAM in Robotics

TL;DR

It is shown that a simplified SLAM problem can be interpreted as a finite element problem using linear elastic bars, highlighting the structural analogy to PDE discretizations and motivating the use of PDE-based preconditioners such as scalable domain decomposition preconditioners.

Abstract

We investigate the application of the additive overlapping Schwarz domain decomposition method as a preconditioner for the large sparse linear systems arising in graph-based nonlinear least-squares problems, specifically the pose-graph optimization back-end in Simultaneous Localization and Mapping (SLAM) in robotics. A brief introduction to both SLAM and domain decomposition preconditioners is given, followed by a description of the nonlinear least-squares formulation, its linearization, and the resulting matrix structure, making the paper accessible to readers without prior knowledge of either field. Numerical experiments for a simple model problem demonstrate the numerical scalability of the preconditioned conjugate gradient method to solve the linear systems resulting from Gauss--Newton linearization: Using the additive overlapping Schwarz preconditioner, the number of conjugate gradient iterations remains bounded independently of the problem size. We also show that a simplified SLAM problem can be interpreted as a finite element problem using linear elastic bars, highlighting the structural analogy to PDE discretizations and motivating the use of PDE-based preconditioners such as scalable domain decomposition preconditioners.
Paper Structure (15 sections, 38 equations, 6 figures, 2 tables)

This paper contains 15 sections, 38 equations, 6 figures, 2 tables.

Table of Contents

  1. Introduction
  2. Simultaneous Localization and Mapping (SLAM)
  3. Mathematical Formulation of Pose Graph SLAM in two dimensions
  4. Least-Squares Formulation
  5. Overlapping Schwarz Domain Decomposition Preconditioners
  6. Equivalence between a Simplified SLAM Problem and a Finite Element Model
  7. A Very Simple SLAM Problem
  8. Least-Squares Formulation
  9. Mechanical Interpretation and Equivalent Finite Element Formulation
  10. Resulting Sparse Linear System
  11. Relation to the Full SLAM Problem
  12. Numerical Results and Discussion
  13. Model Problem
  14. Overlapping Schwarz Preconditioner
  15. Conclusion and Limitations

Figures (6)

  • Figure 1: Simplified flow chart of a complete SLAM system.
  • Figure 2: Example for a SLAM problem; ground truth (red) and odometry data (blue); between the pose $\tilde{p}_{4}$ and $\tilde{p}_{0}$ is a loop closure edge (green)
  • Figure 3: Illustration of a robot trajectory decomposed into two overlapping subdomains $p_{1}$-$p_{2}$-$p_{3}$ and $p_{3}$-$p_{4}$-$p_{5}$ with minimal overlap, i.e., consisting only of the node $p_3$.
  • Figure 4: Sparsity pattern of the system matrix. Loop closures introduce additional off-diagonal coupling visible in the sparsity.
  • Figure 5: Comparison of an optimized robot path with odometry and ground truth; 8 loops, 8 points per side.
  • ...and 1 more figures