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

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

Localization & SLAM · Graph-Based SLAM · M4-L2 · Advanced

Correct Accumulated Trajectory Drift

The robot drove round and round, recognised two places, and now knows its odometry lied. The path does not close, and the map built along it would show every wall twice.

You write the optimiser that pulls the path together: the edge Jacobians first, then the Gauss-Newton loop with the anchor, then an experiment on how many iterations it needs.

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

    Stage 1 is the derivative that most optimisers get wrong, graded against exact values and finite differences. Stage 2 is the Gauss-Newton loop of M4-T4 on hidden drifted loops, graded by the cost reached, by the anchor, and by the error against the true path, which stays in the simulator. Stage 3 is an experiment with a reference optimiser: drift against iterations.

    Theory for this lab

    Graph-Based SLAM labs

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

    Educational simplification: 24 poses on a circle, diagonal information, dense linear algebra (a few hundred unknowns), plain Gauss-Newton. Production back ends (g2o, GTSAM, Ceres, SE-Sync) use sparse factorisation, trust regions, full information matrices and manifold updates in 3D, and run incrementally (iSAM2).

    Get the next lab

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