PathSpace: Rapid continuous map approximation for efficient SLAM using B-Splines in constrained environments

TL;DR

This work proposes PathSpace, a novel semantic SLAM framework that uses continuous B-splines to represent the environment in a compact manner, while also maintaining and reasoning through the continuous probability density functions required for probabilistic reasoning.

Abstract

Simultaneous Localization and Mapping (SLAM) plays a crucial role in enabling autonomous vehicles to navigate previously unknown environments. Semantic SLAM mostly extends visual SLAM, leveraging the higher density information available to reason about the environment in a more human-like manner. This allows for better decision making by exploiting prior structural knowledge of the environment, usually in the form of labels. Current semantic SLAM techniques still mostly rely on a dense geometric representation of the environment, limiting their ability to apply constraints based on context. We propose PathSpace, a novel semantic SLAM framework that uses continuous B-splines to represent the environment in a compact manner, while also maintaining and reasoning through the continuous probability density functions required for probabilistic reasoning. This system applies the multiple strengths of B-splines in the context of SLAM to interpolate and fit otherwise discrete sparse environments. We test this framework in the context of autonomous racing, where we exploit pre-specified track characteristics to produce significantly reduced representations at comparable levels of accuracy to traditional landmark based methods and demonstrate its potential in limiting the resources used by a system with minimal accuracy loss.

Figures (7)

• Figure 1: Example of a continuous B-spline representation of using a limited number of control points for its representation. Base map data by OpenStreetMap (available under the Open Database License)
• Figure 2: Demonstration of continuous uncertainty representation from discrete control point uncertainties and how it creates a continuous distribution of splines.
• Figure 3: Process of expanding the spline $S_z$ based on new readings $\mathbf{z}$. Each reading $\mathbf{z}_i$ are categorised as expansion points of the closest spline point is $\mathbf{S}(1)$ and is beyond a threshold distance from $\mathbf{C}_n$ and $\mathbf{C}_{n-1}$ or as update points otherwise (a), the interpolation point is selected and the rest are re-categorised while the spline extends to interpolate the new point (b), and an update is performed with all remaining points (c).
• Figure 4: Demonstration of $\mathbf{S}_z$ fitting. In (A) we find the closest point in $\mathbf{S}_\mu$ (green) to a new set of readings $\mathbf{z}$ (blue). In (B) we generate the sigma points $\mathbf{X}$ (red) based on the uncertainty of the reading and fit $\mathbf{S}_\mu$ to each one using ridge regression fitting $h()$ to generate the $\mathbf{Z}$ splines (gray). in (C), the the control points in $\mathbf{Z}$ are averaged to obtain the control points and their respective covariance, defining $\mathbf{S}_z$ (purple). (D) shows the basis values of each control point on the closes spline point to the reading, determining how much each control point will be allowed to move to fit in (B)
• Figure 5: Comparison between sample spline before and after simplification. We show a control point reduction from 100 to 32 points with minimal data loss