Simultaneous Localization and Mapping (SLAM)

Simultaneous localization and mapping (SLAM) is the problem of a robot, vehicle, or handheld device that must build a map of an unknown environment while working out where it is in that map. Each is needed to estimate the other: placing an observation in the map requires the sensor’s pose, and estimating the pose requires the map. SLAM lets robots navigate without GPS and lets augmented-reality devices keep virtual content fixed to the world.

Visual odometry solves the localization half incrementally and lets errors accumulate. SLAM adds a persistent map, recognizes when the sensor returns to a place it has mapped (loop closure), and uses those revisits to make the whole trajectory and map consistent. As Cadena et al. (2016) put it, without loop closures SLAM reduces to odometry.

Problem Definition

A sensor moves along poses $x_1, \dots, x_T$ (in $SE(2)$ or $SE(3)$), receives motion measurements $u_{1:T}$ from odometry or an inertial sensor, and observations $z_{1:T}$ of the environment. The full SLAM problem is to estimate the posterior over the whole trajectory and the map $m$ (Grisetti et al., 2010):

$$p(x_{1:T}, m \mid z_{1:T}, u_{1:T}).$$

Online SLAM estimates only the current pose with the map, $p(x_t, m \mid z_{1:t}, u_{1:t})$, the full posterior at time $t$ with the past poses $x_{1:t-1}$ integrated out. Durrant-Whyte and Bailey (2006) formulate it as a recursive Bayes filter with a motion model $p(x_t \mid x_{t-1}, u_t)$ and an observation model $p(z_t \mid x_t, m)$. Filtering methods solve the online problem; smoothing methods solve the full one, usually by least squares (Grisetti et al., 2010).

With Gaussian noise, the maximum a posteriori (MAP) estimate of all variables $X$ (poses, landmarks, and possibly calibration) is a nonlinear least-squares problem (Cadena et al., 2016):

$$X^\star = \arg\min_X \sum_k \big\lVert h_k(X_k) - z_k \big\rVert^2_{\Omega_k},$$

where each measurement $z_k$ depends on a subset $X_k$ through a model $h_k$, $\Omega_k$ is its information (inverse covariance) matrix, $\lVert e \rVert^2_\Omega = e^\top \Omega e$, and a Gaussian prior on $X$ enters as one more term. Each term is a factor in a factor graph over the variables. For visual landmarks this is bundle adjustment, but SLAM factors also come from wheel odometry, inertial sensors, GPS, or scan matching, and the problem must be re-solved as measurements arrive (Cadena et al., 2016).

Input and Output

• Input: calibrated sensor streams: monocular, stereo, or RGB-D cameras, LiDAR, an inertial measurement unit (IMU), or wheel encoders.
• Output: the trajectory (often only at keyframes) and a map, sometimes with uncertainty.

Variants

• Sensors. Monocular cameras give the map only up to scale; stereo, RGB-D, LiDAR, and inertial sensing make it metric.
• Sparse landmarks. Distinctive features such as corners, with descriptors for data association, the representation of most systems and of structure from motion (Cadena et al., 2016).
• Dense maps. Point clouds, surfels (small oriented disks), meshes, and implicit surfaces such as the truncated signed distance function (TSDF), the clipped signed distance to the nearest surface stored on a voxel grid (Cadena et al., 2016).
• Occupancy grids. Cells with a probability of being occupied, common in 2D laser SLAM and for planning.
• Pose graphs. Only poses are estimated; the map is the raw scans or keyframes attached to them.

Challenges

• Data association. Wrong matches between measurements and landmarks corrupt the estimate; rejected correct ones waste information. Perceptual aliasing, different places that look alike, makes long-term association hard, while short-term association between consecutive frames is comparatively easy (Cadena et al., 2016).
• Loop closure. Re-observing landmarks after a long traverse is especially difficult (Durrant-Whyte and Bailey, 2006), as drift may put their predictions far off.
• Nonlinearity. Linearized estimators can become overconfident (see extended Kalman filter), and iterative solvers can converge to wrong local minima.
• Change. Most systems assume a static world, which moving people violate in the short term and seasons and rearranged furniture in the long term.

Approaches

History

As Durrant-Whyte and Bailey (2006) recount, the probabilistic SLAM problem took shape in discussions at the 1986 IEEE Robotics and Automation Conference. Smith, Self and Cheeseman then showed that landmark estimates are correlated through the common error in the vehicle’s pose, so a consistent solution needs a joint state over the pose and every landmark, with computation scaling as the square of the number of landmarks. Expecting map errors not to converge, researchers turned to decoupled approximations. The breakthrough was realizing that the joint problem converges, and that the correlations are what make it work. The structure of the problem, the convergence result, and the acronym SLAM were first presented in a survey at the 1995 International Symposium on Robotics Research; the problem was also called concurrent mapping and localization (CML). Cadena et al. (2016) call 1986–2004 the classical age and 2004–2015 the algorithmic-analysis age, which studied observability, convergence, and consistency, exploited sparsity, and produced the main open-source libraries.

Filtering

EKF-SLAM keeps the pose and all landmarks in one extended Kalman filter state with a full covariance. Durrant-Whyte and Bailey (2006) list its main issues: each update touches the whole covariance, so the cost grows quadratically with the number of landmarks; a single wrong association is hard to undo; and linearization leads to inconsistency.

FastSLAM (Montemerlo et al., 2002) exploits the fact that landmarks become independent when conditioned on the robot’s path. A Rao–Blackwellized particle filter samples paths, and each particle carries one small Kalman filter per landmark. With a tree that shares unchanged landmark estimates between particles, an update takes $O(M \log K)$ time for $M$ particles and $K$ landmarks; the authors mapped up to 50,000 landmarks in simulation.

Graph-based optimization

Grisetti et al. (2010) credit the graph formulation to Lu and Milios (1997): nodes are poses, and each edge is a relative-pose constraint, from odometry or from aligning two scans, with mean $z_{ij}$ and information matrix $\Omega_{ij}$. Pose-graph optimization finds the poses minimizing

$$F(x) = \sum_{\langle i,j \rangle \in \mathcal{C}} e_{ij}^\top \,\Omega_{ij}\, e_{ij},$$

where $e_{ij}$ compares the measured relative pose $z_{ij}$ with the one predicted by the current $x_i$ and $x_j$ (Grisetti et al., 2010). Since $F$ depends only on relative poses, one pose is fixed to remove the gauge freedom. The normal equations are as sparse as the graph, so Gauss–Newton or Levenberg–Marquardt with sparse Cholesky factorization solves large graphs quickly, with rotations handled by local updates on the manifold.

Incremental smoothing avoids re-solving from scratch. iSAM (Kaess et al., 2008) updates a QR factorization of the sparse information matrix as measurements arrive, reordering variables periodically to limit fill-in; iSAM2 (Kaess et al., 2012) uses a Bayes tree to reorder incrementally and relinearize only the variables that need it. Cadena et al. (2016) describe MAP estimation as the de facto standard, more accurate and efficient than the original filtering approaches, while noting that filters such as the multi-state constraint Kalman filter remain competitive in visual-inertial navigation, where the linearization point is accurate.

Front-end and back-end

Cadena et al. (2016) divide a SLAM system into a front-end, which turns sensor data into factors, and a back-end, which performs inference on them. The sensor-specific front-end matches features, initializes variables, and detects loop closures; the back-end optimizes the factor graph and can feed back to the front-end, for example to verify loop closures.

Keyframe-based visual SLAM

MonoSLAM (Davison et al., 2007) ran EKF-SLAM with a single camera at 30 Hz. Its authors describe it as the first successful application of SLAM methodology to a single uncontrolled camera. The full covariance limited the map to around 100 features, and a target of known size in front of the camera at start-up gave the map its scale.

PTAM (Klein and Murray, 2007) split tracking and mapping into two parallel threads. Tracking poses every frame against the map, while mapping refines a smaller set of keyframes and their points with bundle adjustment at its own pace. PTAM initialized with the five-point algorithm and mapped thousands of points, but targeted small workspaces; Mur-Artal et al. (2015) note that it does not detect large loops.

ORB-SLAM (Mur-Artal et al., 2015) uses ORB features for all tasks, in three threads. Tracking poses each frame against the local map (perspective-n-point refinement); local mapping runs local bundle adjustment over a covisibility graph of keyframes that share points; loop closing detects revisits by bag-of-words place recognition, estimates a similarity transformation that captures accumulated scale drift, and optimizes a sparse pose graph. ORB-SLAM2 (Mur-Artal and Tardós, 2017) added stereo and RGB-D input and a full bundle adjustment after each loop closure; ORB-SLAM3 (Campos et al., 2021) added tightly coupled visual-inertial MAP estimation and multiple maps that are merged when an area is revisited.

Direct and dense SLAM

LSD-SLAM (Engel et al., 2014) aligns image intensities rather than features (the photometric error of visual odometry) and runs on a CPU. It keeps a pose graph of keyframes with semi-dense depth maps, linked by direct $\mathrm{Sim}(3)$ alignment so that scale drift is explicit. DTAM (Newcombe et al., 2011) estimates dense keyframe depth maps by regularized optimization over many frames and tracks by aligning whole images to the dense model, on a GPU.

KinectFusion (Newcombe et al., 2011) fuses the depth maps of an RGB-D camera into a TSDF volume and tracks each new frame against a surface predicted from the model with projective, point-to-plane iterative closest point. Tracking against the model rather than the previous frame keeps drift low in rooms of up to about 7 m³; the authors note that larger scenes would bring back the classic SLAM problems of memory and drift.

LiDAR SLAM

LiDAR SLAM registers successive scans with scan matching such as ICP. LOAM (Zhang and Singh, 2014) splits the problem into odometry at about 10 Hz and registration to the map at about 1 Hz, both matching feature points on sharp edges and planar surfaces; the authors note that it does not perform loop closure.

Loop closure and relocalization

Matching each image against every stored feature does not scale, so bag-of-words methods quantize descriptors into visual words with a vocabulary tree and retrieve similar keyframes through an inverted index (Cadena et al., 2016). DBoW2 (Gálvez-López and Tardós, 2012) does this with binary descriptors, adds a direct index to speed up finding point correspondences, and accepts a loop only after checks of temporal and geometric consistency; geometric verification commonly uses RANSAC. The same retrieval serves relocalization when tracking is lost, with the pose then estimated by perspective-n-point. Because aliasing makes some false loops unavoidable, robust back-ends also judge loop-closure factors by the residuals they induce (Cadena et al., 2016).

Example

The script builds a 2D pose graph for a robot driving a 120 m closed loop in 1 m steps, with odometry noise of 3 cm and 0.5° per step. Dead reckoning chains the odometry; then a place recognizer reports that the last pose is back at the start, and SciPy’s least_squares optimizes the graph with the first pose fixed.

import numpy as np
from scipy.optimize import least_squares

rng = np.random.default_rng(177)

def v2t(p):
    """Pose (x, y, theta) -> 3x3 homogeneous transform."""
    c, s = np.cos(p[2]), np.sin(p[2])
    return np.array([[c, -s, p[0]], [s, c, p[1]], [0, 0, 1]])

def t2v(T):
    return np.array([T[0, 2], T[1, 2], np.arctan2(T[1, 0], T[0, 0])])

def between(pi, pj):
    """Pose of j expressed in the frame of i: X_i^{-1} X_j."""
    return t2v(np.linalg.inv(v2t(pi)) @ v2t(pj))

# Ground truth: 120 steps of 1 m around a closed 120 m loop (two straights, two semicircles)
N = 120
turns = np.zeros(N)
turns[30:60] = turns[90:120] = np.pi / 30            # 30 steps of 6 degrees = one half-turn
gt = [np.zeros(3)]
for k in range(N):
    x, y, th = gt[-1]
    gt.append(np.array([x + np.cos(th), y + np.sin(th), th + turns[k]]))
gt = np.array(gt)                                     # poses 0..N; pose N coincides with pose 0

# Odometry edges (i, i+1) with noise, and one loop-closure edge (N, 0) from place recognition
sig_odo = np.array([0.03, 0.03, np.radians(0.5)])     # per-step std: m, m, rad
sig_loop = np.array([0.05, 0.05, np.radians(1.0)])
edges = []
for i in range(N):
    z = between(gt[i], gt[i + 1]) + sig_odo * rng.normal(size=3)
    edges.append((i, i + 1, z, 1 / sig_odo))
loop = (N, 0, between(gt[N], gt[0]) + sig_loop * rng.normal(size=3), 1 / sig_loop)

def residuals(x, edges):
    poses = np.vstack([np.zeros(3), x.reshape(-1, 3)])  # pose 0 fixed at the origin (gauge)
    r = []
    for i, j, z, w in edges:
        e = t2v(np.linalg.inv(v2t(z)) @ v2t(between(poses[i], poses[j])))  # Z_ij^{-1} X_i^{-1} X_j
        r.append(w * e)                                   # whitened by 1/sigma
    return np.concatenate(r)

def ate(est, gt):
    """RMS position error after least-squares rigid alignment (2D Kabsch)."""
    P, Q = est[:, :2] - est[:, :2].mean(0), gt[:, :2] - gt[:, :2].mean(0)
    U, _, Vt = np.linalg.svd(P.T @ Q)
    D = np.diag([1, np.sign(np.linalg.det(U @ Vt))])
    R = (U @ D @ Vt).T
    return np.sqrt(np.mean(np.sum((P @ R.T - Q) ** 2, axis=1)))

# Dead reckoning: chain the odometry (this is also the optimum without the loop edge)
odo = [np.zeros(3)]
for i, j, z, w in edges:
    odo.append(t2v(v2t(odo[-1]) @ v2t(z)))
odo = np.array(odo)

sol = least_squares(residuals, odo[1:].ravel(), args=(edges + [loop],))
opt = np.vstack([np.zeros(3), sol.x.reshape(-1, 3)])

# Pose 0 and the true pose N are both at the origin, so the end-point error is also the loop gap
for name, est in [("odometry only", odo), ("with loop closure", opt)]:
    chi2_loop = np.sum(residuals(est[1:].ravel(), [loop]) ** 2)
    print(f"{name:17s}: end-point error {np.linalg.norm(est[N, :2] - gt[N, :2]):5.2f} m, "
          f"ATE {ate(est, gt):5.3f} m, chi2 of loop edge {chi2_loop:8.1f}")

Output:

odometry only    : end-point error  1.94 m, ATE 0.549 m, chi2 of loop edge   1470.3
with loop closure: end-point error  0.05 m, ATE 0.251 m, chi2 of loop edge      0.0

Small per-step errors, mostly in heading (about 6° here, typical for 120 steps of 0.5°), add up to a 1.9 m gap at the end of the loop. The loop-closure edge disagrees with the chained odometry by a squared Mahalanobis distance of about 1,500, and the optimizer spreads that discrepancy over the 120 odometry edges according to their uncertainty: the gap closes, and the absolute trajectory error (ATE) roughly halves. The rest cannot be removed, because between the two ends only odometry constrains the poses. A false loop closure would look just as inconsistent, which is why loop closures are verified before they reach the back-end.

Datasets and Benchmarks

• KITTI odometry (Geiger et al., 2012): stereo and LiDAR driving sequences. Mur-Artal and Tardós (2017) note that sequences 00, 02, 05, 06, 07, and 09 contain loops.
• TUM RGB-D (Sturm et al., 2012): Kinect sequences with motion-capture ground truth, including sequences with several loop closures and a large-hall pair recorded with and without a loop.
• EuRoC MAV (Burri et al., 2016): stereo and IMU data from a drone, widely used for visual-inertial SLAM; Campos et al. (2021) reported an RMS ATE of 3.5 cm for stereo-inertial ORB-SLAM3, averaged over its 11 sequences.

Evaluation Metrics

SLAM is evaluated mainly by absolute trajectory error (ATE), which Sturm et al. (2012) provide for global consistency, alongside relative pose error for drift; both, and KITTI’s segment-based errors, are defined in visual odometry. Cadena et al. (2016) add success rate, latency, operating time, and mapped area to the requirements.

Applications

• Mobile robots: museum guide robots that remap when exhibits change, cars that park themselves in a mapped garage, and mobile manipulators in changing factories (Grisetti et al., 2010).
• Augmented reality: phones and headsets tracking against a map of the room.
• Drones, vehicles, and 3D scanning: consistent localization without GPS and globally consistent dense models.

Open Problems

Cadena et al. (2016) consider 2D indoor laser SLAM largely solved and vision-based SLAM on slowly moving robots mature, but conclude that SLAM in general is not solved. Their open problems include recovering from failures such as wrong loop closures; changing environments; metric relocalization across day and night or seasons; automatic parameter tuning; deciding what to remember and forget; multi-robot mapping; resource-constrained platforms; semantic maps; and combining deep learning with SLAM.