SLAM in Robotics: How Robots Map and Localize Themselves | Knowledge Hub

Core Engineering Takeaways

•SLAM (Simultaneous Localization and Mapping) solves the circular dependency where a robot needs an accurate map to localize, and needs accurate localization to build a map.
•Dead reckoning from wheel encoders and IMUs inevitably drifts over time due to wheel slip; SLAM uses external environmental observations to bound and eliminate that drift.
•2D LiDAR SLAM (e.g., slam_toolbox, Cartographer) provides robust millimeter accuracy in structured planar indoor environments.
•Visual SLAM (e.g., ORB-SLAM3) uses cameras to track 3D optical feature points, but suffers from motion blur, scale ambiguity (in monocular setups), and low-texture lighting failures.
•The Front-End extracts features and computes high-rate frame-to-frame relative odometry (scan matching).
•The Back-End builds a Factor Graph of robot poses connected by spatial constraints, running non-linear least squares optimization (g2o, GTSAM) to minimize global error.
•Loop closure is the critical event where the robot recognizes a location it visited minutes or hours earlier, allowing the back-end optimizer to retroactively pull all drifting poses back into alignment.

Prerequisites

Linear algebra (homogeneous transformations, rotation matrices)
Probability fundamentals (Gaussian distributions, Bayes theorem, maximum likelihood)

1. The Fundamental SLAM Problem: The Spatial Chicken-and-Egg Dilemma

When a human walks into an unfamiliar convention center, they build a mental map of exits, booths, and stairs while continuously tracking where they are inside that mental map.

For an autonomous robot, this is the classic chicken-and-egg problem:

To build an accurate map: The robot must know exactly where its sensors were located in space at every instant a measurement was taken.
To know where it is located (localization): The robot must compare its current sensor observations against a known, accurate map of the environment.

When both the robot pose trajectory $\mathbf{x}_{1:t}$ and the environmental map $\mathbf{m}$ are unknown, errors compound rapidly. If wheel encoders slip by 2 degrees on a rug, every wall laser point recorded thereafter is drawn at a 2-degree slant. Without SLAM algorithms to detect and correct this error, the resulting map quickly deteriorates into a scrambled, useless blur.

2. Localization vs Mapping vs Full SLAM: Scope Defined

Engineers divide spatial navigation into three distinct paradigms:

1
Pure Localization (AMCL / Monte Carlo Localization):

*Given:* A pre-existing, perfect, static map of the building.
*Task:* Estimate the robot's pose $(x, y, \theta)$ inside that map using particle filters and current laser scans.

2
Pure Mapping (Mapping with Known Poses):

*Given:* Ground truth robot poses (e.g., from an external high-precision optical Vicon motion capture system).
*Task:* Project laser beams into space to generate an occupancy grid or 3D mesh.

3
Full SLAM (Simultaneous Localization and Mapping):

*Given:* Only the initial starting point $(0,0,0)$ and raw, noisy onboard sensor streams.
*Task:* Reconstruct the environment map while simultaneously calculating the robot's trajectory.

3. Sensor Modalities: 2D LiDAR, 3D Point Clouds & Visual SLAM

Different robots select different physical sensor modalities for SLAM:

1. 2D LiDAR SLAM (Planar Laser Scanners)

Hardware: 360-degree spinning single-beam laser rangefinder (RPLiDAR, Sick, Hokuyo).
Strengths: Millimeter distance accuracy, immune to ambient lighting conditions, low compute requirements (runs easily on a Raspberry Pi 4).
Weaknesses: Blind to 3D obstacles below or above the horizontal scan plane (e.g., table legs, low wires, downward stairs).
Popular Algorithms: slam_toolbox, Google Cartographer, Hector SLAM.

2. 3D LiDAR SLAM (Multi-Beam Spatial Scanners)

Hardware: 16 to 128-beam spinning or solid-state LiDARs (Velodyne, Ouster, Hesai).
Strengths: Full 3D volumetric point clouds, long range (100-200m), ideal for autonomous cars, drones, and rugged quadrupeds.
Popular Algorithms: LIO-SAM (LiDAR-Inertial Odometry), Fast-LIO2, LOAM.

3. Visual SLAM (vSLAM)

Hardware: Monocular, Stereo, or RGB-D active infrared cameras (Intel RealSense, Stereolabs ZED).
Strengths: Rich semantic information (can detect *what* an object is, not just that an obstacle exists), low cost, compact form factor.
Weaknesses: Fails in dark or featureless environments (blank white walls), sensitive to motion blur during aggressive rotations.
Popular Algorithms: ORB-SLAM3, RTAB-Map, Kimera.

4. SLAM Front-End: Scan Matching with ICP & Correlative Methods

The SLAM pipeline is split into a Front-End and a Back-End. The Front-End's job is Scan Matching: taking two consecutive sensor frames and finding the exact translation $(\Delta x, \Delta y)$ and rotation $\Delta \theta$ that aligns them best.

The ICP (Iterative Closest Point) Algorithm

Given a source point cloud $\mathcal{P} = \{p_1, \dots, p_N\}$ and a target reference cloud $\mathcal{Q} = \{q_1, \dots, q_M\}$ :

1
Associate: For each point $p_i \in \mathcal{P}$ , find the closest Euclidean distance neighbor $q_j \in \mathcal{Q}$ .
2
Estimate Transform: Compute the optimal rotation $\mathbf{R}$ and translation $\mathbf{t}$ that minimizes the sum of squared distances:

E(\mathbf{R}, \mathbf{t}) = \sum_{i=1}^N \| \mathbf{R} p_i + \mathbf{t} - q_{\text{match}(i)} \|^2

3
Transform & Iterate: Apply the estimated $(\mathbf{R}, \mathbf{t})$ to $\mathcal{P}$ and repeat until the error change falls below an epsilon threshold.

Iterative Closest Point (ICP) Objective Function

\min_{\mathbf{R}, \mathbf{t}} \sum_{i=1}^N \| (\mathbf{R} p_i + \mathbf{t}) - q_i \|^2

In Simple Words: Find the rotation matrix R and translation vector t that minimizes the Euclidean distance between matched corresponding points.

Variables Breakdown:

p_i

Point coordinates from current sensor scanUnit: meters

q_i

Closest corresponding point in previous reference scanUnit: meters

\mathbf{R}

Orthonormal 2D/3D rotation matrixUnit: dimensionless

\mathbf{t}

Translation displacement vectorUnit: meters

Worked Example:

Aligning a 360-point laser scan from step k to step k-1 by solving via Singular Value Decomposition (SVD) of the cross-covariance matrix.

5. SLAM Back-End: Pose Graph Optimization & Non-Linear Least Squares

Scan matching is imperfect. Small errors (0.1° per step) accumulate as the robot travels. After a 500-meter loop through a warehouse, dead reckoning drift can reach 15 meters.

The Factor Graph Representation

The Back-End treats SLAM as a Graph Optimization Problem:

Nodes (Vertices): The discrete poses of the robot over time: $\mathbf{x}_0, \mathbf{x}_1, \mathbf{x}_2, \dots, \mathbf{x}_n$ .
Edges (Factors): Spatial constraints connecting nodes.
*Odometry Edges:* Connect consecutive nodes $(\mathbf{x}_k, \mathbf{x}_{k+1})$ based on scan matching and wheel encoders.
*Loop Closure Edges:* Connect non-consecutive nodes $(\mathbf{x}_k, \mathbf{x}_{500})$ when the robot revisits a previously mapped location.

The Back-End solver (e.g., GTSAM, g2o, or Ceres Solver) sets up a massive sparse non-linear system and solves for the configuration of all node poses that minimizes the global stress across all spring-like edges.

6. Loop Closure: Eliminating Accumulated Drift to Restore Global Consistency

Loop Closure is the defining superpower of SLAM algorithms.

Imagine a robot navigating around a square city block. As it turns corners 1, 2, and 3, odometry drift accumulates. When it returns to corner 1, raw odometry claims it is 10 meters away from where it started.

How Loop Closure Works:

1
Candidate Detection: The robot queries a spatial or visual index (such as DBoW2 for visual features, or Scan Context for LiDAR) to detect if the current scan matches an old pose.
2
Geometric Verification: The front-end runs an aggressive scan-matching check. If the match confidence exceeds a strict threshold (e.g., 95%), a Loop Closure Edge is added between the current node and the initial node.
3
Graph Relaxation: The back-end optimizer executes Levenberg-Marquardt optimization. It distributes the 10-meter error backwards across the entire trajectory, snapping walls into straight alignment and eliminating double-walls.

7. Map Representations: 2D Occupancy Grids, OctoMaps & Voxel Meshes

Once poses are optimized, the robot projects its raw range beams into a usable spatial representation for path planning.

2D Occupancy Grid Maps

The world is discretized into a uniform grid of cells (typically $5\text{ cm} \times 5\text{ cm}$ ). Each cell stores a probability $P(m_i)$ of being occupied:

$P = 0.0$ : Known Free Space (laser passed through)
$P = 0.5$ : Unknown Space (never observed)
$P = 1.0$ : Known Obstacle (laser hit a solid barrier)

To avoid numerical floating-point underflow when multiplying probabilities, SLAM uses the Log-Odds formulation:

l(m_i) = \log\left(\frac{P(m_i)}{1 - P(m_i)}\right)

Updating a cell with a new laser observation simply requires adding a constant log-odds value:

l_{t}(m_i) = l_{t-1}(m_i) + l_{\text{sensor}} - l_0

occupancy_grid_update.py

python

import numpy as np

class OccupancyGridMap:
    def __init__(self, width_m: float, height_m: float, resolution_m: float = 0.05):
        self.resolution = resolution_m
        self.width_cells = int(width_m / resolution_m)
        self.height_cells = int(height_m / resolution_m)
        # Initialize log-odds grid to 0 (P = 0.5 unknown)
        self.log_odds = np.zeros((self.height_cells, self.width_cells), dtype=np.float32)
        
        self.L_OCC = 0.85   # Log-odds increase when laser detects obstacle
        self.L_FREE = -0.40 # Log-odds decrease along free ray
        self.L_CLAMP_MAX = 5.0
        self.L_CLAMP_MIN = -5.0

    def world_to_grid(self, wx: float, wy: float) -> tuple[int, int]:
        gx = int(wx / self.resolution)
        gy = int(wy / self.resolution)
        return gx, gy

    def update_ray(self, robot_x: float, robot_y: float, hit_x: float, hit_y: float):
        """Bresenham ray trace: mark free cells along line, occupied at endpoint."""
        r_gx, r_gy = self.world_to_grid(robot_x, robot_y)
        h_gx, h_gy = self.world_to_grid(hit_x, hit_y)

        # Mark obstacle endpoint
        if 0 <= h_gx < self.width_cells and 0 <= h_gy < self.height_cells:
            self.log_odds[h_gy, h_gx] = np.clip(
                self.log_odds[h_gy, h_gx] + self.L_OCC, 
                self.L_CLAMP_MIN, self.L_CLAMP_MAX
            )

    def get_probability_grid(self) -> np.ndarray:
        """Convert log-odds back to human-readable 0.0 - 1.0 probabilities."""
        return 1.0 - (1.0 / (1.0 + np.exp(self.log_odds)))

Frequently Asked Questions

Common Questions & Troubleshooting

Q:Why does SLAM fail in long, featureless hallways?

In a featureless straight corridor, 2D laser scans look completely identical regardless of how far forward the robot moves. The scan matcher can constrain lateral (Y) and heading (θ) error against the side walls, but has no geometric landmarks to constrain forward (X) translation. This is known as the "corridor degeneracy problem". Fusing wheel encoders and IMU data solves this.

Q:What is the difference between Graph-SLAM and Particle Filter SLAM (FastSLAM)?

Particle Filter SLAM (like Gmapping) tracks multiple potential robot trajectories using weighted particles. It scales poorly to large spaces because memory and compute grow with the square of the area. Graph-SLAM (like slam_toolbox and Cartographer) represents poses as factor graph nodes, scaling efficiently to massive environments using sparse matrix optimization.

Q:Can a robot do SLAM with a single monocular webcam?

Yes (via Monocular Visual SLAM like ORB-SLAM), but with one major physics caveat: scale ambiguity. A single camera cannot distinguish between a 2-meter tall doorway 5 meters away, or a 20-centimeter dollhouse doorway 50 centimeters away. Monocular SLAM requires sensor fusion with an IMU (Visual-Inertial Odometry / VIO) or known baseline wheel movements to recover true metric scale.

Previous ArticleRobot Operating Systems: ROS 2 Architecture Explained from Hardware to Applications Next ArticleComputer Vision in Robotics: Cameras, OpenCV, Object Detection & Depth