Level 5 · Robotics: Planning and Mapping

Level 4 told the robot where it is on a map. This level asks the two questions that come next: how do I get there, and where did the map come from?

The first topic is planning. A robot has a body, so the obstacles grow by its radius before any search starts (the configuration space). The shortest route scrapes along the walls, so a cost map puts a price on clearance. Turning takes time, so the heading joins the search. A route of grid cells is a staircase, so it is straightened by line of sight and translated into wheel commands. And the map is sometimes wrong, so the robot replans as it learns. The second topic is mapping. Every laser beam says "free all the way, something here", and an occupancy grid adds up that evidence cell by cell in log-odds. A robot also wants objects, not just cells: walls as lines (fitted by perpendicular least squares, found among clutter by RANSAC, cut apart by split-and-merge), the corners where walls meet, and the motion between two scans (rigid alignment and ICP), which is how a robot corrects its odometry from what it sees.

The paths meet often here. Breadth-first search, Dijkstra and A* are taught in the Algorithms lesson (Level 5); this lesson teaches only what a robot changes about them, and several of that path's grid problems are on this path too. Log-odds are Bayes' rule from Statistics (Level 3) for one cell with two states, the sense update of Level 4 written as a sum. Line fitting is the least squares of the same Statistics lesson, with the distances measured across the line instead of up and down. numpy appears only as an optional check with an SVD; everything else is plain Python.

The robot is not a point: configuration space

A round robot of radius \rho hits an obstacle exactly when its centre comes within \rho of it. So instead of moving a disc among obstacles, shrink the robot to its centre and grow every obstacle by \rho. The positions the centre may take form the configuration space (C-space). In it the robot is a point, and every planner of this level works unchanged.

On a grid, measure from cell centre to cell centre in cells (side neighbours 1 apart, corner neighbours \sqrt2), and forbid every free cell within \rho of some obstacle cell:

Whether a distance of exactly \rho counts as touching ("at most") or as passing ("more than") is a choice each problem states, and it decides whether a narrow doorway is open. Work outwards from each obstacle: only cells within \lfloor\rho\rfloor rows and columns of it can be affected, so stamp that square window and compare squared distances, dr * dr + dc * dc <= radius * radius, with no square roots.

import math
plan = ["..........",
        "..........",
        "####...###",
        "..........",
        "......#...",
        ".........."]

def forbidden(grid, radius):
    rows, cols = len(grid), len(grid[0])
    k = math.floor(radius)                       # no forbidden cell is further than k rows or columns away
    out = set()
    for r in range(rows):
        for c in range(cols):
            if grid[r][c] != "#":
                continue
            for dr in range(-k, k + 1):
                for dc in range(-k, k + 1):
                    rr, cc = r + dr, c + dc
                    if 0 <= rr < rows and 0 <= cc < cols and grid[rr][cc] == "." \
                            and dr * dr + dc * dc <= radius * radius:     # at most radius: touching
                        out.add((rr, cc))
    return out

for radius in (1, 1.5):
    bad = forbidden(plan, radius)
    print("radius", radius)
    for r, row in enumerate(plan):
        print("  " + "".join("x" if (r, c) in bad else ch for c, ch in enumerate(row)))

radius 1
  ..........
  xxxx...xxx
  ####x.x###
  xxxx..xxxx
  .....x#x..
  ......x...
radius 1.5
  ..........
  xxxxx.xxxx
  ####x.x###
  xxxxxxxxxx
  .....x#x..
  .....xxx..

With radius 1 the side neighbours of every shelf cell close (exactly 1 away, and "at most" counts) while the corner cells stay open (1.414 away). The doorway in row 2 keeps its middle cell (2, 5), 2 cells from both door posts. With radius 1.5 the corner cells close too, and the box at (4, 6) closes (3, 5), the cell below the doorway, so the top of the room is cut off from the bottom. A radius of 2 would close the doorway itself. The obstacle cells keep their #: they are not part of the configuration space at all.

Common mistakes: < where the problem says "at most" (or <= where it says "more than"). The Manhattan or the square (Chebyshev) distance instead of the Euclidean one. Forgetting to clip the window at the edges of the map. Treating the edge of the map as a wall when the problem says it is not. Stamping from cells you have already marked instead of from the original obstacles.

Practice: Room for a Round Robot

Searching the floor plan: reach, regions and clearance

Breadth-first search from the Algorithms lesson answers three robot questions on the configuration space, each with a small twist.

• Can the robot get there, and in how many moves? BFS from the robot's cell over the usable cells only.
• Into how many separate regions does its body cut the floor? Flood-fill from every usable cell that no earlier fill reached; each fill is one region (a connected component).
• How far is every cell from the nearest obstacle? One BFS that starts with every obstacle cell in the queue at distance 0. Its rings grow from all of them at once, so each cell is labelled by the nearest one. This clearance map (a distance transform) is what cost maps are built from.

from collections import deque
plan = ["..........",
        "..........",
        "####...###",
        "..........",
        "......#...",
        ".........."]
R, C = len(plan), len(plan[0])
blocked = [(r, c) for r in range(R) for c in range(C) if plan[r][c] == "#"]

def usable(radius):                              # the configuration space, as a set of free cells
    return {(r, c) for r in range(R) for c in range(C)
            if all((r - br) ** 2 + (c - bc) ** 2 > radius ** 2 for br, bc in blocked)}

def bfs(free, sources):                          # moves from the nearest source, 4-connected
    dist = {s: 0 for s in sources}
    todo = deque(sources)
    while todo:
        r, c = todo.popleft()
        for nxt in ((r - 1, c), (r, c + 1), (r + 1, c), (r, c - 1)):
            if nxt in free and nxt not in dist:
                dist[nxt] = dist[(r, c)] + 1
                todo.append(nxt)
    return dist

for radius in (1, 1.5):
    free = usable(radius)
    regions, seen = 0, set()
    for cell in sorted(free):                    # flood fill from every cell not yet reached
        if cell not in seen:
            regions += 1
            seen |= set(bfs(free, [cell]))
    dist = bfs(free, [(0, 0)])
    print("radius", radius, " free", len(free), " regions", regions,
          " moves to (5, 0):", dist.get((5, 0), "unreachable"))

everything = {(r, c) for r in range(R) for c in range(C)}
steps = bfs(everything, blocked)                 # every shelf cell is a source
for r in range(R):
    print(" ".join(str(steps[(r, c)]) for c in range(C)))

radius 1  free 32  regions 2  moves to (5, 0): 15
radius 1.5  free 26  regions 3  moves to (5, 0): unreachable
2 2 2 2 3 4 3 2 2 2
1 1 1 1 2 3 2 1 1 1
0 0 0 0 1 2 1 0 0 0
1 1 1 1 2 2 1 1 1 1
2 2 2 2 2 1 0 1 2 2
3 3 3 3 3 2 1 2 3 3

The robot of radius 1 reaches (5, 0) in 15 moves, through the doorway. At radius 1.5 the same floor falls apart into three regions (the top, the bottom left and the bottom right) and the goal is out of reach: a planner must say so, not search forever. In the clearance map the doorway's middle (2, 5) is 2 steps from the shelves. BFS counts steps, so a corner neighbour is 2 away, not \sqrt2: this is the Manhattan distance. A problem that measures clearance between cell centres, as the cost map below does, needs the Euclidean distance instead.

Common mistakes: searching the original grid instead of the inflated one. One BFS per source instead of all sources in the queue at once. Marking a cell visited when it is popped instead of when it is pushed (cells enter the queue many times). Assuming the goal is reachable.

Practice: Fewest Moves Across the Warehouse, Largest Lake, Distance to the Nearest Charging Station

A* in the configuration space

A* is Dijkstra's search with the heap ordered by f = g + h: g is the cost so far and h a lower bound on the cost still to go. The Algorithms lesson shows why the first time the goal comes off the heap, its cost is the smallest. On a robot's grid three details matter.

• Search the configuration space. Inflate first, then search the usable cells as if the robot were a point. Check the start and the goal too: a robot parked too close to a shelf has no route.
• The heuristic must match the moves. With moves up, down, left and right of cost 1, use h = |\Delta r| + |\Delta c| (Manhattan). With eight moves, straight ones costing 10 and diagonal ones 14 (about 10\sqrt2), the cheapest trip that ignores obstacles is the octile distance 14\min(|\Delta r|, |\Delta c|) + 10\,\big(\max(|\Delta r|, |\Delta c|) - \min(|\Delta r|, |\Delta c|)\big). A robot with a body should not clip a corner either: allow a diagonal step only when both side cells beside it are free.
• Ties. On an open floor many cells share the smallest f. Breaking ties towards the larger g (push (f, -g, cell)) makes the search dive towards the goal instead of widening. Every tie-break finds a shortest route, but they differ in work and in which shortest route they find.

Remember each cell's parent when its g improves, rebuild the route from the goal back to the start and reverse it, and return None when the heap runs dry. A robot of radius 1 starts inside a U-shaped shelf whose opening faces away from the goal:

import heapq
plan = ["................",
        "................",
        "....########....",
        "...........#....",
        "...........#....",
        "...........#....",
        "....########....",
        "................",
        "................"]
R, C = len(plan), len(plan[0])
blocked = [(r, c) for r in range(R) for c in range(C) if plan[r][c] == "#"]
free = {(r, c) for r in range(R) for c in range(C)
        if all((r - br) ** 2 + (c - bc) ** 2 > 1 for br, bc in blocked)}   # radius 1, "more than"

def astar(start, goal, use_h, deep):
    def h(cell):
        return abs(cell[0] - goal[0]) + abs(cell[1] - goal[1]) if use_h else 0
    best, parent = {start: 0}, {start: None}
    heap = [(h(start), 0, start)]
    expanded = 0
    while heap:
        f, key, cell = heapq.heappop(heap)
        g = -key if deep else key
        if g > best[cell]:
            continue                             # stale entry
        expanded += 1
        if cell == goal:
            route = []
            while cell is not None:
                route.append(cell)
                cell = parent[cell]
            return route[::-1], expanded
        r, c = cell
        for nxt in ((r - 1, c), (r, c + 1), (r + 1, c), (r, c - 1)):
            if nxt in free and g + 1 < best.get(nxt, R * C):
                best[nxt], parent[nxt] = g + 1, cell
                heapq.heappush(heap, (g + 1 + h(nxt), -(g + 1) if deep else g + 1, nxt))
    return None, expanded

for name, use_h, deep in [("Dijkstra (h = 0)", False, False), ("A*, ties to small g", True, False),
                          ("A*, ties to large g", True, True)]:
    route, expanded = astar((4, 8), (4, 14), use_h, deep)
    print(f"{name:21} moves {len(route) - 1}  expanded {expanded}")
route, _ = astar((4, 8), (4, 14), True, True)
for r in range(R):
    print("".join("o" if (r, c) in route else "#" if plan[r][c] == "#" else "." if (r, c) in free else "x"
                  for c in range(C)))

Dijkstra (h = 0)      moves 26  expanded 85
A*, ties to small g   moves 26  expanded 72
A*, ties to large g   moves 26  expanded 42
...oooooooooooo.
..ooxxxxxxxx..o.
..ox########x.o.
..ooxxxxxxx#x.o.
...oooooo.x#x.o.
....xxxxxxx#x...
...x########x...
....xxxxxxxx....
................

All three searches find a 26-move route, and all of them first lead the robot west, out of the U, although the goal lies east: the heuristic guides the search, it never forbids a move. Dijkstra expands 85 cells, A* 72 when ties go to the smaller g and 42 when they go to the larger. Inflation closed the rows of the U next to the shelf, leaving the one-cell corridor along row 4. On the printed map o is the route and x a cell the robot's centre must avoid.

Common mistakes: the Manhattan heuristic with diagonal moves, which overestimates and gives wrong answers without any error. Skipping the stale-entry check. Forgetting to check that the start and the goal are usable. Returning the route without its start, or backwards. Letting a diagonal step clip a corner when the problem forbids it.

Practice: A* With Room to Spare, Rover Across the Rubble Field

Cost maps: the cheapest route, not the shortest

The shortest route hugs every obstacle it passes: the worst place for a robot whose position is uncertain (Level 4) and whose controller wobbles. A cost map gives each cell a price for entering it, higher near obstacles, and the planner looks for the cheapest route. The prices differ from cell to cell, so breadth-first search no longer finds the best route: use Dijkstra, ordering the heap by the price so far. The price of a route is the sum over the cells it enters: the goal is charged, the start is not.

The pricing is a design choice, and every problem states its own. This example uses the clearance d, the Euclidean distance from a cell's centre to the centre of the nearest display case, and charges 1 + w/d^2: with w = 4 a cell beside a case costs 5, one two cells away 2, and cells far away about 1.

import heapq, math
plan = ["...........",
        "...........",
        "...........",
        "....#..#...",
        "...........",
        "..........."]
R, C = len(plan), len(plan[0])
blocked = [(r, c) for r in range(R) for c in range(C) if plan[r][c] == "#"]
clear = {(r, c): min(math.dist((r, c), b) for b in blocked)       # Euclidean, to the nearest case
         for r in range(R) for c in range(C) if plan[r][c] == "."}

def cheapest(start, goal, weight):
    best, parent = {start: 0.0}, {start: None}
    heap = [(0.0, start)]
    while heap:
        g, cell = heapq.heappop(heap)
        if g > best[cell]:
            continue
        if cell == goal:
            route = []
            while cell is not None:
                route.append(cell)
                cell = parent[cell]
            return g, route[::-1]
        r, c = cell
        for nxt in ((r - 1, c), (r, c + 1), (r + 1, c), (r, c - 1)):
            if nxt in clear and g + weight(nxt) < best.get(nxt, math.inf):
                best[nxt], parent[nxt] = g + weight(nxt), cell
                heapq.heappush(heap, (g + weight(nxt), nxt))
    return None, None

for w in (4, 0.5):
    price = {cell: 1 + w / d ** 2 for cell, d in clear.items()}      # this example's own pricing
    _, short = cheapest((3, 0), (3, 10), lambda cell: 1)              # all prices 1: fewest moves
    cost, cheap = cheapest((3, 0), (3, 10), price.get)
    print(f"w={w}: fewest moves {len(short) - 1}, price {sum(price[c] for c in short[1:]):.3f};"
          f" cheapest {len(cheap) - 1} moves, price {cost:.3f}")
    for r in range(R):
        print("   ", "".join("o" if (r, c) in cheap else plan[r][c] for c in range(C)))

w=4: fewest moves 12, price 31.080; cheapest 16 moves, price 21.296
    ooooooooooo
    o.........o
    o.........o
    o...#..#..o
    ...........
    ...........
w=0.5: fewest moves 12, price 14.385; cheapest 12 moves, price 14.385
    ...........
    ...........
    ooooooooooo
    o...#..#..o
    ...........
    ...........

With w = 4 the fewest-moves route squeezes past both cases (12 moves, price 31.08), and the cheapest one walks 16 moves around them for 21.30. With the gentler w = 0.5 the detour no longer pays, and the cheapest route is a 12-move one, a row away from the cases. The search code never changed; only the prices did.

A route's price need not be a sum. A rover whose steepest step decides whether it gets stuck judges a route by its largest step. Dijkstra still works, because extending a route can never lower its largest step: relax with max(g, w) instead of g + w, and the first time the goal comes off the heap its largest step is the smallest possible.

Common mistakes: breadth-first search on a grid with prices. Charging the start, or not charging the goal. Taking a cell's clearance from the sum over all obstacles instead of the nearest one. BFS steps where the problem measures Euclidean distance. Returning infinity instead of None for an unreachable goal.

Practice: Keep Off the Walls, Gentlest Hiking Trail

Turning takes time: put the heading in the state

A planner that counts cells hands a robot a zigzag, and a two-wheeled robot (Level 2) has to stop and turn at every zig. When turning costs time, the cell alone no longer says what can happen next: standing in a cell facing north and facing east are different situations. Put the heading into the state. A state is (cell, heading), four per cell on a four-way grid, and every action is an edge between states. A turn keeps the cell and changes the heading (an edge with no distance but its own cost); a drive changes the cell and keeps the heading. Dijkstra on the states then finds the fastest plan. The goal is a set of states: every heading on the goal cell, or a single one for a robot that must dock.

A charging robot drives one cell ahead in 1 s, backs up one cell in 2 s and turns a quarter turn on the spot in 1.5 s. The charger is at (3, 2), and some chargers want the robot to arrive facing a particular way:

import heapq, math
plan = ["......",
        ".##.#.",
        "......",
        ".#.##."]
R, C = len(plan), len(plan[0])
STEP = {"N": (-1, 0), "E": (0, 1), "S": (1, 0), "W": (0, -1)}
LEFT = {"N": "W", "W": "S", "S": "E", "E": "N"}
RIGHT = {v: k for k, v in LEFT.items()}
FWD, BACK, TURN = 1.0, 2.0, 1.5                  # seconds: drive ahead, back up, quarter turn

def moves(state):
    (r, c), h = state
    yield "left", ((r, c), LEFT[h]), TURN       # turning changes the heading, not the cell
    yield "right", ((r, c), RIGHT[h]), TURN
    for name, sign, t in (("ahead", 1, FWD), ("back", -1, BACK)):
        rr, cc = r + sign * STEP[h][0], c + sign * STEP[h][1]
        if 0 <= rr < R and 0 <= cc < C and plan[rr][cc] == ".":
            yield name, ((rr, cc), h), t

def dock(start, facing, goal, need):
    first = (start, facing)
    best, how = {first: 0.0}, {first: None}
    heap = [(0.0, first)]
    while heap:
        t, state = heapq.heappop(heap)
        if t > best[state]:
            continue
        if state[0] == goal and (need is None or state[1] == need):
            actions = []
            while how[state] is not None:
                state, name = how[state]
                actions.append(name)
            return t, actions[::-1]
        for name, nxt, dt in moves(state):
            if t + dt < best.get(nxt, math.inf):
                best[nxt], how[nxt] = t + dt, (state, name)
                heapq.heappush(heap, (t + dt, nxt))
    return None, []

for need in (None, "N", "S"):
    t, actions = dock((2, 0), "E", (3, 2), need)
    print("facing", need or "any", " time", t, " ", " ".join(actions))

facing any  time 4.5   ahead ahead right ahead
facing N  time 5.5   ahead ahead left back
facing S  time 4.5   ahead ahead right ahead

To arrive any way, the robot drives two cells, turns right and drives in: 4.5 s. The charger can only be entered from above, so the only way to end up facing north is to turn left instead and back in: 5.5 s. The action list comes from the parent links, which store the action as well as the previous state.

When every cost is 0 or 1 (flying on is free, each command costs 1), a heap is more than you need: a 0-1 BFS keeps a deque, puts states reached at cost 0 at the front and states reached at cost 1 at the back, and still takes them out in order of cost. Dijkstra gives the same answers, a little more slowly.

Common mistakes: searching cells only, which forgets which way the robot faces. Letting the robot drive sideways. Forgetting the turn the robot may need before its first drive. Requiring a heading at the goal when any heading will do, or the opposite. Breadth-first search when the costs differ.

Practice: Turning Takes Time Too, Steering the Warehouse Drone

Line of sight through the cells, and shortcuts

A route of moves up, down, left and right is a staircase; a robot would rather drive straight between a few waypoints. It may, when the straight line between them crosses only free cells. Which cells does the line from cell a = (r_0, c_0) to cell b = (r_1, c_1) cross? A common rule steps along the longer direction one cell at a time and rounds the other coordinate. With \Delta r = r_1 - r_0, \Delta c = c_1 - c_0 and n = \max(|\Delta r|, |\Delta c|), the k-th cell, for k = 0, 1, \dots, n, is

The line has n + 1 cells, no gaps, and ends exactly on b. The rounding is part of the rule: \lfloor x + \frac12 \rfloor sends halves up. Python's round sends halves to the even neighbour, and int rounds towards zero, which is wrong for negative numbers. Whole numbers avoid floats altogether: \lfloor k\,\Delta r/n + \frac12 \rfloor is (2 * k * dr + n) // (2 * n), because // rounds down for negative numbers too.

import math

def cells(a, b, rounding):
    (r0, c0), (r1, c1) = a, b
    dr, dc = r1 - r0, c1 - c0
    n = max(abs(dr), abs(dc))
    if n == 0:
        return [a]
    return [(r0 + rounding(k * dr / n), c0 + rounding(k * dc / n)) for k in range(n + 1)]

def exact(a, b):                                 # floor(k*d/n + 1/2) in whole numbers
    (r0, c0), (r1, c1) = a, b
    dr, dc = r1 - r0, c1 - c0
    n = max(abs(dr), abs(dc))
    if n == 0:
        return [a]
    return [(r0 + (2 * k * dr + n) // (2 * n), c0 + (2 * k * dc + n) // (2 * n)) for k in range(n + 1)]

a, b = (0, 0), (2, 4)
print("floor(x + 1/2):", cells(a, b, lambda x: math.floor(x + 0.5)))
print("whole numbers: ", exact(a, b))
print("round():       ", cells(a, b, round))
a, b = (4, 4), (1, 0)
print("floor(x + 1/2):", cells(a, b, lambda x: math.floor(x + 0.5)))
print("int(x + 1/2):  ", cells(a, b, lambda x: int(x + 0.5)))

floor(x + 1/2): [(0, 0), (1, 1), (1, 2), (2, 3), (2, 4)]
whole numbers:  [(0, 0), (1, 1), (1, 2), (2, 3), (2, 4)]
round():        [(0, 0), (0, 1), (1, 2), (2, 3), (2, 4)]
floor(x + 1/2): [(4, 4), (3, 3), (3, 2), (2, 1), (1, 0)]
int(x + 1/2):   [(4, 4), (4, 4), (3, 3), (3, 2), (2, 1)]

round turns the first line's half step (row 0.5 at k = 1) into row 0 and draws a different line. int is worse on the second line, which runs towards smaller rows and columns: it repeats (4, 4) and ends on (2, 1) instead of on b.

Shortcuts. Given a route from the planner, keep its first cell. From the last kept cell, find the farthest cell of the route that is in plain sight, keep it, and repeat until the end of the route is kept. Search from the end of the route backwards and stop at the first clear line: searching forwards and stopping at the first blocked line misses a cell that comes back into view behind an obstacle.

import math
lawn = ["..........",
        "..........",
        "...##.....",
        "...##.....",
        "..........",
        "......#..."]

def visible(a, b):
    (r0, c0), (r1, c1) = a, b
    dr, dc = r1 - r0, c1 - c0
    n = max(abs(dr), abs(dc), 1)                 # n = 1 also covers a == b
    return all(lawn[r0 + (2 * k * dr + n) // (2 * n)][c0 + (2 * k * dc + n) // (2 * n)] == "."
               for k in range(n + 1))

route = [(5, 0), (4, 0), (4, 1), (4, 2), (4, 3), (4, 4), (4, 5), (3, 5), (2, 5), (1, 5),
         (1, 6), (1, 7), (1, 8), (0, 8), (0, 9)]          # a staircase from a 4-way planner
keep, i = [route[0]], 0
while i < len(route) - 1:
    j = len(route) - 1                           # look from the far end back towards i
    while j > i + 1 and not visible(route[i], route[j]):
        j -= 1
    keep.append(route[j])
    i = j
print("waypoints", keep)
print("staircase", len(route) - 1, "cells; straight legs",
      round(sum(math.dist(p, q) for p, q in zip(keep, keep[1:])), 3), "cells")

waypoints [(5, 0), (4, 5), (0, 9)]
staircase 14 cells; straight legs 10.756 cells

Fourteen moves of staircase become two straight legs, 10.76 cells long. The same cell rule will draw laser beams into a map later in this lesson.

Common mistakes: round or int for the rounding. Starting at k = 1, which never checks the first cell. Stepping along the shorter direction, which leaves gaps. Forgetting n == 0 (then a = b and the line is that one cell). Searching forwards for the shortcut and stopping at the first blocked line.

Practice: A Straight Line Through the Cells, Cutting the Staircase Short

Line of sight among boxes

Without a grid, obstacles are shapes. For a straight drive from p to q past axis-aligned boxes (x_1, y_1, x_2, y_2), two tests decide exactly whether the segment touches a box, its border included.

1. Bounding boxes. If the segment's own box (from the smaller to the larger end coordinate, in x and in y) does not overlap the obstacle, they are apart.
2. The line. Otherwise they are apart only when all four corners lie strictly on the same side of the line through p and q. Level 2's cross product tells the side: s = \Delta x\,(y - p_y) - \Delta y\,(x - p_x) is positive on the left, negative on the right and 0 on the line.

Why these two? Two convex shapes are apart exactly when some direction separates them, and for a segment and an axis-aligned box only three directions can: the x axis and the y axis (test 1) and the direction across the segment (test 2). With whole-number coordinates every test is exact, and s = 0, a touch, counts as in the way.

def blocks(p, q, box):
    (px, py), (qx, qy) = p, q
    x1, y1, x2, y2 = box
    if max(px, qx) < x1 or min(px, qx) > x2 or max(py, qy) < y1 or min(py, qy) > y2:
        return "apart: bounding boxes"
    dx, dy = qx - px, qy - py
    sides = [dx * (cy - py) - dy * (cx - px) for cx, cy in ((x1, y1), (x2, y1), (x2, y2), (x1, y2))]
    if all(s > 0 for s in sides) or all(s < 0 for s in sides):
        return f"apart: corners all on one side {sides}"
    return f"in the way {sides}"

p, q = (0, 0), (12, 6)                           # a drive along y = x / 2, in decimetres
for box in [(2, 3, 5, 6), (5, 2, 7, 4), (5, 0, 7, 2), (8, 4, 9, 7), (13, 6, 15, 8)]:
    print(box, blocks(p, q, box))

(2, 3, 5, 6) apart: corners all on one side [24, 6, 42, 60]
(5, 2, 7, 4) in the way [-6, -18, 6, 18]
(5, 0, 7, 2) apart: corners all on one side [-30, -42, -18, -6]
(8, 4, 9, 7) in the way [0, -6, 30, 36]
(13, 6, 15, 8) apart: bounding boxes

The second box straddles the line, with signs of both kinds. The fourth has its corner (8, 4) exactly on the line: s = 0, a touch. The last box lies on the line beyond q: its corners have mixed signs, and only the bounding boxes show that the drive stops short of it. Without test 1, every box anywhere along the infinite line would block.

Common mistakes: testing only whether the segment crosses a side, which misses a segment that lies wholly inside a box. Testing only the infinite line. Strict comparisons that let a touch through. Floating-point clipping where whole numbers give an exact answer.

Practice: Which Crates Are in the Way?

From cells to wheel commands

The planner speaks in cells; the wheels speak in turns and distances. Lay the grid on the floor as in Level 3: x along increasing columns, y towards decreasing rows (row 0 at the top). A step (\Delta r, \Delta c) then points along

with the minus sign because rows grow downwards. A straight step is one cell long, a diagonal one \sqrt2 cells. Group equal consecutive steps into legs. At the start of each leg the robot turns on the spot by the shortest turn from its heading to the leg's heading (Level 1: the difference wrapped into (-\pi, \pi], a half turn being +\pi), then drives the whole leg; its heading is then the leg's heading. The number of turns after the start is the number of places where a step differs from the one before.

A route of 8-connected cells 0.4 m wide, for a robot that drives at 0.5 m/s, turns at 1.2 rad/s and starts out facing up the map:

import math

def wrap(a):                                     # into (-pi, pi]; a half turn becomes +pi
    a = math.fmod(a, 2 * math.pi)
    if a <= -math.pi:
        a += 2 * math.pi
    elif a > math.pi:
        a -= 2 * math.pi
    return a

route = [(6, 1), (5, 1), (4, 1), (3, 2), (2, 3), (2, 4), (2, 5), (2, 6), (3, 6)]
cell, v, w = 0.4, 0.5, 1.2                       # metres per cell, m/s, rad/s
steps = [(r1 - r0, c1 - c0) for (r0, c0), (r1, c1) in zip(route, route[1:])]
legs = []                                        # [step, how many times in a row]
for s in steps:
    if legs and legs[-1][0] == s:
        legs[-1][1] += 1
    else:
        legs.append([s, 1])

heading, total = math.pi / 2, 0.0                # the robot starts facing up the map (+y)
for (dr, dc), count in legs:
    target = math.atan2(-dr, dc)                 # rows grow downwards, y grows upwards
    turn = wrap(target - heading)
    dist = count * cell * (math.sqrt(2) if dr and dc else 1)
    total += abs(turn) / w + dist / v
    print(f"step {(dr, dc)} x{count}: turn {math.degrees(turn):7.1f} deg, drive {dist:.3f} m")
    heading = target
print("legs", len(legs), " turns after the start", sum(1 for a, b in zip(steps, steps[1:]) if a != b),
      " time", round(total, 2), "s")

step (-1, 0) x2: turn     0.0 deg, drive 0.800 m
step (-1, 1) x2: turn   -45.0 deg, drive 1.131 m
step (0, 1) x3: turn   -45.0 deg, drive 1.200 m
step (1, 0) x1: turn   -90.0 deg, drive 0.400 m
legs 4  turns after the start 3  time 9.68 s

Negative turns are clockwise. The first leg needs no turn at all, since the robot already faces up the map, and the four legs bring three turns after the start: 45°, 45° and 90°, all to the right. Each turn costs its size divided by the turning rate, each leg its length divided by the speed: 9.68 s in all.

Common mistakes: atan2(dr, dc), which mirrors every turn. One command per step instead of one per leg. Measuring every turn from the starting heading instead of from the previous leg. A half turn coming out as -\pi. A diagonal step of length 1 (or 2).

Practice: How Long, How Many Corners, From Cells to Wheel Commands

When the map is wrong: replanning

Plans are made on the map the robot has, and maps go stale: a trolley parked in a corridor, a door closed for the night. A widely used policy is the free-space assumption: whatever the robot has not seen blocked, it assumes free. It plans, drives, adds what its sensors find to its belief, and plans again from where it stands whenever its plan runs through a cell now believed blocked. Keep the belief apart from the truth: the robot learns about a cell only when its sensors reach it.

Replanning is easier to follow, and to test, when every plan is unique. One recipe: BFS from the goal labels every believed-free cell with its distance to the goal; then, from the robot's cell, keep stepping to the first neighbour, in a fixed order, whose label is one less. Every such step stays on a shortest route, and the fixed order chooses one of them for good.

Here the robot knows nothing about the room, so its first plan is a straight run; a ring of bumpers tells it, at every cell, which of its four neighbours are blocked:

from collections import deque
truth = ["......",
         ".####.",
         "....#.",
         "..#.#.",
         "......"]
R, C = len(truth), len(truth[0])
ORDER = [(0, 1), (-1, 0), (1, 0), (0, -1)]       # this example's fixed order: E, N, S, W

def plan(blocked, start, goal):
    dist, todo = {goal: 0}, deque([goal])        # BFS from the goal over believed-free cells
    while todo:
        r, c = todo.popleft()
        for dr, dc in ORDER:
            nxt = (r + dr, c + dc)
            if 0 <= nxt[0] < R and 0 <= nxt[1] < C and nxt not in blocked and nxt not in dist:
                dist[nxt] = dist[(r, c)] + 1
                todo.append(nxt)
    if start not in dist:
        return None
    route = [start]
    while route[-1] != goal:                     # first neighbour, in ORDER, one step closer
        r, c = route[-1]
        route.append(next((r + dr, c + dc) for dr, dc in ORDER if dist.get((r + dr, c + dc)) == dist[(r, c)] - 1))
    return route

start, goal = (2, 2), (0, 5)
blocked, here, driven, replans = set(), start, [start], 0   # free-space assumption: nothing known
route = plan(blocked, here, goal)
while route is not None and here != goal:
    r, c = here
    seen = {(r + dr, c + dc) for dr, dc in ORDER if 0 <= r + dr < R and 0 <= c + dc < C
            and truth[r + dr][c + dc] == "#"} - blocked     # the bumper ring senses four neighbours
    blocked |= seen
    if seen & set(route):                        # the plan runs through a cell just found blocked
        replans += 1
        route = plan(blocked, here, goal)
        continue
    here = route[route.index(here) + 1]
    driven.append(here)
print("replans", replans, " moves", len(driven) - 1)
print(driven)
known = {(r, c) for r in range(R) for c in range(C) if truth[r][c] == "#"}
print("with the true map from the start:", len(plan(known, start, goal)) - 1, "moves")

replans 2  moves 9
[(2, 2), (2, 3), (3, 3), (4, 3), (4, 4), (4, 5), (3, 5), (2, 5), (1, 5), (0, 5)]
with the true map from the start: 9 moves

From (2, 2) the first plan runs east along row 2. At (2, 3) the robot finds (2, 4) blocked, a cell on its plan, and replans down and east along row 3; at (3, 3) it finds (3, 4) blocked and replans along row 4. Nine moves, as many as the best route on the true map; on other maps a replanning robot drives into dead ends and back out. Each replan starts from the robot's current cell, not from the start.

Common mistakes: giving the robot the whole true map at once. Planning from the start instead of the current cell. Choosing among shortest routes in a different order from the one stated. Forgetting the start cell in the list of cells driven. Looping forever when no plan exists.

Practice: The Map Was Wrong

Where the echoes land

Mapping starts with putting readings in cells. A laser reading (a, r) from pose (x, y, \theta) left at angle a from the heading, so its echo is at

and a reading of the sensor's maximum range (or more) means no echo: the beam met nothing, so there is no point to place. Level 3's grid convention then finds the cell: with the map's bottom-left corner at the origin and row 0 at the top, \text{col} = \lfloor p_x/\text{res} \rfloor and \text{row} = (\text{rows} - 1) - \lfloor p_y/\text{res} \rfloor, and a point whose floors fall outside the map is skipped.

import math
rows, cols, res, max_range = 6, 8, 0.5, 3.0
x, y, theta = 1.3, 1.1, 0.4                      # the robot's pose in the world
scan = [(-1.8, 1.4), (-1.2, 1.25), (-0.6, 2.2), (0.0, 2.65), (0.6, 2.0), (1.2, 1.6), (1.8, 3.0), (2.6, 1.1)]

def cell(px, py):
    col, up = math.floor(px / res), math.floor(py / res)
    return (rows - 1 - up, col) if 0 <= col < cols and 0 <= up < rows else None

echoes = {}
for a, r in scan:
    if r >= max_range:
        print(f"a={a:5.2f}  no echo")
        continue
    px, py = x + r * math.cos(theta + a), y + r * math.sin(theta + a)   # the beam turns with the robot
    hit = cell(px, py)
    print(f"a={a:5.2f}  point ({px:.3f}, {py:.3f})  cell {hit}")
    if hit:
        echoes[hit] = echoes.get(hit, 0) + 1
me = cell(x, y)
for r in range(rows):
    print("".join("R" if (r, c) == me else str(echoes.get((r, c), ".")) for c in range(cols)))

a=-1.80  point (1.538, -0.280)  cell None
a=-1.20  point (2.171, 0.203)  cell (5, 4)
a=-0.60  point (3.456, 0.663)  cell (4, 6)
a= 0.00  point (3.741, 2.132)  cell (1, 7)
a= 0.60  point (2.381, 2.783)  cell (0, 4)
a= 1.20  point (1.253, 2.699)  cell (0, 2)
a= 1.80  no echo
a= 2.60  point (0.211, 1.255)  cell (3, 0)
..1.1...
.......1
........
1.R.....
......1.
....1...

Seven readings came back; one of them, below the robot, lies off the map, and the reading at 1.8 rad is the sensor's 3.0 m maximum: no echo. Counting echoes per cell is the simplest map there is. It cannot tell "no echo came from here" from "this cell was never looked at", which is what the next sections fix.

Common mistakes: forgetting to add the heading. floor(py / res) as the row (the map upside down). int instead of floor for points left of or below the map. Indexing before checking the bounds. Counting a maximum-range reading as an echo. Building the grid with [[0] * cols] * rows, which repeats one row.

Practice: Where the Echoes Land

Log-odds: evidence that adds up

An occupancy grid keeps, for every cell, the probability p that it is occupied, and updates each cell on its own with Level 4's sense update for two states, occupied or free. Mapping usually describes the sensor by an inverse sensor model: P(\text{occ} \mid z), how likely the cell is to be occupied given this one reading alone, with the prior p_0 already inside. Bayes' rule then says that the odds p / (1 - p) multiply,

where the last factor takes the prior out again; otherwise every reading would count it once more. Logarithms turn the products into sums. With the log-odds \ell = \log\frac{p}{1 - p}, and back p = 1 - \frac{1}{1 + e^{\ell}}:

A cell is one number and an update is one addition. A cell starts at \ell_0 (which is 0 only when p_0 = \frac12); above \ell_0 the readings say occupied, below it free.

import math

def logit(p):
    return math.log(p / (1 - p))

def prob(l):
    return 1 - 1 / (1 + math.exp(l))

p0 = 0.3                                         # prior: 30% of this floor is furniture
model = {"echo": 0.8, "through": 0.12}           # P(occupied | this one reading), prior included
l, odds = logit(p0), p0 / (1 - p0)
for z in ["echo", "echo", "through", "echo"]:
    pz = model[z]
    l += logit(pz) - logit(p0)                   # add the evidence, take the prior out again
    odds *= (pz / (1 - pz)) / (p0 / (1 - p0))    # Bayes' rule in odds form: multiply
    print(f"{z:8} log-odds {l:7.4f}   p {prob(l):.4f}   from odds {odds / (1 + odds):.4f}")

def flip_after(free_readings, lo, hi):           # a chair is pushed onto a cell seen free for a long time
    l = logit(p0)
    for _ in range(free_readings):
        l = min(hi, max(lo, l + logit(0.12) - logit(p0)))   # clamp after every reading
    hits = 0
    while prob(l) <= 0.5:
        l = min(hi, max(lo, l + logit(0.8) - logit(p0)))
        hits += 1
    return hits

for free in (5, 50, 500):
    print(f"free {free:3} times: echoes to flip, no clamp {flip_after(free, -math.inf, math.inf):3},"
          f" clamped to [-3, 3] {flip_after(free, -3, 3)}")

echo     log-odds  1.3863   p 0.8000   from odds 0.8000
echo     log-odds  3.6199   p 0.9739   from odds 0.9739
through  log-odds  2.4748   p 0.9224   from odds 0.9224
echo     log-odds  4.7083   p 0.9911   from odds 0.9911
free   5 times: echoes to flip, no clamp   3, clamped to [-3, 3] 2
free  50 times: echoes to flip, no clamp  27, clamped to [-3, 3] 2
free 500 times: echoes to flip, no clamp 257, clamped to [-3, 3] 2

Both columns agree: adding log-odds is Bayes' rule. From the prior, one echo gives exactly what the inverse model says, 0.8.

Clamping. Without limits, a cell seen free 500 times sits at a huge negative log-odds, and when a chair is pushed onto it the map needs 257 echoes to believe it. Real rooms change, so maps clamp the log-odds into [\ell_{\min}, \ell_{\max}] after every update. Then a cell can change its mind within a couple of readings, whatever its history. The order of readings now matters, and clamping once at the end gives different answers.

Common mistakes: starting at 0 when the prior is not \frac12. Forgetting to subtract \ell_0 from each reading's evidence. Clamping only at the end. Writing the inverse as \frac{1}{1 + e^{\ell}}, which is the probability of free.

Practice: Is That Tile a Chair Leg?

A beam frees its path and marks its end

One beam from pose (x, y, \theta) at angle a with range r says two things: the cells it flew through are free, and the cell where it stopped holds something. On a grid:

1. If r is below the maximum range, the beam hit something at distance d = r. Otherwise there was no echo, and the beam is followed to d = the maximum range; its end cell stays unchanged, because nothing was seen there.
2. Find the robot's cell A and the end point's cell B with the grid formula. Keep the formula's pair of whole numbers even when the end point is off the map.
3. Walk the cells from A to B with the cell rule of the line-of-sight section. Cells k = 0, \dots, n - 1, the robot's own cell included, get \ell_{\text{free}} (negative); B, at k = n, gets \ell_{\text{occ}} (positive) if the beam hit.
4. Skip cells off the map before indexing: Python's negative indices would quietly write into the opposite edge of the map.

The fixed steps \ell_{\text{occ}} and \ell_{\text{free}} are the last section's \operatorname{logit} P(\text{occ} \mid z) - \ell_0, worked out once. A whole map is many beams from many poses, applied in order, with a clamp after every change:

import math
rows, cols, res, max_range = 4, 7, 0.5, 2.5
l_occ, l_free, lo, hi = 0.85, -0.4, -1.0, 2.0
L = [[0.0] * cols for _ in range(rows)]

def cell(px, py):                                # may lie off the map: checked when used
    return (rows - 1 - math.floor(py / res), math.floor(px / res))

def add(r, c, v):
    if 0 <= r < rows and 0 <= c < cols:          # off the map: skip, never index
        L[r][c] = min(hi, max(lo, L[r][c] + v))  # clamp after every change

def beam(pose, a, rng):
    x, y, theta = pose
    hit = rng < max_range
    d = rng if hit else max_range                # no echo: follow it to max_range
    (r0, c0) = cell(x, y)
    (r1, c1) = cell(x + d * math.cos(theta + a), y + d * math.sin(theta + a))
    dr, dc = r1 - r0, c1 - c0
    n = max(abs(dr), abs(dc))
    crossed = [(r0 + (2 * k * dr + n) // (2 * n), c0 + (2 * k * dc + n) // (2 * n)) for k in range(n)]
    for r, c in crossed:
        add(r, c, l_free)                        # the beam flew through: free
    if hit:
        add(r1, c1, l_occ)                       # it stopped here: occupied
    return crossed, (r1, c1), hit

scans = [((0.6, 0.7, 0.0), [(0.3, 2.1), (-0.5, 2.5), (0.7, 1.45), (1.6, 1.2)]),
         ((2.4, 1.3, math.pi), [(0.0, 2.2), (0.0, 2.2), (-0.8, 1.2), (2.4, 1.15)])]
for pose, readings in scans:
    for a, rng in readings:
        crossed, end, hit = beam(pose, a, rng)
        print(pose[:2], a, "free", crossed, "end", end, "hit" if hit else "no echo")
for row in L:
    print("".join("#" if v > 0 else "." if v < 0 else "?" for v in row), [round(v, 2) for v in row])

(0.6, 0.7) 0.3 free [(2, 1), (2, 2), (2, 3), (1, 4)] end (1, 5) hit
(0.6, 0.7) -0.5 free [(2, 1), (3, 2), (3, 3), (4, 4)] end (4, 5) no echo
(0.6, 0.7) 0.7 free [(2, 1), (1, 2)] end (0, 3) hit
(0.6, 0.7) 1.6 free [(2, 1), (1, 1)] end (0, 1) hit
(2.4, 1.3) 0.0 free [(1, 4), (1, 3), (1, 2), (1, 1)] end (1, 0) hit
(2.4, 1.3) 0.0 free [(1, 4), (1, 3), (1, 2), (1, 1)] end (1, 0) hit
(2.4, 1.3) -0.8 free [(1, 4), (0, 4)] end (-1, 3) hit
(2.4, 1.3) 2.4 free [(1, 4), (2, 5)] end (2, 6) hit
?#?#.?? [0.0, 0.85, 0.0, 0.85, -0.4, 0.0, 0.0]
#....#? [1.7, -1.0, -1.0, -0.8, -1.0, 0.85, 0.0]
?...?.# [0.0, -1.0, -0.4, -0.4, 0.0, -0.4, 0.85]
??..??? [0.0, 0.0, -0.4, -0.4, 0.0, 0.0, 0.0]

The second beam had no echo: it frees the cells it crosses, and its end cell (4, 5), below the map, is skipped. The seventh beam is a hit whose end lies above the map, so it only frees. The robot's first cell (2, 1) was crossed four times and sits at the floor of the clamp, -1.0; (1, 0) was hit twice, 1.7. Every ? is a cell no beam has touched yet.

Common mistakes: marking the end cell after a beam with no echo. Skipping the robot's own cell. int instead of floor, or Python's round, in the cell rule. Dropping the whole beam when its end leaves the map instead of only the cells off the map. Clamping once at the end instead of after every change.

Practice: One Beam Clears a Path, The Night Shift Draws the Map

A wall is a line: normal form and perpendicular least squares

Indoors, most laser points lie on walls, and a wall is a line. The Statistics line y = a + bx cannot describe a wall straight ahead of the robot (x = 2.5 has no slope), so robots use the normal form

where (\cos\alpha, \sin\alpha) is the unit vector from the robot (the origin) straight towards the line and d > 0 is the line's distance. A point (x, y) is |x\cos\alpha + y\sin\alpha - d| from the line.

The best line through some points minimises the sum of squared perpendicular distances (total least squares). Whatever \alpha is, the best d puts the line through the mean point (\bar x, \bar y). With the centred sums S_{xx} = \sum (x - \bar x)^2, S_{yy} = \sum (y - \bar y)^2 and S_{xy} = \sum (x - \bar x)(y - \bar y), the sum to minimise is

which is smallest when (\cos 2\alpha, \sin 2\alpha) points opposite to \big(\frac{S_{xx} - S_{yy}}{2}, S_{xy}\big):

If d comes out negative, the normal points away from the line: use -d and \alpha + \pi, wrapped into (-\pi, \pi]. The least-squares line of Statistics minimises the vertical distances instead, which is right when x is known exactly and wrong for a wall, where both coordinates are measured.

import math
import numpy as np
wall = [(2.53, -1.0), (2.47, -0.5), (2.55, 0.0), (2.46, 0.5), (2.52, 1.0), (2.49, 1.5)]   # robot frame
n = len(wall)
mx, my = sum(p[0] for p in wall) / n, sum(p[1] for p in wall) / n
sxx = sum((x - mx) ** 2 for x, y in wall)
syy = sum((y - my) ** 2 for x, y in wall)
sxy = sum((x - mx) * (y - my) for x, y in wall)

b = sxy / sxx                                    # Statistics' line y = a + b x: vertical distances
print("y on x: slope", round(b, 3), " intercept", round(my - b * mx, 3))

alpha = 0.5 * math.atan2(-2 * sxy, syy - sxx)    # perpendicular distances
d = mx * math.cos(alpha) + my * math.sin(alpha)
if d < 0:                                        # make the normal point from the robot to the wall
    d, alpha = -d, alpha + math.pi
    alpha = alpha - 2 * math.pi if alpha > math.pi else alpha
print("normal form: alpha", round(alpha, 4), " d", round(d, 4))
print("residuals", [round(x * math.cos(alpha) + y * math.sin(alpha) - d, 3) + 0.0 for x, y in wall])

_, _, vt = np.linalg.svd(np.array(wall) - [mx, my])
normal = vt[1]                                   # the direction the points spread least
if normal @ [mx, my] < 0:                        # numpy may return either sign: point it at the wall
    normal = -normal
print("svd normal", np.round(normal, 4).tolist(), " cos, sin", round(math.cos(alpha), 4), round(math.sin(alpha), 4))

y on x: slope -5.526  intercept 14.084
normal form: alpha 0.008  d 2.5053
residuals [0.017, -0.039, 0.045, -0.041, 0.023, -0.003]
svd normal [1.0, 0.008]  cos, sin 1.0 0.008

The wall is 2.505 m ahead and almost square to the robot (\alpha = 0.008). Fitting y against x tilts it by almost 10°: with vertical distances, the scatter of the x values pulls the slope. The residuals are the signed perpendicular distances, all within 5 cm. With numpy, the SVD of the centred points gives the same line: the last row of vt is the direction in which the points spread least, the normal (numpy may return it with either sign).

Common mistakes: fitting y = a + b x. atan instead of atan2. Forgetting the flip, so d comes out negative. Wrapping into [-\pi, \pi) instead of (-\pi, \pi]. Computing the sums from uncentred points.

Practice: The Wall Behind the Dots

RANSAC: let the points vote

A scan holds clutter as well as walls: legs, boxes, stray reflections. Least squares listens to every point, so a few of them tilt the line. RANSAC (random sample consensus) turns the fit around. Two points propose a line; every point within a tolerance of it is an inlier; the proposal with the most inliers wins, and the final line is fitted to the winner's inliers alone. The distance from p to the line through a and b is a cross product over a length (Level 2):

Real RANSAC draws random pairs and stops early. For a short scan, trying every pair in a fixed order gives one exact answer; a tie then needs a rule, such as "the earliest pair wins".

import math
scan = [(-1.0, 1.52), (-0.5, 1.47), (0.0, 1.5), (0.5, 1.53), (1.0, 1.49), (1.5, 1.51),   # a wall on the left
        (0.6, 0.4), (0.8, 0.45), (1.3, 0.9), (0.2, 2.6)]                                 # legs, a box, a reflection

def fit(points):                                 # perpendicular least squares, normal form, d > 0
    n = len(points)
    mx, my = sum(p[0] for p in points) / n, sum(p[1] for p in points) / n
    sxx = sum((x - mx) ** 2 for x, y in points)
    syy = sum((y - my) ** 2 for x, y in points)
    sxy = sum((x - mx) * (y - my) for x, y in points)
    alpha = 0.5 * math.atan2(-2 * sxy, syy - sxx)
    d = mx * math.cos(alpha) + my * math.sin(alpha)
    if d < 0:
        d, alpha = -d, alpha + math.pi
        alpha = alpha - 2 * math.pi if alpha > math.pi else alpha
    return round(alpha, 4), round(d, 4)

def distance(p, a, b):                           # from p to the line through a and b
    vx, vy = b[0] - a[0], b[1] - a[1]
    return abs(vx * (p[1] - a[1]) - vy * (p[0] - a[0])) / math.hypot(vx, vy)

print("all points:", fit(scan))
best = []
for i in range(len(scan)):
    for j in range(i + 1, len(scan)):
        if scan[i] == scan[j]:
            continue
        crowd = [k for k, p in enumerate(scan) if distance(p, scan[i], scan[j]) <= 0.05]
        if len(crowd) > len(best):               # strictly more: the earlier pair keeps a tie
            best, pair = crowd, (i, j)
print("winning pair", pair, "inliers", best)
print("refit:     ", fit([scan[k] for k in best]))

q = (6 / 10) * (5 / 9)                           # a random pair is two wall points
for k in (1, 5, 10, 20):
    print(k, "random pairs: P(at least one is all wall) =", round(1 - (1 - q) ** k, 4))

all points: (1.099, 1.3909)
winning pair (0, 2) inliers [0, 1, 2, 3, 4, 5]
refit:      (1.5731, 1.5028)
1 random pairs: P(at least one is all wall) = 0.3333
5 random pairs: P(at least one is all wall) = 0.8683
10 random pairs: P(at least one is all wall) = 0.9827
20 random pairs: P(at least one is all wall) = 0.9997

Fitted to all ten points, the wall's normal leans to \alpha = 1.10 and its distance shrinks to 1.39 m; the four stray points pulled it. The pair (0, 2) gathers the six wall points and none of the clutter, and the refit puts the wall 1.503 m to the left, nearly parallel to the robot's heading (\alpha = 1.573, close to \pi/2). Random pairs work almost as well: a pair is two wall points with probability \frac{6}{10} \cdot \frac59 = \frac13, so 10 random pairs miss the wall only 1.7% of the time. The more clutter, the more pairs it takes.

Common mistakes: < where the tolerance is "at most". Letting a later pair win a tie. Returning the proposing pair's line instead of the refit. Dividing by zero for two equal points. Refitting with y = a + bx.

Practice: RANSAC Picks the Wall From the Clutter

Split-and-merge: cutting a scan into walls

A laser sweeps, so its points come in order, and a room becomes a chain of walls. Split-and-merge cuts the chain where it bends. The chord of a piece runs from its first point to its last.

• Split. Find the point of the piece farthest from its chord (the perpendicular distance above). If it is farther than a threshold, cut there and split both halves the same way; otherwise the piece is one wall. This is also called the iterative end-point fit.
• Merge. The farthest point is not always a corner, so some cuts are unnecessary. Walk the sorted cuts from left to right and drop a cut when every point between the last kept cut and the next cut lies within the threshold of their chord.

import math
scan = [(0.0, 1.92), (0.5, 1.95), (1.0, 2.03), (1.5, 1.97), (2.0, 2.05),
        (2.5, 1.95), (2.8, 1.65), (3.1, 1.35)]   # in sweep order: a wall ahead, then one slanting away

def dist(k, i, j):                               # point k to the chord through points i and j
    (ax, ay), (bx, by), (px, py) = scan[i], scan[j], scan[k]
    vx, vy = bx - ax, by - ay
    return abs(vx * (py - ay) - vy * (px - ax)) / math.hypot(vx, vy)

def split(i, j, threshold, cuts):
    if j - i < 2:
        return
    k = max(range(i + 1, j), key=lambda k: dist(k, i, j))     # max keeps the first of equals
    if dist(k, i, j) > threshold:
        cuts.add(k)
        split(i, k, threshold, cuts)
        split(k, j, threshold, cuts)

for threshold in (0.15, 0.05):
    cuts = {0, len(scan) - 1}
    split(0, len(scan) - 1, threshold, cuts)
    b = sorted(cuts)
    kept = [b[0]]
    for t in range(1, len(b) - 1):
        a, c = kept[-1], b[t + 1]                # chord from the last KEPT breakpoint
        if any(dist(k, a, c) > threshold for k in range(a + 1, c)):
            kept.append(b[t])
    kept.append(b[-1])
    print("threshold", threshold, " split", b, " merged", kept)

threshold 0.15  split [0, 4, 5, 7]  merged [0, 5, 7]
threshold 0.05  split [0, 4, 5, 7]  merged [0, 4, 5, 7]

At a threshold of 0.15 m the first split, on the chord from point 0 to point 7, cuts at point 4, the farthest from it. The piece from 4 to 7 still bends, so point 5, the true corner, is cut too: breakpoints 0, 4, 5, 7. The merge then finds that points 1 to 4 all lie within 0.15 m of the chord from 0 to 5, and drops 4: two walls, from 0 to 5 and from 5 to 7. At 0.05 m, points 2 and 4 are too far from that chord (0.098 and 0.106 m), so the cut at 4 stays: the threshold decides how bumpy a wall may be.

Common mistakes: >= where the split rule is "greater than". Measuring the merge chord from the previous breakpoint in the sorted list instead of the last kept one. Distances to the ends of the chord instead of to its line. Deep recursion on a long scan (a list used as a stack of pieces does the same job).

Practice: Cutting the Scan Into Walls

Where two walls meet

Corners make good landmarks: they pin down a position in both directions, where a single wall pins down only one. Two walls in normal form, (\alpha_1, d_1) and (\alpha_2, d_2), meet where both equations hold:

Cramer's rule (Level 3) solves it; the determinant is \cos\alpha_1\sin\alpha_2 - \sin\alpha_1\cos\alpha_2 = \sin(\alpha_2 - \alpha_1):

Two lines cross at the same angle as their normals, but a line has no direction, so the angle between the lines is |\alpha_1 - \alpha_2| reduced modulo \pi and folded into [0, \pi/2]. Nearly parallel walls make a determinant near 0 and a corner that jumps far away with every bit of noise, so check the angle first and refuse a corner below a minimum.

import math

def corner(line1, line2, min_angle):
    (a1, d1), (a2, d2) = line1, line2
    phi = abs(a1 - a2) % math.pi                 # the angle between the normals, mod a half turn
    if phi > math.pi / 2:
        phi = math.pi - phi                      # a line has no direction: fold into [0, pi/2]
    if phi < min_angle:
        return None                              # nearly parallel: no trustworthy corner
    det = math.sin(a2 - a1)                      # cos a1 sin a2 - sin a1 cos a2
    x = (d1 * math.sin(a2) - d2 * math.sin(a1)) / det
    y = (d2 * math.cos(a1) - d1 * math.cos(a2)) / det
    return round(x, 4), round(y, 4), round(phi, 4)

shelf, wall = (0.25, 2.0), (1.95, 1.4)           # (alpha, d) in the robot's frame
x, y, phi = corner(shelf, wall, 0.3)
print("corner", (x, y), "angle", phi, "rad =", round(math.degrees(phi), 1), "deg")
for a, d in (shelf, wall):
    print("  on the line?", round(x * math.cos(a) + y * math.sin(a) - d, 4) + 0.0)
print(corner((0.25, 2.0), (-2.9, 1.0), 0.3))     # normals 3.15 rad apart: almost parallel lines
print(corner((0.25, 2.0), (-1.4, 1.0), 0.3))

corner (1.5243, 2.1145) angle 1.4416 rad = 82.6 deg
  on the line? 0.0
  on the line? 0.0
None
(2.2253, -0.631, 1.4916)

The shelf and the wall meet at (1.524, 2.115), at 82.6°, and the corner satisfies both equations. The second pair's normals are 3.15 rad apart: almost a half turn, so the lines are almost parallel (0.008 rad), and no corner is reported. In the third, the difference 1.65 rad folds to \pi - 1.65 = 1.49 rad. The corner may lie beyond the walls' end points: the lines are what meet.

Common mistakes: reporting the angle in [0, \pi] instead of folding it. Solving before checking for nearly parallel lines. The determinant's sign or a swapped numerator. Assuming the corner lies between the walls' ends.

Practice: Where Two Walls Meet

Two views of the same landmarks: rigid alignment

A robot that measures the same landmarks before and after a drive can work out the drive. In the old frame, every landmark satisfies \text{old}_i \approx R(\theta)\,\text{new}_i + t, where (t_x, t_y, \theta) is the new pose seen from the old one (Level 1's relative pose). With noisy points, take the \theta and t that minimise

This has a closed form in two steps. Translation: for any rotation, the best t lays the rotated mean of the new points on the mean of the old ones, t = \bar p - R(\theta)\,\bar q (\bar p the mean of old, \bar q of new). Rotation: subtract the means (p'_i, q'_i); the error is a constant minus 2\sum_i p'_i \cdot R(\theta)q'_i, and that sum is C\cos\theta + S\sin\theta with

C adds dot products and S cross products: the best rotation turns each centred new point towards its old partner, weighted by how far the points are from the centre. With numpy the same answer comes from the SVD of the 2 \times 2 matrix H = \sum_i q'_i\,{p'_i}^T (the Kabsch method): R = V U^T, with one sign flipped if \det R = -1, which would be a mirror image.

import math
import numpy as np
posts = [(3.0, 1.0), (4.5, 2.0), (2.5, 3.5), (5.0, 4.0), (1.0, 2.5)]       # world frame, metres

def seen(pose, pts, wobble):                     # world -> robot frame (Level 1), plus a small error
    x, y, t = pose
    c, s = math.cos(t), math.sin(t)
    return [(c * (px - x) + s * (py - y) + e, -s * (px - x) + c * (py - y) - e) for (px, py), e in zip(pts, wobble)]

A, B = (0.5, 0.5, 0.2), (1.3, 0.9, 0.65)         # the robot before and after a short drive
old = seen(A, posts, [0.01, -0.02, 0.0, 0.015, -0.01])
new = seen(B, posts, [-0.01, 0.0, 0.02, -0.015, 0.01])

n = len(old)
px, py = sum(p[0] for p in old) / n, sum(p[1] for p in old) / n
qx, qy = sum(q[0] for q in new) / n, sum(q[1] for q in new) / n
C = sum((bx - qx) * (ax - px) + (by - qy) * (ay - py) for (ax, ay), (bx, by) in zip(old, new))
S = sum((bx - qx) * (ay - py) - (by - qy) * (ax - px) for (ax, ay), (bx, by) in zip(old, new))
theta = math.atan2(S, C)
c, s = math.cos(theta), math.sin(theta)
tx, ty = px - (c * qx - s * qy), py - (s * qx + c * qy)
rms = math.sqrt(sum((c * bx - s * by + tx - ax) ** 2 + (s * bx + c * by + ty - ay) ** 2
                    for (ax, ay), (bx, by) in zip(old, new)) / n)
print("estimate", round(tx, 4), round(ty, 4), round(theta, 4), " rms", round(rms, 4))

ca, sa = math.cos(A[2]), math.sin(A[2])          # the truth: B seen from A (Level 1's relative pose)
print("truth   ", round(ca * (B[0] - A[0]) + sa * (B[1] - A[1]), 4),
      round(-sa * (B[0] - A[0]) + ca * (B[1] - A[1]), 4), round(B[2] - A[2], 4))

H = (np.array(new) - [qx, qy]).T @ (np.array(old) - [px, py])    # Kabsch: 2x2 SVD
U, _, Vt = np.linalg.svd(H)
R = Vt.T @ U.T
print("svd angle", round(math.atan2(R[1, 0], R[0, 0]), 4), " det", round(np.linalg.det(R), 4))

estimate 0.8586 0.239 0.448  rms 0.0308
truth    0.8635 0.2331 0.45
svd angle 0.448  det 1.0

Five posts seen from two poses, each coordinate off by up to 2 cm, give the drive to within 8 mm and 0.002 rad; the rms error of 3 cm is the noise that no rigid motion can explain. The SVD agrees on the angle. Swapping the roles of the two scans gives the inverse motion, angle -\theta and a different translation, so read carefully which scan is mapped onto which.

Common mistakes: computing C and S from uncentred points. Averaging the angles of individual pairs, which breaks near \pm\pi. Mapping old onto new when the problem asks for new onto old. Dividing by n - 1 in the rms. Forgetting the reflection check with the SVD.

Practice: Two Scans of the Same Posts

ICP: when nobody tells you the pairs

A real scan does not say which map point each laser point belongs to. ICP (iterative closest point) guesses: place the scan with the current pose guess, pair every placed point with its nearest map point, drop pairs farther apart than a gate (a person, an open door: points the map does not have), solve the rigid alignment of the last section for a correction (\Delta\theta, \Delta t), and apply it. The correction moves the whole placed scan, the robot with it:

Then pair again from the improved pose, and repeat. Each round the pairs get better, as long as the start is close enough.

import math

def walls(step, offset):                         # points along the walls of a 4 x 2.5 m room
    pts, k = [], 0
    while offset + k * step < 13.0:              # the perimeter, walked anticlockwise from (0, 0)
        t = offset + k * step                    # k * step: no rounding errors piling up
        k += 1
        if t < 4.0:
            pts.append((t, 0.0))
        elif t < 6.5:
            pts.append((4.0, t - 4.0))
        elif t < 10.5:
            pts.append((10.5 - t, 2.5))
        else:
            pts.append((0.0, 13.0 - t))
    return pts

room = walls(0.1, 0.0)                           # the map: a point every 10 cm
true = (1.6, 1.0, 0.3)
c, s = math.cos(true[2]), math.sin(true[2])
scan = [(c * (x - true[0]) + s * (y - true[1]), -s * (x - true[0]) + c * (y - true[1]))
        for x, y in walls(0.55, 0.23)] + [(0.8, 0.2)]          # what the laser saw, plus a person

def wrap(a):
    a = math.fmod(a, 2 * math.pi)
    return a - 2 * math.pi if a > math.pi else a + 2 * math.pi if a <= -math.pi else a

def icp_step(pose, max_pair):
    x, y, t = pose
    c, s = math.cos(t), math.sin(t)
    pairs = []
    for sx, sy in scan:
        w = (x + c * sx - s * sy, y + s * sx + c * sy)              # place the point with the guess
        m = min(room, key=lambda p: math.dist(p, w))                # nearest map point (first on ties)
        if math.dist(m, w) <= max_pair:                             # gate: no partner for the person
            pairs.append((w, m))
    n = len(pairs)
    wx, wy = sum(w[0] for w, m in pairs) / n, sum(w[1] for w, m in pairs) / n
    mx, my = sum(m[0] for w, m in pairs) / n, sum(m[1] for w, m in pairs) / n
    C = sum((w[0] - wx) * (m[0] - mx) + (w[1] - wy) * (m[1] - my) for w, m in pairs)
    S = sum((w[0] - wx) * (m[1] - my) - (w[1] - wy) * (m[0] - mx) for w, m in pairs)
    dt = math.atan2(S, C)
    dc, ds = math.cos(dt), math.sin(dt)
    ex, ey = mx - (dc * wx - ds * wy), my - (ds * wx + dc * wy)
    return (dc * x - ds * y + ex, ds * x + dc * y + ey, wrap(t + dt)), n   # the robot moves with its scan

pose = (1.3, 1.2, 0.1)                           # odometry's guess; the truth is (1.6, 1.0, 0.3)
for it in range(1, 13):
    pose, n = icp_step(pose, 0.5)
    if it % 3 == 0:
        print(f"{it:2}: ({pose[0]:.4f}, {pose[1]:.4f}, {pose[2]:.4f})  pairs {n} of {len(scan)}")
good = pose
pose = (2.4, 1.5, 1.2)                           # a guess that is far off
for it in range(8):
    pose, n = icp_step(pose, 0.5)
print(f"far-off start ends at ({pose[0]:.4f}, {pose[1]:.4f}, {pose[2]:.4f})  pairs {n} of {len(scan)}")
x, y, t = good
plot([p[0] for p in room], [p[1] for p in room], kind="scatter", label="map")
plot([x + math.cos(t) * a - math.sin(t) * b for a, b in scan],
     [y + math.sin(t) * a + math.cos(t) * b for a, b in scan], kind="scatter", label="scan")

 3: (1.5000, 1.0711, 0.2010)  pairs 24 of 25
 6: (1.5637, 1.0361, 0.2483)  pairs 24 of 25
 9: (1.5945, 1.0092, 0.2753)  pairs 24 of 25
12: (1.6079, 1.0076, 0.2796)  pairs 24 of 25
far-off start ends at (2.2322, 1.5211, 1.2545)  pairs 13 of 25

From odometry's guess, 36 cm and 0.2 rad off, ICP creeps towards the true pose (1.6, 1.0, 0.3) and is within about 1 cm and 0.02 rad after 12 rounds; the person is never paired. Point-to-point ICP is slow along walls, because a point that slides along a wall barely changes its nearest neighbour. From a guess 0.9 rad off the truth, it settles on a wrong pose with only 13 of 25 points paired, where every placed point is near some wall but most are near the wrong one. ICP refines a good guess; it does not find a lost robot (that is Level 4's job). Press Run to see the scan laid over the map after the twelfth round.

Common mistakes: adding \Delta t to the position without rotating the position by \Delta\theta first. Forgetting to wrap the heading. Comparing squared distances with the gate instead of its square. Letting the last of several equally near map points win when the first should. Pairing points that are far from everything.

Practice: ICP: Sliding the Scan Into Place

Practice

These problems are required on this path at this level; the path adds one problem from each of the level's topics, picked for you and kept.

• Room for a Round Robot
• A* With Room to Spare
• Keep Off the Walls
• One Beam Clears a Path
• RANSAC Picks the Wall From the Clutter
• Two Scans of the Same Posts

Checklist

You can now:

• Grow obstacles by a robot's radius and plan for its centre as a point, with the boundary rule stated.
• Use breadth-first search for reachability, regions and clearance (many sources at once), and A* on the configuration space with a heuristic that fits the moves.
• Price clearance in a cost map and find the cheapest route with Dijkstra; put the heading into the state when turns cost time.
• List the cells on a line by an exact rounding rule, cut a staircase route into straight legs, and test a straight drive against boxes exactly.
• Turn a route of cells into turn-and-drive commands, and replan under the free-space assumption.
• Place laser echoes in grid cells, keep each cell's evidence in clamped log-odds, and update a map beam by beam.
• Fit a wall in normal form by perpendicular least squares, find it among clutter with RANSAC, cut a scan into walls with split-and-merge, and intersect two walls.
• Align two scans of the same landmarks in closed form, and refine a pose with ICP.

Further reading: Wikipedia: Occupancy grid mapping and Wikipedia: Iterative closest point.