Mapping and SLAM

Objectives

  • Understand the difference between mapping and SLAM
  • Discuss different map representations and when they are used
  • For SLAM
    • Define the SLAM problem 
    • Taxonomy for the SLAM problem
    • Describe the difference between online and full SLAM
    • Understand what loop closure is and why is it important in SLAM
    • Rough idea about how EKF SLAM, particle methods and graph based optimisation methods frame the problem
  • Define visual odometry

 

Pre-Lecture reading material

  • Re-cap your understanding of covariance by looking at this page Links to an external site. up until Figure 3.
  • Read until but not including "Approaches to SLAM"
  • Skim section 45.2 in SHoR about occupancy grids, topological maps and landmark based maps.

 

Introduction

In the previous lecture we talked about localisation and we saw that we needed a map to localise with respect to. The map contains the information that we associate our measurements with, which allows us to correct the position. This map has to be created somehow. Mapping can be done under two different conditions, i) we know the position and ii) we are estimating the position along with the map. 

Case 1: Mapping

For the first of these two cases the corresponding Bayesian networks looks like in the figure below. This is the pure Mapping scenario.

BN_Mapping.png

From the graph we see that we know the positions, xk, and have measurements, zk, (these nodes are grey) and we use these to build a map, M. This corresponds to what you are doing / will do in Assignment 4: Mapping. In mapping in mobile robotics the sensors are typically mounted on the robot and thus give measurements relative to the robot's pose. This means that in order to model the world, i.e., to estimate the map we need both the pose of the robot and the measurements to place what the sensor sees into a fixed map frame. That is, if our laser sensor registers an obstacle 3m away at a bearing of 34 degs we only know where to place this in the map if we take into account where the robot was when this measurement was acquired (and also where the sensor is on the robot if not in its center).

Most non-robot applications of mapping correspond to this use case, where GPS today is the typical source of position information. One of the most common use case in robotics for this type of mapping is when a local map is built to represent the surroundings of the robot, accumulating and fusing the information from all the available sensors that can give information about where there are obstacles. Some of these sensors have a local nature (short range or low angular resolution like a sonar sensor) that make them less suitable for localisation but offer valuable information to stay safe from obstacles. 

The problem of mapping can be formalised as estimating p(M | X1:k, Z1:k ) (using the same notation as in Localization).

 

Case 2: Simultaneous Localisation and Mapping - SLAM

The other use case is when the robot has no map of the environment and no external positioning system to allow it to build a map like in use case 1. This leads to a situation where the map and the position of the robot has to be estimated simultaneously, a problem referred to as Simultaneous Localisation and Mapping or SLAM for short. It is only relatively recently (80s) that it was concluded that you must perform mapping and localisation at the same time and SLAM has been one of the most active research areas since then. SLAM - Artificial Intelligence for Robotics Links to an external site.SLAM - Artificial Intelligence for Robotics

The Bayesian network describing the SLAM problem is shown below.

BN_SLAM.png

We typically define the starting point of the robot when SLAM is initialised as the origin. The robot uses its sensors to build the map and to use this map as it is evolving to localise. Historically there have been three main methods to address this problem. The first two are based on the idea of filtering, and are essentially extensions of the methods based on Kalman filters and particle filters presented in the part on Localization. The addition that needs to be made here is that the robot must add information to the filtering process that captures the state of the map. More recently, optimisation based methods have started to dominate the SLAM community. The basic idea for these methods is to build a graph consisting of nodes corresponding to robot poses and edges corresponding to constraints in the form of relative measurements between these poses (essentially corresponding to the graph seen above). We often distinguish between the front-end and the back-end for such methods. The front-end adds nodes to the graph and the back-end is responsible for optimising the graph.

One of the most important ingredients in successful mapping, independent of the method used, is to be able to identify and exploit so called loop closures. These are situations where the robot revisits some areas it has been to and is able to exploit the, typically, lower uncertainty present in part of the map that was constructed earlier to reduce the overall uncertainty of the map. This allows us to create more accurate and consistent maps. The video below shows how the maps gets corrected when loops are closed. 

Cartographer 2D SLAM Loop Closure Demo Links to an external site.Cartographer 2D SLAM Loop Closure Demo

There are two formulations of SLAM, corresponding to the cases where the entire path of the robot is being estimated jointly with the map or if it is only the last pose. The first case is called the full SLAM problem where we want to estimate p( X1:k, M | Z1:k, U1:k ) and the latter is called online SLAM where we estimate p( Xk, M | Z1:k, U1:k ).

Approaches to SLAM

There are three main approaches to SLAM which are briefly introduced below. See below for reading instructions.

EKF SLAM

In EKF SLAM we use the same Kalman filter framework as was discussed for estimating the position in Localization. However, since we are estimating the map here as well we need to extend the state vector x, which previously only contained the pose of the robot to also include the map. The most common map representation used in this context is to use landmarks which could be for example points which in a 2D setting and would then correspond to the coordinates (x,y) of such a point. When new landmarks are detected they are added to the state vector and estimated jointly with the pose of the robot. The state vector therefore grows as the map grows. This computational costs increases quadratically with the number of landmarks and this is one of the bottlenecks of EKF SLAM.

 

If you want more, Cyril Stachniss gives a really good introduction to EKF SLAM in this lecture

SLAM Course - 05 - EKF SLAM (2013/14; Cyrill Stachniss) Links to an external site.SLAM Course - 05 - EKF SLAM (2013/14; Cyrill Stachniss) 

Particle filter methods

When talking about localisation we showed how one could use particle filters. They had the advantage of the EKF method that they, e.g., handle multi-modal distributions and non-linearities. However, we also learned that they scaled really badly with the number of dimension of the state we are trying to estimate. Above we just said that in SLAM the number of dimensions become large when the number of landmark / size of the map increases. The key to using particle filter based methods is to not use it for the entire state. We use it to represent the highly non-linear part and the part where we typically have multi-modal distributions, which corresponds to the robot pose part of the state. This is made possible by something called Roa-Blackwellisation where we factor the probability distribution into two parts, the robot pose estimated by a particle filter and the map which is estimated separately for each particle. In the first implementation of this, FastSLAM, the map consists of landmarks estimated using EKF, i.e., each particle was running an EKF to estimate its own map. 

A frequently used implementation of this approach is gMapping Links to an external site., available in ROS Links to an external site., here the map is represented as a grid. The video below shows how the inside of one of the buildings at MIT is being mapped using gMapping.SLAM - GMapping - MIT - 2005-2007 Links to an external site.SLAM - GMapping - MIT - 2005-2007

If you want more, check out Cyrill Stachniss video

SLAM Course - 10a - FastSLAM - Part 1 - Cyrill Stachniss Links to an external site.SLAM Course - 10a - FastSLAM - Part 1 - Cyrill Stachniss

 

 

Graph-based optimisation methods

The graph based methods are now the dominating approach to SLAM. The idea is simple. For every pose where the robot has acquired information from its sensors, you add a node to a graph. These nodes corresponds to the pose nodes xk in the figure above. Odometric information provide estimates for the relative pose between these nodes. This information results in a constraint for the relative pose between the nodes. Measurements of landmarks provide constrains for the relative pose between robot and landmark nodes. By optimising for the robot and landmark poses we find the map as well as the robot pose. The following video explains this 

Graph SLAM - Artificial Intelligence for Robotics Links to an external site.Graph SLAM - Artificial Intelligence for Robotics

There are several optimisation frameworks available such as g2o Links to an external site., gtsam Links to an external site. and ceres  Links to an external site.that simplifies implementing an optimisation based SLAM. Today, a major part of the research in SLAM is focused on visual SLAM in which visual information is used both to estimate the relative position between frames ("visual odometry") and the map. It is also quite common to include information from an IMU as a way to get a good prediction for how the camera moved before the visual information is used. Links to an external site.

 

Reading instructions (refs only to 2nd edition)

  • Map representations (What are these and when suitable)
    • 2D occupancy grid maps (SHoR 45.2.1)
      • Make sure you understand the role of the sensor model
    • Feature maps (SHoR 45.2.2 and 45.2.4)
    • Topological maps (SHoR  45.2.3)
    • Elevation maps (SHoR 45.3.1)
    • 3D grid maps (SHoR 45.3.2)
    • Point sets (SHoR 45.3.2)
    • Meshes (SHoR 45.3.3)
  • SLAM (SHoR 46)
  • Visual odometry (wikipedia Links to an external site.)