Localisation & SLAM, step 6

Closing the loop

Turn every scan match into an edge of a graph, add one edge that closes the loop, and solve for all the poses at once: the lap closes and the map lines up.

Pose graphLoop closureGauss–Newton

Builds on Least squares: fitting noisy data and Optimisation: gradient descent, from the free Foundations.

Write and run this step in the simulator with Pro

Why the loop doesn't close

Your ICP track from step 5 still drifts: every match is a little off and the errors add up. When the rover gets back to its start its track doesn't quite, and the map shows a seam.

A graph of poses

Pose-graph SLAM keeps every keyframe pose $x_i = [x_i, y_i, \theta_i]$ as a node and every measurement as an edge $(i, j, z, W)$ : "seen from pose $i$ , pose $j$ is at $z = [dx, dy, d\theta]$ ". ICP links consecutive keyframes. One more match, of the last scan against the first, adds a loop closure. $W$ , the edge's information matrix (its inverse covariance), says how much to trust it.

The error of an edge

With $t_i$ the position of pose $i$ and $R_i$ the rotation by $\theta_i$ ,

e = \begin{bmatrix} R_i^T (t_j - t_i) - z_{xy} \\ \operatorname{wrap}(\theta_j - \theta_i - z_\theta) \end{bmatrix}

Its Jacobians, with $c = \cos\theta_i$ , $s = \sin\theta_i$ and $[d_x, d_y] = t_j - t_i$ , are

A = \frac{\partial e}{\partial x_i} = \begin{bmatrix} -c & -s & -s d_x + c d_y \\ s & -c & -c d_x - s d_y \\ 0 & 0 & -1 \end{bmatrix}, \qquad B = \frac{\partial e}{\partial x_j} = \begin{bmatrix} c & s & 0 \\ -s & c & 0 \\ 0 & 0 & 1 \end{bmatrix}

Gauss–Newton

The best poses minimise $\sum e^T W e$ over every edge. Linearise each edge at the current poses and add its blocks into one system: $A^T W A$ into block $(i, i)$ of $H$ , $A^T W B$ into $(i, j)$ , $B^T W A$ into $(j, i)$ , $B^T W B$ into $(j, j)$ , and $A^T W e$ and $B^T W e$ into blocks $i$ and $j$ of $b$ . Solve $H\,\Delta x = -b$ , add $\Delta x$ to the poses, and repeat. Foundations: Least squares covers the idea.

Edges only fix relative poses, so the whole graph could slide and spin for free and $H$ is singular. Hold pose 0 still: drop its three rows and columns and solve for the rest.

The program

The rover drives step 5's loop, matching a scan every half second, then closes the loop and calls your optimiser. The Images tab shows the map before and after; the 3D view shows the ICP chain in orange and your optimised track in green.

Your task

Write edge_error(xi, xj, z), edge_jacobians(xi, xj) → (A, B), build_system(poses, edges) → (H, b) and optimize(poses, edges, iters). The grader checks each on fixed graphs, then your track and your map against the real maze.

Closing the loop

Turn every scan match into an edge of a graph, add one edge that closes the loop, and solve for all the poses at once: the lap closes and the map lines up.

Pose graphLoop closureGauss–Newton

Write and run this step in the simulator with Pro

A graph of poses

Pose-graph SLAM keeps every keyframe pose

x_i = [x_i, y_i, \theta_i]

as a node and every measurement as an edge

(i, j, z, W)

: "seen from pose

i

, pose

j

is at

z = [dx, dy, d\theta]

". ICP links consecutive keyframes. One more match, of the last scan against the first, adds a loop closure.

W

, the edge's information matrix (its inverse covariance), says how much to trust it.

The error of an edge

With

t_i

the position of pose

i

and

R_i

the rotation by

\theta_i

e = \begin{bmatrix} R_i^T (t_j - t_i) - z_{xy} \\ \operatorname{wrap}(\theta_j - \theta_i - z_\theta) \end{bmatrix}

Its Jacobians, with

c = \cos\theta_i

s = \sin\theta_i

and

[d_x, d_y] = t_j - t_i

, are

A = \frac{\partial e}{\partial x_i} = \begin{bmatrix} -c & -s & -s d_x + c d_y \\ s & -c & -c d_x - s d_y \\ 0 & 0 & -1 \end{bmatrix}, \qquad B = \frac{\partial e}{\partial x_j} = \begin{bmatrix} c & s & 0 \\ -s & c & 0 \\ 0 & 0 & 1 \end{bmatrix}

Gauss–Newton

The best poses minimise

\sum e^T W e

over every edge. Linearise each edge at the current poses and add its blocks into one system:

A^T W A

into block

(i, i)

H

A^T W B

into

(i, j)

B^T W A

into

(j, i)

B^T W B

into

(j, j)

, and

A^T W e

and

B^T W e

into blocks

i

and

j

b

. Solve

H\,\Delta x = -b

, add

\Delta x

to the poses, and repeat. Foundations: Least squares covers the idea.

Edges only fix relative poses, so the whole graph could slide and spin for free and

H

is singular. Hold pose 0 still: drop its three rows and columns and solve for the rest.

Closing the loop

Why the loop doesn't close

A graph of poses

The error of an edge

Gauss–Newton

The program

Your task

This step is part of CrackRobotics Pro

Closing the loop

Why the loop doesn't close

A graph of poses

The error of an edge

Gauss–Newton

The program

Your task

This step is part of CrackRobotics Pro