Problems · Problem 18 · Perception & mapping · Hard

Graph SLAM

Turn a drifting lap of odometry into a consistent trajectory by optimising every pose at once against all the measurements.

Pose graphsLoop closureGauss–Newton

Builds on Least squares: fitting noisy data, from the free Foundations.

Write and run this problem in the simulator with Pro

What it computes

Odometry drifts: a turn measured 3 % too big bends the whole rest of the path. Graph SLAM corrects a trajectory all at once. Each keyframe pose $x_i = (x, y, \theta)$ is a node. Each measurement is an edge, "seen from pose $i$ , pose $j$ is at $z_{ij}$ ", weighted by an information matrix $\Omega_{ij}$ (an inverse covariance). Odometry links consecutive poses, and a loop closure says the lap ended where it began. The best trajectory minimises

F(x) = \sum_{ij} e_{ij}^T\, \Omega_{ij}\, e_{ij}

The error and its Jacobians

With $R_i$ the rotation by $\theta_i$ , and $\Delta t = t_j - t_i$ the difference in position:

e_{ij} = \begin{bmatrix} R_i^T \Delta t - z_{xy} \\ \operatorname{wrap}(\theta_j - \theta_i - z_\theta) \end{bmatrix}, \quad A_{ij} = \frac{\partial e_{ij}}{\partial x_i} = \begin{bmatrix} -R_i^T & \frac{\partial R_i^T}{\partial \theta_i} \Delta t \\ 0 \;\; 0 & -1 \end{bmatrix}, \quad B_{ij} = \frac{\partial e_{ij}}{\partial x_j} = \begin{bmatrix} R_i^T & 0 \\ 0 \;\; 0 & 1 \end{bmatrix}

where $R_i^T = \begin{bmatrix} c & s \\ -s & c \end{bmatrix}$ and $\frac{\partial R_i^T}{\partial \theta_i} = \begin{bmatrix} -s & c \\ -c & -s \end{bmatrix}$ , with $c = \cos\theta_i$ and $s = \sin\theta_i$ .

Gauss–Newton

repeat iters times:
    H = zeros(3n, 3n);  b = zeros(3n)
    for each edge (i, j, z, Ω):
        e, A, B = the error and Jacobians at the current poses
        H[i, i] += Aᵀ Ω A      H[i, j] += Aᵀ Ω B
        H[j, i] += Bᵀ Ω A      H[j, j] += Bᵀ Ω B
        b[i] += Aᵀ Ω e         b[j] += Bᵀ Ω e
    solve H Δx = -b for poses 1 … n-1 only    # pose 0 stays put
    x += Δx, then wrap every θ

Edges only fix relative poses, so the whole graph could slide and spin for free: $H$ is singular until you hold one pose still.

Tools

H[3*i:3*i+3, 3*j:3*j+3] is block $(i, j)$ . rover.wrap(a) wraps angles to $(-\pi, \pi]$ . np.linalg.solve(M, v) solves $M \Delta = v$ .

Your task

Implement optimize(poses, edges, iters). poses is $(n, 3)$ and each edge is (i, j, z, info); return the optimised $(n, 3)$ poses. The program drives the rover once round the loop, keeps a keyframe every half second, and draws raw odometry in red and your trajectory in green. The grader compares optimize with the reference on three test graphs, then checks your lap against the rover's true path.

What it computes

Odometry drifts: a turn measured 3 % too big bends the whole rest of the path. Graph SLAM corrects a trajectory all at once. Each keyframe pose

x_i = (x, y, \theta)

is a node. Each measurement is an edge, "seen from pose

i

, pose

j

is at

z_{ij}

", weighted by an information matrix

\Omega_{ij}

(an inverse covariance). Odometry links consecutive poses, and a loop closure says the lap ended where it began. The best trajectory minimises

F(x) = \sum_{ij} e_{ij}^T\, \Omega_{ij}\, e_{ij}

The error and its Jacobians

With

R_i

the rotation by

\theta_i

, and

\Delta t = t_j - t_i

the difference in position:

e_{ij} = \begin{bmatrix} R_i^T \Delta t - z_{xy} \\ \operatorname{wrap}(\theta_j - \theta_i - z_\theta) \end{bmatrix}, \quad A_{ij} = \frac{\partial e_{ij}}{\partial x_i} = \begin{bmatrix} -R_i^T & \frac{\partial R_i^T}{\partial \theta_i} \Delta t \\ 0 \;\; 0 & -1 \end{bmatrix}, \quad B_{ij} = \frac{\partial e_{ij}}{\partial x_j} = \begin{bmatrix} R_i^T & 0 \\ 0 \;\; 0 & 1 \end{bmatrix}

where

R_i^T = \begin{bmatrix} c & s \\ -s & c \end{bmatrix}

and

\frac{\partial R_i^T}{\partial \theta_i} = \begin{bmatrix} -s & c \\ -c & -s \end{bmatrix}

, with

c = \cos\theta_i

and

s = \sin\theta_i

Gauss–Newton

repeat iters times: H = zeros(3n, 3n); b = zeros(3n) for each edge (i, j, z, Ω): e, A, B = the error and Jacobians at the current poses H[i, i] += Aᵀ Ω A H[i, j] += Aᵀ Ω B H[j, i] += Bᵀ Ω A H[j, j] += Bᵀ Ω B b[i] += Aᵀ Ω e b[j] += Bᵀ Ω e solve H Δx = -b for poses 1 … n-1 only # pose 0 stays put x += Δx, then wrap every θ

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

H

is singular until you hold one pose still.

Your task

Implement optimize(poses, edges, iters). poses is

(n, 3)

and each edge is (i, j, z, info); return the optimised

(n, 3)

poses. The program drives the rover once round the loop, keeps a keyframe every half second, and draws raw odometry in red and your trajectory in green. The grader compares optimize with the reference on three test graphs, then checks your lap against the rover's true path.

Graph SLAM

What it computes

The error and its Jacobians

Gauss–Newton

Tools

Your task

This problem is part of CrackRobotics Pro

Graph SLAM

What it computes

The error and its Jacobians

Gauss–Newton

Tools

Your task

This problem is part of CrackRobotics Pro