Mapping While Localizing

A robot in an unknown room has to build a map and find itself in it at the same time. This chapter gets there in steps. It starts with mapping from known poses, adds the question of which landmark a measurement belongs to, and then estimates the pose and the map together. It then brings in two sensors that measure the robot’s own motion, an IMU and a camera, and ends with a camera that maps the room and localizes itself from its images alone. The last step is the Visual SLAM assignment.

Before you start

The chapter assumes the extended Kalman filter and EKF localization sections, which supply the motion model, the measurement model and the filter equations every later section reuses. The D435i for landmark features section explains how the depth camera turns a pixel and a depth into a range and a bearing.

Reading order

Step	Section	What you get from it
1	Occupancy grid mapping	A map built from lidar scans when the poses are known, and the distortion that odometry drift causes when they are not.
2	Correspondence estimation	A way to decide which landmark a measurement came from: the Mahalanobis gate and joint compatibility.
3	Scan registration with ICP	The motion between two LIDAR scans from ICP: the closed-form fit, Gauss-Newton, point-to-line errors and robust weights against outliers.
4	EKF SLAM	The pose and the landmark map in one filter, with unknown correspondences and new landmarks added as they appear. Lidar and D435i front ends feed the same filter, and loop closure corrects the drift.
5	Visual SLAM	Camera motion from two views, the scale a single camera cannot measure, and loop closure with a pose graph, mapped onto the modules of StellaVSLAM.
6	Visual SLAM assignment	Your own monocular pipeline on KITTI, StellaVSLAM in your own room, and StellaVSLAM with Nav2 in the TurtleBot maze.
7	IMU	What a gyroscope and an accelerometer measure, how their noise and bias grow under integration, and why the pose integrated from an IMU alone stays accurate for only a few seconds.
8	Visual-inertial odometry	A filter in which camera images correct the IMU’s prediction and estimate the IMU’s biases.

Steps 1 to 4 use landmarks and range sensors, and step 4 is the baseline SLAM algorithm. In step 5 a camera replaces both the range sensor and wheel odometry. Read it before the assignment in step 6, because the assignment’s first task asks you to build the same front end on real images. Steps 7 and 8 add an inertial sensor. Step 5 shows that a single camera cannot measure scale, and an accelerometer can. Read step 7 before step 8, because the VIO filter’s prediction is the IMU integration of step 7.

Occupancy grid mapping

Mapping with known poses, and what odometry drift does to the map.

Correspondence estimation

Matching measurements to the map: data association and joint compatibility.

Scan registration

Aligning two LIDAR scans with ICP, from the closed-form fit to point-to-line and robust variants.

EKF SLAM

Pose and landmark map in one EKF, with landmarks from a lidar or a D435i.

Visual SLAM

Two-view geometry, monocular scale, and loop closure with a pose graph.

IMU

Gyroscope and accelerometer models, bias, and IMU dead reckoning.

Visual-inertial odometry

The camera corrects the IMU and estimates its biases.

Found an error on this page? Report an issue.

State Estimation & Mapping

State Estimation

SLAM

Mapping While Localizing

Before you start

Reading order

Occupancy grid mapping

Correspondence estimation

Scan registration

EKF SLAM

Visual SLAM

IMU

Visual-inertial odometry

​Before you start

​Reading order

Occupancy grid mapping

Correspondence estimation

Scan registration

EKF SLAM

Visual SLAM

IMU

Visual-inertial odometry

Before you start

Reading order