SLAM & Mapping

Week 11 • CMPSC 304 Robotic Agents

Press Space or → to advance

Where We Are

Activity 5
Nodes, Topics

→

Today
SLAM & Mapping

→

Project 4
Full Pipeline

Activity 5: You drove a robot, inspected topics — you made a robot move
Today: Where is the robot? What does the world look like? — building a map
P4 Part 1: You'll build a complete map of a simulated environment

The Mapping Problem

Core question: How does a robot build a representation of its environment using only sensor data?

The robot starts with no map
It has sensors (lidar, cameras, etc.) that measure local surroundings
It needs to combine many local measurements into a global map
But to combine them correctly, it needs to know where it was for each measurement

Chicken-and-egg problem: To build a map, you need to know where you are. To know where you are, you need a map.

SLAM: Simultaneous Localization and Mapping

SLAM solves both problems at once: build the map and track position simultaneously.

Sensor Data
(lidar scans)

→

SLAM
Algorithm

→

Map +
Robot Pose

Inputs: sensor readings (lidar), odometry (wheel encoders)
Outputs: a map of the environment + the robot's estimated trajectory
The algorithm iterates: each new scan updates both the map and the pose estimate

Lidar: How the Robot "Sees"

How It Works

Emits laser beams in a 360° sweep
Measures time of flight for each beam
Converts to distance measurements
TurtleBot3 LDS-02: 360 samples/revolution
Published on /scan topic

What You Get

Array of distances at known angles
Range: 0.12m to 3.5m (TurtleBot3)
Close objects → short distances
Open space → max range or inf
Visualized as red dots in RViz

ros2 topic echo /scan --once   # See one lidar scan message

TIME OF FLIGHT (ToF): The sensor fires a laser pulse and records the time until reflected light returns. Distance = (speed of light × travel time) / 2. Speed of light ≈ 3×10⁸ m/s, so a 1 nanosecond round trip = 15 cm. This nanosecond-level timing gives centimeter-level distance accuracy. HOW LIDAR WORKS MECHANICALLY: A rotating platform (or MEMS mirror) sweeps the laser through 360°. For each angular step, it fires a pulse and records the return time. One full rotation = one complete scan. In ROS2 this arrives as a LaserScan message on /scan containing: angle_min, angle_max, angle_increment, range_min, range_max, and an array ranges[] of distances — one entry per angular step. TURTLEBOT3 (what students use in Docker): - LDS-02 lidar - Range: 0.12 m – 3.5 m - 360 samples per revolution (~1° angular resolution) - ~5 Hz scan rate, typical map resolution: 5 cm/pixel TURTLEBOT4 (what alic.pgm was built with in Activity 6): - RPLidar C1 - Range: 0.1 m – 12 m (much longer!) - ~1,080 samples per revolution (3× more angular density) - 10 Hz scan rate - This is why alic.pgm has 3 cm/pixel resolution instead of the typical 5 cm Common confusion: /scan returns 'inf' for any direction where nothing was detected within max range — this is normal, not an error.

Occupancy Grids: The Map Representation

An occupancy grid divides the world into a grid of cells. Each cell stores the probability that it is occupied.

1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 0.0 0.0 0.0 0.0 0.0 0.0 1.0 1.0 0.0 ? ? ? 0.0 0.0 1.0 1.0 0.0 ? ? ? 0.0 0.0 1.0 1.0 0.0 0.0 0.0 0.0 0.0 0.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0

Value	Meaning	Color in Map
0.0 (free)	Definitely empty / traversable	White
1.0 (occupied)	Definitely an obstacle / wall	Black
-1 / unknown	Not yet observed	Gray

WHAT IS PROBABILITY HERE: Each cell stores P(occupied) ∈ [0.0, 1.0]. 0.0 = certain this cell is free. 1.0 = certain it contains an obstacle. 0.5 = no information at all. ALL CELLS START AT 0.5 (complete uncertainty → rendered gray). As the robot drives and scans, cells update toward 0.0 (free/white) or 1.0 (occupied/black). ACTIVITY 6 PART 1 — PIXEL SHADE MEANINGS (answers): - BLACK pixels: walls/obstacles. Low PGM value (near 0) → occupancy probability above occupied_thresh (0.65) → drawn black. - WHITE pixels: free traversable space. High PGM value (near 255) → occupancy probability below free_thresh (0.25) → drawn white. - GRAY pixels: unknown / never scanned. The robot's lidar beam never reached those cells during the mapping session. PGM FORMAT NOTE: PGM stores values 0–255 where 0 = black. This is inverted from raw probability — more "ink" means more obstacle. The map_server handles this conversion automatically using the negate field. The grid-demo on this slide: black border = walls, white interior = mapped free space, gray = unmapped area (e.g., behind a wall the robot never went around).

How Cells Get Updated

Each cell starts at 0.5 (unknown). As the robot scans, cells are updated using Bayes' rule.

Lidar beam hits an obstacle → cell at that distance gets more occupied (probability increases toward 1.0)
Lidar beam passes through → cells along the beam path get more free (probability decreases toward 0.0)
Multiple observations of the same cell → probability converges to the true state

Log-odds trick: Instead of multiplying small probabilities (numerical issues), SLAM implementations use log-odds: l = log(p / (1-p)). Additions in log-odds space = multiplications in probability space.

WHAT IS PROBABILITY (in this context): A number 0.0–1.0 representing how likely something is. P=0 means impossible, P=1 means certain, P=0.5 means "I have no idea." BAYES' RULE: P(A|B) = P(B|A) × P(A) / P(B) In mapping: A = "this cell is occupied", B = "I got this lidar reading at this distance" - P(A) = prior: current belief about the cell (starts at 0.5 for all cells) - P(B|A) = likelihood: how probable is this lidar reading IF the cell really is occupied? - P(A|B) = posterior: updated belief after seeing the new reading INTUITION: A lidar "hit" at a cell increases that cell's probability toward 1.0. A beam that passes THROUGH a cell (before hitting something further away) decreases that cell's probability toward 0.0 — the cell must be free for the beam to have passed through it. LOG-ODDS TRICK: - l = 0 corresponds to p = 0.5 (uncertain) - l positive → p > 0.5 (probably occupied) - l negative → p < 0.5 (probably free) - Each "hit" observation adds a fixed positive value to l; each "pass-through" subtracts - Saturate at ±10 to prevent cells from locking permanently - Convert back: p = 1 / (1 + e^(−l))

Scan Matching: Aligning Scans

How does SLAM know where the robot moved between scans?

Odometry gives an initial guess (from wheel encoders) — but it drifts
Scan matching aligns consecutive lidar scans to refine the pose
Algorithm: find the rotation & translation that best aligns scan B to scan A

Scan at
time t

→

Scan
Matching

→

Scan at
time t+1

→

Refined
Pose

ICP (Iterative Closest Point) is a classic scan matching algorithm: iteratively finds closest points between scans, computes optimal transform, repeats until convergence.

ICP STEP BY STEP: 1. Take scan A (time t) and scan B (time t+1). 2. For each point in B, find the closest point in A — these are "corresponding pairs." 3. Compute the rotation (θ) and translation (x, y) that minimizes the sum of squared distances between all pairs. 4. Apply this transform to B. Repeat until the change drops below a threshold (convergence). WHY ODOMETRY ALONE FAILS: Wheel encoders measure revolutions and convert to distance assuming no wheel slip. On carpet or with sharp turns, slip introduces ~1–2% error per meter. After 10 m of travel, you can be off by 10–20 cm. Over a full room, the map tears apart and walls no longer align. CARTOGRAPHER'S APPROACH — SCAN-TO-SUBMAP: Each new scan is matched against a locally built "submap" (a small occupancy grid from recent scans). This averages out noise in individual scans and is far more robust than scan-to-scan matching. OTHER ALGORITHMS: - NDT (Normal Distributions Transform): fits Gaussians to local regions, then maximizes likelihood. Faster than ICP for dense scans. - Feature-based: extracts corners/edges, matches them between scans. Very fast but fails in featureless corridors.

Loop Closure: Fixing Accumulated Error

Even with scan matching, small errors accumulate over time
After a long drive, the robot's position estimate drifts from truth
Loop closure: the robot returns to a previously visited location

When the SLAM algorithm detects it has returned to a known place, it distributes the accumulated error back across the entire trajectory, correcting the map.

Before Loop Closure

Walls don't quite line up
Rooms appear slightly shifted
Corridors may look bent

After Loop Closure

Walls snap into alignment
Room shapes correct
Map becomes globally consistent

POSE GRAPH OPTIMIZATION — what Cartographer runs when it detects a loop closure: - Every robot pose (position + orientation at a moment in time) is a NODE in a graph. - Every scan match between consecutive poses is an EDGE with a relative transform constraint (Δx, Δy, Δθ) and an uncertainty value. - Loop closure adds a new EDGE between the current node and a historical node far back in the graph: "these two poses are actually the same place." - OPTIMIZATION: find the set of all N pose positions that satisfies all edge constraints with minimum total error. This is a nonlinear least-squares problem solved by Ceres Solver inside Cartographer. - Result: all poses shift slightly to be globally consistent — no single dramatic snap, but small adjustments distributed across every pose since the divergence point. ANALOGY: Imagine a chain of paper clips. Each clip is a pose, connected to the next with a known relative position (the scan match edge). Loop closure is like connecting the last clip back to an earlier one in the chain — suddenly the whole chain must rearrange to satisfy all connections simultaneously. PRACTICAL TIP FOR P4: If students don't drive back toward their starting area, they won't trigger a loop closure, and maps may show walls that don't quite close at corners. Encourage them to complete a full circuit and revisit the start area.

Google Cartographer

Cartographer is the SLAM algorithm installed in our Docker environment.

Developed by Google, open-sourced in 2016
Uses a submap approach: builds small local maps, then optimizes their alignment
Handles loop closure through pose graph optimization
Works with 2D lidar (our TurtleBot3) or 3D lidar

# Launch Cartographer with TurtleBot3
ros2 launch turtlebot3_cartographer cartographer.launch.py

What you'll see in RViz: The map builds in real-time as you drive. Gray = unknown, white = free, black = obstacles. The robot's position is tracked by a small arrow.

Saving & Using Maps

Once mapping is complete, save the map for later use (navigation).

# Save the current map
ros2 run nav2_map_server map_saver_cli -f ~/map

This creates two files:

File	Contents
`map.pgm`	Grayscale image — each pixel is one grid cell
`map.yaml`	Metadata: resolution (m/pixel), origin, thresholds

# alic.yaml — the Activity 6 map file:
image: alic.pgm
mode: trinary
resolution: 0.03        # 3 cm per pixel (TurtleBot4 quality)
origin: [-8.64, -10.4, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

ACTIVITY 6 PART 1 — COMPLETE ANSWERS: RESOLUTION: 0.03 m/pixel = 3 cm per pixel. This is a TurtleBot4-quality map. TurtleBot3 maps typically use 0.05 m = 5 cm/pixel due to the shorter lidar range. ORIGIN: [-8.64, -10.4, 0] is the [x, y, θ] of the BOTTOM-LEFT corner of the PGM image in real-world coordinates. The robot started mapping near [0, 0] in world space. The image extends 8.64 m to the left of start and 10.4 m below. θ=0 means no rotation. IMPORTANT: the origin is NOT where the robot started — it's the corner of the bounding image box that was allocated to hold all explored area. THRESHOLDS: - occupied_thresh 0.65: cells with probability > 65% → drawn BLACK (wall/obstacle) - free_thresh 0.25: cells with probability < 25% → drawn WHITE (free space) - Between 0.25 and 0.65 → drawn GRAY (unknown) HOW TO FIND PIXEL DIMENSIONS OF alic.pgm: Mac: sips -g pixelWidth -g pixelHeight alic.pgm Output: pixelWidth: 414 / pixelHeight: 553 Windows: right-click alic.pgm → Properties → Details tab REAL-WORLD AREA CALCULATION: Width: 414 pixels × 0.03 m/pixel = 12.42 m Height: 553 pixels × 0.03 m/pixel = 16.59 m Total bounding area ≈ 12.42 × 16.59 ≈ 206 m² (includes walls and unmapped interior regions)

Tips for Building Good Maps

✓ Do

Drive slowly — fast motion distorts scans
Cover the entire area methodically
Return to start for loop closure
Drive close to walls for clear readings
Make smooth turns

✗ Don't

Race through corridors
Make sharp, jerky turns
Leave large unknown areas
Spin in place (confuses odometry)
Stop mapping before revisiting areas

ROS2 Topics During SLAM

During mapping, multiple nodes communicate through these key topics:

Topic	Message Type	Purpose
`/scan`	LaserScan	Lidar distance readings
`/odom`	Odometry	Wheel encoder pose estimate
`/map`	OccupancyGrid	The map being built
`/tf`	TransformStamped	Coordinate frame transforms
`/cmd_vel`	TwistStamped	Velocity commands (your teleop input)

# Inspect Cartographer's subscriptions:
ros2 node info /cartographer_node

Textbook Connection: IAR Chapter 13

Section 13.1–13.2: Map representations — occupancy grids are one of several options (topological maps, feature maps)
Section 13.3: Probabilistic grid mapping — the Bayesian update rule we discussed
Section 13.4: SLAM problem formulation — why it's "one of the most important problems in robotics"

Key insight from the textbook: Mapping is fundamentally a probabilistic problem. Sensor noise, odometry drift, and environmental ambiguity mean we can never be 100% certain about any cell. The occupancy grid elegantly encodes this uncertainty.

Summary

Concept	What It Does
Lidar	Measures distances to obstacles in 360°
Occupancy Grid	Grid-based map — each cell: occupied, free, or unknown
SLAM	Builds map while tracking robot position
Scan Matching	Aligns consecutive scans to estimate motion
Loop Closure	Corrects drift when revisiting known areas
Cartographer	Google's SLAM algorithm — what we use in class

Activity 6: A real map (alic.pgm) is already in your repo — decode it. What do the pixel shades mean? What real-world area is mapped? Then trace the live SLAM data flow in Docker. → Activity 6