FixTheRobot Explore Robotics / Localization & SLAM / M4 / M4-L1

This lab works on phones, but writing code is much easier on a laptop.

Localization & SLAM · Graph-Based SLAM · M4-L1 · Intermediate

Visualize a Pose Graph

A pose graph is a picture that makes SLAM debuggable: dots for poses, lines for measurements, and a colour for how much each line disagrees with the dots.

You build the picture yourself. First the dots, by chaining odometry. Then the colours, by computing the error of each edge. Finally the stiffness of each line, and what it does to the answer.

Your task

    Simulation
    solution.pyLoading Python…
    Ctrl+Enter runs
    What Python can I use?

    Plain Python with math and, for matrices, slamkit (matmul, transpose, solve, inverse, identity). The data arrive as the dictionary D. There is no NumPy, random, json or copy in the browser runtime. Your code runs locally and never leaves this page.

    Hints

    What this lab teaches

    Stages 1 and 2 are the building blocks of M4-T2, graded against reference values to 1e-9 and including a heading near ±180°. Stage 3 is a reference optimiser (the one you will write in M4-L2) run at different loop-closure information levels, so you can see that information plays the role of spring stiffness. The true trajectory stays in the simulator.

    Theory for this lab

    Graph-Based SLAM labs

    1. M4-L1: Visualize a Pose Graph: this lab.
    2. M4-L2: Correct Accumulated Trajectory Drift
    3. M4-L3: Diagnose a Broken Loop Closure
    4. M4-L4: Compare Trajectories Before and After Optimization

    Educational simplification: a circular trajectory of 24 poses, diagonal information matrices, one or two loop closures, and a dense optimiser. Real pose graphs have thousands of nodes, full 3×3 (or 6×6) information matrices and sparse solvers (g2o, GTSAM, Ceres).

    Get the next lab

    Get notified when new robotics labs are released. One email per new lab, nothing else.