SoMaSLAM: 2D Graph SLAM for Sparse Range Sensing with Soft Manhattan World Constraints

TL;DR

This work proposes SoMaSLAM, a 2D graph SLAM designed for tiny drones with sparse range sensing that incorporates a soft Manhattan world utilizing landmark-landmark constraints into graph SLAM.

Abstract

We propose a graph SLAM algorithm for sparse range sensing that incorporates a soft Manhattan world utilizing landmark-landmark constraints. Sparse range sensing is necessary for tiny robots that do not have the luxury of using heavy and expensive sensors. Existing SLAM methods dealing with sparse range sensing lack accuracy and accumulate drift error over time due to limited access to data points. Algorithms that cover this flaw using structural regularities, such as the Manhattan world (MW), have shortcomings when mapping real-world environments that do not coincide with the rules. We propose SoMaSLAM, a 2D graph SLAM designed for tiny robots with sparse range sensing. Our approach effectively maps sparse range data without enforcing strict structural regularities and maintains an adaptive graph. We implement the MW assumption as soft constraints, which we refer to as a soft Manhattan world. We propose novel soft landmark-landmark constraints to incorporate the soft MW into graph SLAM. Through extensive evaluation, we demonstrate that our proposed SoMaSLAM method improves localization accuracy on diverse datasets and is flexible enough to be used in the real world. We release our source code and sparse range datasets at https://SoMaSLAM.github.io/.

Figures (11)

• Figure 1: Our SoMaSLAM results (right) where non-Manhattan (top left) and Manhattan (left-middle) worlds co-exist in a structured environment. We obtain sparse data by a low-cost, lightweight (2.3 g), four radially-spaced ToF range sensor attached to the Crazyflie giernacki2017crazyflie nano drone (bottom left).
• Figure 2: Overview of the proposed SoMaSLAM. We construct poses (magenta arrows) and landmarks (blue) in graph SLAM from odometry and line segments given sparse range measurements. We generate and impose pose-landmark constraints (purple edges) and soft landmark-landmark constraints (red edges) between poses as well as between landmarks. The pose graph optimization occurs using loop closure (light brown edges).
• Figure 3: Visualization of how four distances are measured between two landmarks (left). $\textup{d}_{m,n}$ is the distance between point $m$ and landmark segment $\mathbf{l_n}$. Orientation relationship between two landmarks (right) where $\mathbf{\theta_i}$ is the slope angle of i-th landmark $\mathbf{l_i}$.
• Figure 4: Pose and landmark graph representation for our SoMaSLAM, with pose-landmark constraints (purple edges), soft landmark-landmark constraints (red edges), and loop closure constraints (light brown edges). The orientation of the landmarks (blue) and the constraints between them are shown as examples above each landmark node.
• Figure 5: SoMaSLAM results on MIT Killian with two variables (left) and one variable (right) in the constraint error terms. Formulating a landmark-landmark constraint with one model parameter shows better performance.