Skip to content
Tutorials Center
My learning

Topics

Lesson 29 of 38 Β· Localization and Mapping

SLAM Explained: Mapping While Localizing

Understand Simultaneous Localization and Mapping, why odometry drifts, how loop closure fixes it, and build a tiny pose-graph optimizer in Python.

Localization needs a map. Mapping needs the robot's pose. Drop a robot into an unknown building and you have a chicken-and-egg problem: to build the map it must know where it is, and to know where it is it needs the map. SLAM (Simultaneous Localization and Mapping) solves both at once.

SLAM is what lets a robot vacuum learn your apartment, a warehouse robot build a floor plan on its first day, a drone navigate without GPS, and a phone place virtual furniture on your floor.

Why odometry alone fails

Odometry estimates motion by counting wheel turns (or integrating an IMU, or comparing camera frames). Each step has a tiny error. Because the robot adds each step to the previous estimate, the errors accumulate: this is drift. After a long trip the estimated position can be meters away from the truth, and a map drawn from it shows doubled walls and corridors that never close.

Python Dead reckoning drift around a square

Press Run (or Ctrl + Enter) to see the result here.

Program input Β· what you'd type while the program runs

Type the answers before you press Run, one per line. Each time the program reads a line (with input()), it gets the next line from this box.

The robot drove a closed 40 m square, so the true end point is the start. The estimates miss it by tens of centimeters or more, and the error grows with every lap. SLAM's key insight: when the robot recognizes a place it has seen before, it learns how much it has drifted and can correct the entire trajectory. That event is a loop closure.

The SLAM problem

Unknown pose Where am I right now?
Unknown map What does the world look like?
Coupled Each estimate needs the other.
1 / 2

The main families of SLAM

Family Idea Example systems
EKF-SLAM One big Gaussian over the robot pose and every landmark position Early landmark-based systems; small maps
Particle-filter SLAM (FastSLAM, GMapping) Each particle carries its own trajectory and map 2D lidar grid maps on small robots
Graph-based SLAM Poses are nodes, measurements are constraints; solve a least-squares problem Cartographer, SLAM Toolbox, g2o, GTSAM
Visual and visual-inertial SLAM Track image features, often with an IMU ORB-SLAM, VINS, phone AR
LiDAR odometry and mapping Align 3D point clouds LOAM, LIO-SAM, FAST-LIO

Modern systems are mostly graph-based, because the optimization is accurate and scales well.

Front end and back end

Most SLAM systems split into two parts.

  • Front end: processes raw sensor data. It estimates frame-to-frame motion by scan matching (ICP or correlative matching for lidar) or feature tracking (for cameras), and it detects loop closures by recognizing places (for example with image descriptors or scan context).
  • Back end: takes the resulting constraints and optimizes the whole trajectory (and map) to be as consistent as possible.
flowchart LR
  S["Sensors: lidar, camera, IMU, wheels"] --> F["Front end: scan matching and place recognition"]
  F -->|"odometry and loop-closure constraints"| B["Back end: graph optimization"]
  B --> M["Consistent map and trajectory"]
  M -.->|"better pose"| F

The front end finds constraints; the back end makes them agree.

Pose graph optimization

In a pose graph, each node is a robot pose. Each edge is a measured relative motion, such as "node 7 is 0.5 m ahead of node 6" from odometry, or "node 80 is at the same place as node 0" from a loop closure. Every edge has uncertainty. Optimization finds the poses that minimize the total weighted error across all edges: the least-squares solution.

For simplicity we model only 2D positions (no heading), which makes the problem linear. The mechanics are identical to the real thing.

Python A loop closure corrects the whole path

Press Run (or Ctrl + Enter) to see the result here.

Program input Β· what you'd type while the program runs

Type the answers before you press Run, one per line. Each time the program reads a line (with input()), it gets the next line from this box.

One extra constraint, "the last pose equals the first," pulled the whole trajectory back toward the truth. The optimizer distributed the correction along the path, in proportion to how much each odometry edge was trusted. The weights represent confidence: the loop closure was weighted heavily, the anchor very heavily.

For real robots the poses include heading and the problem is non-linear, so libraries such as g2o, GTSAM and Ceres linearize and iterate (Gauss-Newton or Levenberg-Marquardt). The structure of the problem is the same.

Practical difficulties

  • Data association. Is this landmark one I saw before, or a new one? A wrong match can corrupt the whole map; use robust cost functions and consistency checks.
  • Perceptual aliasing. Identical corridors or office desks cause false loop closures.
  • Dynamic worlds. Moving people and furniture break the assumption of a static world.
  • Scale. The map and graph grow with exploration; systems prune old nodes or use submaps.
  • Sensors. Lidar is accurate indoors but costly. Cameras are cheap but fail in the dark or on blank walls. Fusing an IMU helps with fast motion.

In ROS 2, slam_toolbox (2D lidar graph SLAM) and nav2 give you a working mapping setup. You drive the robot around, save the map as .pgm + .yaml, then switch to localization-only mode using AMCL. See ROS 2 concepts.

Check your understanding

Quick check Β· 4 questions

of 4 answered You scored / 4 Β· perfect!

  1. 1. Why does odometry-only position estimation drift?

    Each step is added to the previous estimate with its error, so the total error grows without bound.
  2. 2. What is a loop closure?

    A loop closure ties the current pose to an old one, letting the optimizer correct the accumulated error.
  3. 3. In graph-based SLAM, what do the nodes and edges represent?

    Poses are the variables and each measurement becomes a weighted constraint between two poses.
  4. 4. What does the SLAM back end do?

    The front end produces constraints; the back end solves for the most consistent estimate.

Summary

  • SLAM estimates the robot's pose and the map together because each needs the other.
  • Odometry drifts; loop closures reveal and correct the drift.
  • Families include EKF, particle-filter, graph-based, visual and lidar-inertial SLAM; graph SLAM is the modern default.
  • A front end finds constraints (scan matching, place recognition); a back end optimizes the pose graph.
  • Data association errors, aliasing and dynamic objects are the hard practical problems.

Next: ROS 2 Concepts: Nodes, Topics, Services and Actions