Lab 6 · Seeing the Path

What you do in this lab

AprilTags gave the car a waypoint where you stuck a tag. This lab gives it one with no tag at all: the depth camera looks at the floor ahead, and your code turns that picture into one number, where the open path is. That number is what steers the car in the next lab.

The avoid frame with the open-path mask, its centroid, and the steering offset — // This lab's own `avoid` frame, a duct on the left: open floor in green, its centroid in yellow, offset +0.53. Steer right, around the duct.

Do this: read the five cards below. Then in Play step through the six frames of one lap of the car on the duct track, and switch the view: depth, mask, path, rgb. One tuning that gets all six frames right finishes the lab.

The depth frame

The same depth frame as raw grayscale millimeters and through the color map — // The `open` frame twice. Left: the raw numbers, brightness = distance. Right: the same numbers through the color map.

The car's camera makes a second picture next to the color one. It is not a photo. Every pixel is one number: the distance to whatever is there, in millimeters. The floor two meters out is the number 2000. A pixel that reads 0 means "no measurement", never "touching".

Raw, the numbers are a gray ramp your eye cannot read. The color map spends thousands of colors on the same range so near pops from far. The rainbow is for you. Your code reads the numbers.

Where depth goes missing

The same depth frame raw with black dropout holes, and hole-filled — // The `wall` frame, nose in the duct. Left: raw, black is dropout, 39% of the frame. Right: the same frame with hole_fill 2. The `holes` frame is worse: 74%.

Black pixels are holes: places the camera could not measure. Shiny surfaces, dark materials, the edges of objects, and anything too close. A hole reads 0 mm. Left alone, it is never open floor.

Do this in Play: pick the holes frame in the depth view. Move hole_fill from 0 to 2 to 3. With 2 a hole becomes far, so an obstacle's missing edge reads as open. With 3 a hole becomes near. That is the cautious pick when the question is "can I drive there".

The mask

The avoid frame with the near mask raw and after open+close — // The `avoid` frame. Left: the raw mask inside the box. Right: after open + close with kernel 5. 62% of the box is duct, so with ratio 0.5 this frame is BLOCKED.

The pipeline never looks at the whole frame. It looks at a box on the floor just ahead of the bumper, the ROI (region of interest). Inside the box, every pixel farther than the threshold is open floor, white. Everything else is obstacle, black. That is the mask.

A raw mask is full of speckle and gaps. Morphology cleans it: open eats specks, close fills gaps. Then one rule: BLOCKED when obstacle covers more than ratio of the box.

Do this in Play: switch to the mask view. Set kernel to 0 and watch the speckle come back. Set it to 5 again. Then drag the box up to the horizon on the avoid frame: 13% obstacle, OPEN. Same duct, same threshold. The box is the only part of the picture the next second of driving depends on.

The path: a blob and its centroid

The avoid frame's cleaned mask with each open blob numbered, the winner outlined, and the offset arithmetic — // The `avoid` frame's cleaned mask, magnified. Blob 1 (13506 px) touches the top edge and wins. Blob 2 (111 px) is a pocket at the bottom left: it does not reach the top, so it cannot be the path.

A mask is pixels. A blob is pixels that touch. One OpenCV call labels every white region and hands back its area and its centroid. Two rules pick the path: the blob must touch the top of the box (open floor you can see through a gap under the duct is not floor you can drive onto), and it must beat min_area (speckle that survived cleanup must not win the wheel).

The centroid is the mean of the blob's pixels. In the box above that is x = 276 of 360. The offset scales it to the box: (276 - 180) / 180 = +0.53. Zero is the middle, -1 the left edge, +1 the right edge.

Do this in Play: switch to the path view on the avoid frame and read the offset on the bar. Raise min_area and watch the small blob go red in the mask panel below. Then pick wall: no blob reaches the top, so the honest answer is "boxed in". The car stops.

The pull

The offset from the centroid becomes a pull on the steering — // The `avoid` frame again: offset +0.53 is a pull to the right, around the duct. The offset is an error, how far the open path sits from the middle of the box, and an error is what a controller eats.

Depth frame, mask, blob, centroid, offset. That number is the whole result of this lab, and it is the same shape as the heading error the policy saw in The Twin: zero means straight on, the sign says which way.

The next lab, Obstacle Avoidance, feeds it to the wheel: you drive, and your code pulls the steering toward the open path while you do. The one file you tuned here is the one that pulls.

Follow the White Rabbit

The depth camera is an Intel RealSense D436. Two infrared cameras a known distance apart look at the same scene, and a projector sprays an infrared dot pattern so even a blank wall has texture to match. The same point lands on different pixels in the two images; that shift is the disparity, and depth is Z = f * B / d. The spec is about 2% at 2 m. Point it at a blank wall with the laser off and the depth goes away where the scene has no texture of its own. A visual SLAM rig sees those dots too, and a dot pattern that moves with your car poisons its features, so SLAM setups often run the laser off and pay with sparser depth.

How do you know the depth image is right? Measure the same thing two ways. AprilTags did: a tag's pose from geometry against the depth at the tag, within a few percent. These frames once shipped with the wrong intrinsics and every pose read 58% too far. The depth image caught it.

librealsense: depth from stereo - how the camera makes the frame, and why a pixel with no match reads 0 mm.
OpenCV: morphological operations - erosion, dilation, open, close, with pictures.
OpenCV: connectedComponentsWithStats - every blob's area, bounding box and centroid in one pass.
Connected-component labeling - the algorithm underneath, and why 4- vs 8-connectivity changes which speckle joins the path.

Play

the mask inside the box, before and after cleanup (white = open floor, black = obstacle) - the winning blob outlined, its centroid dotted

draw a box on the image to set the watched region

depth_min_thres 500

kernel 5

ratio 0.5

min_area 100

hole_fill 0

what do these knobs do?

roi box: the watched region - drag a box on the image to set it (roi_x, roi_y = top-left corner, roi_w, roi_h = size, in px)
depth_min_thres: the threshold (mm): farther is open floor, closer is obstacle
kernel: square size used by open/close - bigger = stronger cleanup, 0 = off
ratio: BLOCKED when obstacle covers more than this fraction of the box
min_area: smallest open blob (px) worth steering at - speckle cannot win the centroid
hole_fill: fills depth dropout (black): 0 off, 1 from-left, 2 farthest (bg), 3 nearest (fg)

Code

"""Seeing the Path - from a depth frame to a steering number.

Six real frames from one lap of T4YLOR on the duct track: blocked, wall and
holes (nose in the duct; holes has most of its depth missing), avoid (duct
on the left, floor on the right), edge and open. One pipeline, one tuning,
all six right:

1. Crop the ROI (region of interest) onto the path ahead.
  2. Open floor = anything FARTHER than depth_min_thres. That is the mask.
     Dropout reads 0 mm, so a hole is never open floor until hole_fill
     repairs it.
  3. Morphology cleans the mask: open eats speckle, close fills gaps.
     kernel 0 turns it off so you can see why it is needed.
  4. BLOCKED when obstacle (not open) covers more than `ratio` of the box.
  5. The biggest open blob that touches the top of the box and beats
     min_area is the path. Its centroid reads off a steering offset in
     [-1, 1]: the error signal Obstacle Avoidance feeds the wheel.
"""

import cv2
import numpy as np

from goat_vision import viz
from goat_vision.params import goat_param

def crop(depth: np.ndarray, p) -> tuple[np.ndarray, tuple[int, int, int, int]]:
    """The roi_* box: everything downstream sees only this slice of the frame.
    Returns (roi, (x, y, w, h)) with w/h clamped to the frame edge."""
    x, y = p.roi_x, p.roi_y
    w = min(p.roi_w, depth.shape[1] - x)
    h = min(p.roi_h, depth.shape[0] - y)
    return depth[y : y + h, x : x + w], (x, y, w, h)

def make_mask(roi_depth: np.ndarray, p) -> tuple[np.ndarray, np.ndarray]:
    """(raw mask, cleaned mask) of OPEN floor inside the ROI: 1 = farther than
    the threshold. A hole reads 0 mm, so with hole_fill off it is never open."""
    raw = (viz.fill_holes(roi_depth, p.hole_fill) > p.depth_min_thres).astype(np.uint8)
    if p.kernel > 0:
        k = np.ones((p.kernel, p.kernel), np.uint8)
        clean = cv2.morphologyEx(raw, cv2.MORPH_OPEN, k)
        clean = cv2.morphologyEx(clean, cv2.MORPH_CLOSE, k)
    else:
        clean = raw
    return raw, clean

def blocked(clean: np.ndarray, p) -> tuple[bool, float]:
    """(verdict, obstacle coverage): BLOCKED when the not-open share of the
    box beats ratio."""
    coverage = 1.0 - float(np.sum(clean)) / clean.size
    return coverage > p.ratio, coverage

def find_path(clean: np.ndarray, p):
    """(offset in [-1, 1], centroid (cx, cy)) of the path, or (None, None).
    The winning blob must TOUCH THE TOP of the box - open floor you can see
    through a gap but cannot drive into is not a path - and beat min_area.
    Its true centroid is where the car should go."""
    n, labels, stats, centroids = cv2.connectedComponentsWithStats(clean, connectivity=8)
    best, best_area = None, 0
    for lab in range(1, n):  # label 0 is background
        area = stats[lab, cv2.CC_STAT_AREA]
        if area < p.min_area or stats[lab, cv2.CC_STAT_TOP] != 0:
            continue
        if area > best_area:
            best, best_area = lab, area
    if best is None:
        return None, None
    cx, cy = int(centroids[best][0]), int(centroids[best][1])
    w = clean.shape[1]
    return (cx - w / 2) / (w / 2), (cx, cy)  # -1 = far left of the box, +1 = far right

def see(depth_mm: np.ndarray, p) -> dict:
    """The whole pipeline on one frame: the box, both masks, the verdict and
    the path. The harness draws from this dict; the car's loop steers by
    offset."""
    roi, rect = crop(depth_mm, p)
    raw, clean = make_mask(roi, p)
    is_blocked, coverage = blocked(clean, p)
    offset, centroid = find_path(clean, p)
    return {
        "rect": rect,
        "raw": raw,
        "clean": clean,
        "blocked": is_blocked,
        "coverage": coverage,
        "offset": offset,
        "centroid": centroid,
    }

Output