FixTheRobot Explore Robotics / Localization & SLAM / M3 / M3-L3

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

Localization & SLAM · Mapping and State Estimation · M3-L3 · Intermediate

Diagnose Scan-Matching Failure

ICP is a few lines of code and a source of long afternoons. It converges beautifully in the demo, and then, in the building, it slides the robot down a corridor, or locks a wall onto the wrong wall.

You write it, push it until it breaks to learn where it breaks, and then make it tolerate the things real scans contain: people, and parts of the scene the other scan never saw.

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 implements the algorithm of M3-T5 and is graded against the true relative motion on scans you never see. Stage 2 is an experiment on a reference ICP: how the convergence basin, outliers and a corridor affect the result (the three failure modes of M3-T4). Stage 3 is the repair for outliers and partial overlap: trimmed ICP. The simulator keeps the true motion private.

    Theory for this lab

    Mapping and State Estimation labs

    1. M3-L1: Build a 2D Occupancy Grid
    2. M3-L2: Fix a Misaligned Map
    3. M3-L3: Diagnose Scan-Matching Failure: this lab.
    4. M3-L4: Explore Particle-Filter Localization
    5. M3-L5: Implement a Simplified EKF-SLAM

    Educational simplification: 2D point-to-point ICP with brute-force nearest neighbours, a few dozen points per source scan, a target scan densified by interpolation (point-to-point ICP is only as accurate as the point spacing), and outliers injected at random. Production LiDAR odometry uses k-d trees, point-to-line and point-to-plane distances, 3D data, robust kernels and a degeneracy check on the Hessian.

    Get the next lab

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