EECE 5550 Mobile Robotics

Lecture 7

Mapping and SLAM

Dr. Xian Li

Department of Electrical and Computer Engineering

Northeastern University

Back to the question

Lecture 6 localized against a map. Who made the map?

Mapping, poses knownIf someone tells you exactly where the robot was for every scan, a map is bookkeeping: paint each scan into a grid.

Localization, map knownLecture 6. Given the map, a particle filter finds the pose.

Both unknown: SLAMEvery pose error bends the map, and every map error misplaces the next pose. You have to estimate them together.

The trap. Odometry drifts (Lecture 4). Paint scans at odometry poses and the walls smear and double. SLAM fixes the poses using the scans themselves, then paints the map.

Your Lab 4 robot navigates on a map. In Lab 3 you build it.

2

Today

A map is a model of the world, and it is only as straight as the poses it was painted from.

Maps. Four representations, and the one Nav2 plans on.
Occupancy grids. Log-odds per cell, the inverse sensor model, and what a cell size costs.
Maps in ROS 2. The OccupancyGrid message, and the files map_server loads.
The SLAM problem. Why the poses and the map must be estimated together.
Scan matching. ICP and correlative matching: two scans give a relative pose.
Pose graphs and loop closure. Least squares over the trajectory, and slam_toolbox on your TurtleBot 4.

Reading

Thrun, Burgard, and Fox, Probabilistic Robotics, Ch. 9 (occupancy grid mapping) and Ch. 11 (GraphSLAM)
Elfes, occupancy grids, IEEE Computer 1989
Hornung et al., OctoMap, Autonomous Robots 2013
Grisetti, Kümmerle, Stachniss, and Burgard, A tutorial on graph-based SLAM, IEEE ITS Magazine, 2010
Olson, Real-time correlative scan matching, ICRA 2009
Macenski and Jambrecic, SLAM Toolbox: SLAM for the dynamic world, JOSS, 2021

3

Mapping: the same room, four ways

Lecture 6's likelihood field needed a map, and so does every planner. Which representation should a TurtleBot 4 keep?

Localization assumes a known map and estimates the pose. Mapping assumes known poses and estimates the map. SLAM, the second half of today, does both at once.

4

Four representations

No map is best. Each answers some questions cheaply and others not at all.

Map	Stores	Good for	Weak at
Occupancy grid	P(occupied) per cell	planning, lidar localization	memory in large or 3D spaces
Landmarks	positions of distinct features	EKF localization and SLAM	free space: it says nothing about it
Topological graph	places and connections	long routes, "go to room 122"	exact geometry
Point cloud, voxels	raw 3D points, or 3D cells	perception, 3D obstacles	size, and no free space unless you add it

Nav2 uses grids for planning and control. Lectures 10 and 11 add 3D; Lecture 12 needs a graph for exploration.

5

Occupancy grids

Elfes, 1989. Divide the floor into cells; each one stores the probability that it is occupied.

l_{t, i} = l_{t - 1, i} + \log \frac{p (m_{i} | z_{t}, x_{t})}{1 - p (m_{i} | z_{t}, x_{t})} - l_{0}

p = 1 - \frac{1}{1 + e^{l}}

Independent cellseach cell is its own binary Bayes filter

Known posesx_t is given: from odometry, or SLAM (later today)

Static worldpeople who walk through are noise

More: ·

6

Try it: one beam, many scans

The wall is at 2.2 m. Scan, then let a person stand in the beam and walk away. How long does the ghost persist?

The sensor model

range noise (m)clamp |l| at

7

Resolution: what a cell costs

Every planner, costmap, and localizer pays for the cell size you choose here.

Halve the cell4× the cells in 2D, 8× in 3D

Too coarsedoorways close, walls thicken by half a cell

Too finemore noise per cell, slower planning everywhere downstream

3D needs a treeOctoMap stores only the cells that matter

8

Try it: does the doorway survive?

The real doorway is wide enough. Coarsen the grid until the map says it is not.

The map

cell size (m)doorway (m)

The robot

radius (m)

■ wall cells■ too close for the robot's center▬ a way through, if one exists

9

Maps in ROS 2

The grid is a message. The files on disk are only its picture.

nav_msgs/msg/OccupancyGrid

header.frame_id # mapinfo.resolution # meters per cellinfo.width, height # cellsinfo.origin # pose of cell (0, 0)data # int8, row by row from the origin

map_serverloads a YAML and PGM, publishes /map

map_saver_clisaves /map as YAML and PGM

Later todayslam_toolbox publishes /map while it builds

10

The files Nav2 loads

A saved map is two files: an image and the YAML that says how to read it. The values above are the map_saver defaults.

room122.yaml

image: room122.pgmmode: trinaryresolution: 0.05 # m per cellorigin: [-4.2, -3.1, 0.0] # lower-left cellnegate: 0occupied_thresh: 0.65free_thresh: 0.25

p = \frac{255 - x}{255}

Each pixel x of the PGM becomes a probability p (with negate: 0).

p	In trinary mode	Pixel in the PGM
above 0.65	occupied, cost 100	black
below 0.25	free, cost 0	white
in between	unknown, −1	gray (205)

The origin is the trap. It places the lower-left pixel in the map frame. Crop the image in an editor and you must change the origin too, or every pose shifts.

You may clean a map by hand: paint out a person who walked through, or close a gap at a doorway. Keep the original, and say what you changed.

The YAML format and map_saver_cli options: nav2_map_server README (Jazzy).

11

Try it: build a map from scans

Mapping with known poses works. Turn up the drift and clear the map: painted from odometry, the room smears and the walls double.

The inverse sensor model

l_occl_free

The poses you paint from

odometry drift

black: occupiedgray: unknownwhite: free● where the robot thinks it is

12

What SLAM estimates

Two questions with the same data. They differ in whether you keep the past.

p (x_{1 : t}, m | z_{1 : t}, u_{1 : t})

Full SLAM: the whole trajectory and the map, given every scan and every odometry step. Graph-based SLAM solves this one.

p (x_{t}, m | z_{1 : t}, u_{1 : t})

Online SLAM: only the current pose and the map. Filters (EKF-SLAM, particle filters) solve this one.

Family	State	Strength	Weakness
EKF-SLAM	pose plus landmark positions, one Gaussian	exact for small linear problems	cost grows as the square of the number of landmarks; one bad association is permanent
Particle filter (FastSLAM, GMapping)	particles, each with its own map	multimodal, handles ambiguity	memory per particle; particle depletion on long loops
Graph-based (Karto, Cartographer, slam_toolbox)	every pose, linked by constraints	sparse, re-optimizes the whole past at loop closure	needs a good front end; a false loop closure bends everything

The hard part is not the math. It is data association: deciding that this wall is the wall you saw ten minutes ago.

13

Scan matching: two scans give a relative pose

The front end of every lidar SLAM system.

ICP: iterative closest point

Pair each point p_i of the new scan with the closest point q_i of the old one, solve for the rigid motion, repeat until it stops moving.

\min_{R, t} \sum_{i} ‖^{} R p_{i} + t - q_{i} ‖^{2}

With pairs fixed, the answer is closed form. Center both sets, take the SVD of H = Σ (p_i − p̄)(q_i − q̄)^T = UΣV^T; then R = VU^T and t = q̄ − Rp̄ (Arun et al., 1987).

Fast and precise near the answer. From a bad start it locks onto the wrong pairs, and nothing tells you.

Correlative scan matching slam_toolbox

Rasterize the old scans into a lookup grid of likelihoods. Try every pose in a window around the odometry guess, coarse then fine, and score each one (Olson, 2009).

Brute force, but robust to a poor guess, and the shape of the score around the best pose gives a covariance for free. That covariance becomes the edge's information in the pose graph.

Both answer the same question: given two scans, how did the robot move between them? The answer is a measurement, with an uncertainty, of a relative pose.

14

Try it: ICP aligns two scans

Pair nearest points, solve for the motion, repeat. A good guess converges; a bad one settles somewhere wrong.

● old scan (reference)● new scan, at the current estimate─ nearest-neighbor pairs- - true walls

The initial guess (error from the truth)

Δx (cm)Δy (cm)Δθ (°)

Pairing

max pair distance (m)

15

Try it: correlative matching and its covariance

Score every shift in a window: the peak is the match, and the shape of the score is its uncertainty.

● old scan● new scan at the best match◯ 2σ covariance of the score

The score

blur σ (m)

Olson, 2009. Rasterize the old scan into a blurred grid, then score every candidate shift (and, in slam_toolbox, every rotation) by how well the new scan lands on it. The spread of the score is the match's covariance; its inverse becomes the edge information Ω in the pose graph.

16

Pose graphs

Grisetti et al., 2010. Forget the map while you optimize: estimate the poses, then paint.

Nodes are robot poses x_i. Edges are measured relative poses z_ij with Ω_ij: from odometry between neighbors, from scan matching, and from loop closures between poses far apart in time but close in space.

e_{i j} = t2v ({Z_{i j}}^{- 1} ({X_{i}}^{- 1} X_{j}))

x^{*} = \arg \min_{x} \sum_{i j} {e_{i j}}^{T} Ω_{i j} e_{i j}

: linearize every error, build HΔx = −b, solve, repeat. H is sparse (each edge touches two poses), so thousands of poses solve in milliseconds. slam_toolbox uses Ceres for this step.

Front endTurns sensor data into edges: odometry, scan matches, loop-closure candidates. Its mistakes are the expensive ones.

Back endFinds the poses that best satisfy every edge at once. Pure least squares, and blind to whether an edge is true.

The mapPainted last, from the optimized poses. Change the poses and you repaint the map.

17

Try it: close the loop

A 48-pose loop with drifting odometry. One scan match says the end is back at the start.

Odometry

heading noise

The loop closure

its information

▬ true path▬ odometry▬ optimized⋯ loop-closure edge⋯ false closure

18

Loop closure: it removes drift, or ruins the map

The graph is only as good as its worst edge.

What it fixesDrift accumulates along the trajectory. One edge from the end back to the start spreads the correction over every pose, weighted by how uncertain each one was.

How it is foundslam_toolbox searches for earlier scans within a distance of the current pose (loop_search_maximum_distance), matches against them, and accepts only a strong, consistent match.

What it can breakA false loop closure, between two corridors that look alike, is a confident edge that is wrong. Least squares obeys it, and the map folds.

Defenses: require a minimum match response and a chain of consistent scans, prefer distinctive places, and use robust kernels that down-weight an edge that disagrees with the rest. For you: drive back through the same distinctive spot on purpose, slowly, and watch the map snap.

19

slam_toolbox on the TurtleBot 4

Macenski and Jambrecic, 2021: Karto-style correlative matching, a pose graph, and a Ceres back end.

Mode or parameter	What it means	Why you care
synchronous	processes every scan, queueing if it falls behind	the TurtleBot 4 manual recommends it on a remote PC, for a better map
asynchronous (`sync:=false`)	drops scans when busy, stays current	for a slow computer, or the Raspberry Pi
`resolution`	cell size of the published map	0.05 m is the usual choice; finer costs memory and time everywhere downstream
`minimum_travel_distance`, `_heading`	how far the robot must move before a new node	smaller: more nodes, more matching; larger: gaps on tight turns
`do_loop_closing`, `loop_search_maximum_distance`	whether to close loops, and how far to look	the switch between a drifting map and a corrected one
save_map, serialize_map	save the grid (PGM and YAML), or save the whole pose graph	only a serialized graph lets you keep mapping later, or localize with slam_toolbox

Check what your launch actually loaded before you tune it: ros2 param get /slam_toolbox resolution. Every parameter and service: the slam_toolbox repository (ROS 2 branches, including Jazzy).

20

Mapping well with a TurtleBot 4

Most bad maps are bad driving, not bad software.

Drive slowly, turn slowerThe A1M8 sweeps a few times a second. Turning fast while it sweeps bends each scan, and a bent scan matches badly.

Close loops on purposeReturn to where you started, through a distinctive spot, before mapping the next area. Small loops early beat one huge loop at the end.

Mind the planeOnly what crosses the lidar's height exists. For Cardboard Canyon: walls taller than the lidar, and no gaps at its height.

No mirrors, no glassThey return the wrong range or none. Cover them, or expect holes and ghost walls.

Empty the roomPeople walking through become permanent obstacles in a static map. Map when the room is quiet.

Check against the tapeMeasure two walls in the room. If the map disagrees by more than a few cells, something slipped: start again.

21

How good is your map?

A map you have not checked is a guess with a file extension.

Without ground truthStraight walls should be straight and corners square. A doubled wall is a failed or missing loop closure. Distances should match the tape.

With ground truthATE, absolute trajectory error: align the estimated trajectory to the true one and take the RMS of the position differences. RPE, relative pose error: compare motions over a fixed distance, which measures drift and ignores where it started.

For navigationThe map only has to be consistent where the robot drives, and correct in the cells Nav2 plans through. A beautiful map with a closed doorway is a bad map.

In Lab 3 you will not have a motion-capture system. You will have a tape measure and the same doorway seen twice. That is enough to know whether to trust the map.

22

Verify in simulation

Exercise 1: read a map as a message, then as files

Run this on your Ubuntu PC: no robot needed

terminal 1

$ros2 launch turtlebot4_gz_bringup turtlebot4_gz.launch.py \$ slam:=true nav2:=true rviz:=true

terminal 2

$ros2 topic echo /map --once --field info$ros2 run nav2_map_server map_saver_cli -f ~/maps/sim_map$cat ~/maps/sim_map.yaml

Drivea short loop with teleop while the map grows in RViz

Predict firstwidth × height cells: how many bytes is this map?

Checkdoes the YAML's resolution match info.resolution?

23

Verify in simulation

Exercise 2: map the simulated warehouse, well and badly

Run this on your Ubuntu PC: no robot needed

terminal 1

$ros2 launch turtlebot4_gz_bringup turtlebot4_gz.launch.py \$ slam:=true nav2:=true rviz:=true

terminal 2

$ros2 run teleop_twist_keyboard teleop_twist_keyboard \$ --ros-args -p stamped:=true

terminal 3

$ros2 service call /slam_toolbox/save_map slam_toolbox/srv/SaveMap \$ "name: {data: 'warehouse_sim'}"

DoPress Play in Gazebo. Drive one loop of the warehouse slowly, then save. The PGM and YAML land in the current directory.

Then break itMap again, driving fast and turning hard. Save it under another name and compare the two images.

Predict firstWhich will look worse: fast straight lines, or fast turns? Why?

24

Verify on your TurtleBot

Exercise 3: the map your robot will navigate on

Run these on your Ubuntu PC, with the robot on the network

terminal 1

$ros2 launch turtlebot4_navigation slam.launch.py

terminal 2

$ros2 launch turtlebot4_viz view_navigation.launch.py

terminal 3

$ros2 run teleop_twist_keyboard teleop_twist_keyboard \$ --ros-args -p stamped:=true

terminal 4: after mkdir -p ~/maps

$ros2 run nav2_map_server map_saver_cli -f ~/maps/room122

Map Room 122One lap along the walls, then through the middle. Close the loop where you started.

Check itMeasure two walls with the tape, and the same two in the PGM (cells times 0.05 m). How far off?

Keep itThis map is Lab 4's map. Save the YAML and the PGM with your team's repository.

25

Verify on your TurtleBot

Exercise 4: is your map accurate enough to plan on?

With your Room 122 map from the robot

Quantity	Tape	Map (cells × resolution)
the doorway
wall to wall, long side
the narrowest gap in Cardboard Canyon

Open the PGMin any image editor; count pixels between the edges

Comparewithin one cell? If not, which side is wrong?

Decideis 0.05 m fine enough for your narrowest gap?

Your robot is about 0.34 m across. A gap mapped two cells too narrow can turn passable into lethal.

26

Verify on your TurtleBot

Exercise 5: watch a loop close

Same terminals as Exercise 3

terminal

$ros2 param get /slam_toolbox do_loop_closing$ros2 param get /slam_toolbox minimum_travel_distance$ros2 param get /slam_toolbox resolution

terminal

$ros2 service call /slam_toolbox/serialize_map \$ slam_toolbox/srv/SerializePoseGraph "{filename: 'room122_graph'}"

Drive a long loop without returning to the start, and watch a wall begin to double in RViz. Then drive back through the start, slowly. The moment of loop closure is visible: the whole map shifts at once.

PredictWhich wall doubles first: the one you mapped first, or the one you mapped last?

MeasureScreenshot the map just before and just after the closure. How far did the far wall move?

Why serializeA PGM is a picture. The pose graph is the evidence behind it, and the only way to keep mapping later.

27

What to take away

The one habitClose the loop on purpose, and check the map against the tape before anyone navigates on it.

1Choose the map for the question

Grid, landmarks, graph, or points: each answers some questions cheaply and others not at all.

2A grid is Bayes per cell

Log-odds add: free along each ray, occupied at its end, clamped so the map can change.

3Cell size costs

Four times the cells per halving in 2D. Too coarse, and doorways close.

4A map is a message

OccupancyGrid; the YAML and PGM are its picture, and the origin places it.

5SLAM couples two errors

Pose errors bend the map and map errors misplace the pose, so they are estimated together.

6Scan matching makes edges

ICP is precise near the answer; correlative matching is robust and reports a covariance.

7A pose graph is least squares

Odometry, scan matches, and loop closures are the measurements; the poses are the unknowns.

8Loop closure removes drift

And a false one folds the map. Save the graph, not just the picture.

28