Python SLAM Algorithms — Core Concepts

The SLAM Problem

SLAM (Simultaneous Localization And Mapping) estimates a robot’s trajectory and a map of its environment at the same time, using only sensor data and motion commands. The robot does not start with a GPS signal or a pre-built map. It must figure out both from scratch.

The problem was formally described in 1986 and has since become one of robotics’ most studied challenges. Every self-driving car, delivery robot, and drone operating indoors uses some form of SLAM.

Three Families of SLAM

1. Filter-Based SLAM (EKF-SLAM)

The earliest approach uses an Extended Kalman Filter to maintain a probability distribution over the robot’s position and all landmark positions simultaneously. The state vector contains the robot pose (x, y, angle) plus every landmark’s coordinates. At each step:

• Predict: Use the motion command (e.g., “drive forward 1 meter, turn 10 degrees”) to predict where the robot should be
• Update: Compare predicted landmark observations with actual sensor readings and correct both the robot position and landmark positions

EKF-SLAM works well for small environments with dozens of landmarks but scales poorly. The state vector grows with every new landmark, and updating it requires matrix operations that become expensive beyond a few hundred landmarks.

2. Particle Filter SLAM (FastSLAM)

Instead of tracking one estimate, particle filter SLAM maintains many possible “guesses” (particles) about where the robot might be. Each particle carries its own robot pose and its own map. Particles that better explain the sensor data survive; poor guesses die off.

FastSLAM decouples the robot pose from the map, meaning each landmark can be tracked independently given a known pose. This dramatically reduces computational cost compared to EKF-SLAM. It handles non-linear motion models and non-Gaussian noise better than Kalman filters.

3. Graph-Based SLAM

The modern dominant approach. Instead of filtering, graph-based SLAM accumulates constraints and optimizes them all at once:

• Nodes represent robot poses at different times
• Edges represent constraints between poses (from odometry or sensor matching)
• When the robot recognizes a previously visited place (loop closure), an edge connects the current node to the earlier one
• A global optimizer adjusts all node positions to best satisfy all constraints simultaneously

This “optimize everything at once” approach produces more accurate maps than filtering, which can only incorporate information forward in time.

Key Concepts

Odometry

Wheel encoders, IMUs, or visual tracking provide estimates of how the robot moved between time steps. Odometry is always noisy — wheels slip, IMU drifts, visual tracking fails in featureless corridors. SLAM works precisely because it does not blindly trust odometry; it corrects drift using sensor observations.

Loop Closure

The moment a robot recognizes it has returned to a previously visited location. Loop closures are critical — they let the algorithm correct accumulated drift in one correction. Without loop closures, maps gradually distort. Detecting loop closures requires matching current observations against stored ones, often using visual feature descriptors or scan matching.

Landmarks vs. Dense Maps

Some SLAM variants track sparse landmarks (distinctive features like corners). Others build dense maps (occupancy grids where each cell is marked as free, occupied, or unknown). Landmark SLAM is lighter but gives less information. Dense SLAM produces complete maps but costs more computation.

Python SLAM Tools

GTSAM (via Python bindings) is the go-to library for graph-based SLAM. Originally from Georgia Tech, it implements factor graphs and nonlinear optimization. g2o has Python bindings too, used by ORB-SLAM and many research systems.

For 2D occupancy grid SLAM, Bresenham’s line algorithm implemented in NumPy can ray-trace laser scans into grid maps. OpenCV provides feature detection and matching for visual SLAM prototyping.

mrob (Mobile Robotics library) offers pure-Python implementations of EKF-SLAM and pose graph optimization for educational purposes.

Common Misconception

“SLAM gives you a perfect map.” SLAM maps are probabilistic estimates, not ground truth. They contain errors, especially in areas without loop closures. Long corridors with few distinctive features produce the worst maps because the algorithm has little to anchor corrections to. Real deployments combine SLAM with other information sources (GPS outdoors, floor plans indoors) to improve reliability.

One thing to remember: Modern SLAM builds a graph of robot poses connected by motion and observation constraints, then optimizes the entire graph at once — producing maps that self-correct when the robot revisits known locations.