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

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

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

Compare Trajectories Before and After Optimization

The optimiser finished and the map looks better. “Looks better” is not a result. You need numbers that survive the choice of coordinate frame, tell local from global error, and work in the field, where there is no ground truth.

You write the three standard measures, and then use the last one to find out whether the graph is honest about its own uncertainty.

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 ATE after rigid alignment, Stage 2 is RPE, both standard in SLAM benchmarks (TUM RGB-D, EuRoC). Stage 3 is the χ² consistency check of M4-T4, the one metric available without ground truth. Stage 4 runs the reference optimiser so that you can see that the information scale changes χ² and the reported covariance, but not the trajectory. Ground truth is passed to the metric functions only because that is what a benchmark does; the grader’s own values stay in the simulator.

    Theory for this lab

    Graph-Based SLAM labs

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

    Educational simplification: 2D trajectories, rigid alignment without scale, translational RPE only, diagonal information. Benchmarks use SE(3) trajectories, timestamps association, similarity alignment for monocular systems, and rotational errors as well (the evo and rpg_trajectory_evaluation tools).

    Get the next lab

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