Camera geometry and object queries

Why this matters

The Sensor encoders module says cameras become tokens and then a BEV grid. Two pieces of machinery sit underneath and are assumed everywhere in the driving literature. First, geometry: which pixel does a 3D point land on, and where is a point seen a moment ago now? Second, set prediction: a scene has an unknown number of objects with no natural order, so how do you write a loss for "predict all the cars"? This module covers both with code you can run.

1. Frames

A driving stack uses several coordinate frames, and most bugs are a point in the wrong one.

• World (or map) frame: fixed to the ground.
• Vehicle (ego) frame: attached to the car. In this module, x forward, y left, z up.
• Camera frame: attached to a camera. x right, y down, z forward, so z is the depth.
• Image (pixel) frame: 2D coordinates on the sensor, origin at the top left.

Moving a point between frames is a rigid transform: a rotation $R$ and a translation $t$, written as one 4×4 matrix. The extrinsics are where each camera sits and points relative to the vehicle (calibrated once per car). The intrinsics describe the camera itself.

2. The pinhole camera

For a point $(X, Y, Z)$ in the camera frame, with focal lengths $f_x, f_y$ (in pixels) and principal point $(c_x, c_y)$:

The division by depth $Z$ is why far objects are small and parallel lines converge. The reference code takes points in the vehicle frame, applies the inverse of the camera's pose to move them into the camera frame, and projects:

def project_points(
    points_ego: torch.Tensor,
    intrinsics: torch.Tensor,
    camera_to_ego: torch.Tensor,
) -> tuple[torch.Tensor, torch.Tensor]:
    """Projects 3D points from the vehicle frame into a pinhole camera.

    The vehicle frame has x forward, y left, z up. The camera frame has x right, y
    down, z forward (depth). ``camera_to_ego`` is the camera's pose in the vehicle
    frame (the extrinsics); its inverse maps vehicle points into the camera.

    Args:
        points_ego: Shape (n, 3).
        intrinsics: The 3x3 matrix [[fx, 0, cx], [0, fy, cy], [0, 0, 1]].
        camera_to_ego: A 4x4 rigid transform.

    Returns:
        Pixel coordinates (n, 2) and depths (n,). A point with depth <= 0 is behind
        the camera and gets NaN pixels.
    """
    ego_to_camera = torch.linalg.inv(camera_to_ego)
    homogeneous = torch.cat([points_ego, torch.ones(len(points_ego), 1)], dim=1)
    camera = (homogeneous @ ego_to_camera.T)[:, :3]
    depth = camera[:, 2]
    pixels = camera @ intrinsics.T
    ahead = depth > 0
    safe_depth = torch.where(ahead, depth, torch.ones_like(depth))
    uv = torch.where(ahead[:, None], pixels[:, :2] / safe_depth[:, None], torch.nan)
    return uv, depth

The tests check the properties that matter: a point straight ahead at camera height lands on the principal point; left in the vehicle frame is left in the image; doubling the distance halves the offset from the image centre; a point behind the camera gets a negative depth and no pixel.

Why this matters for learning. Going from a 3D point to a pixel is easy. Going from a pixel to a 3D point is not: a pixel is a ray, and every depth along it projects to the same place. This is why camera-to-BEV methods must estimate depth, or let attention figure out where to look. BEVFormer (2203.17270) takes the second route: grid-shaped BEV queries each pull features from the camera regions that their 3D location projects to, where the regions are the ones a query's 3D location projects to, the operation above (we have not re-checked that detail against the paper's method section), and earlier BEV frames are fused in to give temporal context.

3. Ego-motion: where is that object now?

A model that uses frames from the past has to put them in the same frame. A static sign seen 0.5 s ago is not at the same vehicle-relative position now, because the car has moved. Going through the world frame, with $T_{\text{then}}$ and $T_{\text{now}}$ the vehicle's poses in the world:

def pose_matrix(x: float, y: float, heading: float) -> torch.Tensor:
    """A 3x3 planar pose: where the vehicle is in a world frame."""
    c, s = math.cos(heading), math.sin(heading)
    return torch.tensor([[c, -s, x], [s, c, y], [0.0, 0.0, 1.0]])


def to_current_frame(
    points_then: torch.Tensor, pose_then: torch.Tensor, pose_now: torch.Tensor
) -> torch.Tensor:
    """Re-expresses (n, 2) points seen in the vehicle frame at time then, at time now.

    Static things do not move in the world, but the vehicle does, so their position
    relative to the vehicle changes. Going via the world frame:
    now_frame = pose_now^-1 @ pose_then @ points_then.

    Args:
        points_then: Shape (n, 2) in the vehicle frame at the earlier time.
        pose_then: 3x3 vehicle pose in the world at the earlier time.
        pose_now: 3x3 vehicle pose in the world now.
    """
    homogeneous = torch.cat([points_then, torch.ones(len(points_then), 1)], dim=1)
    moved = homogeneous @ (torch.linalg.inv(pose_now) @ pose_then).T
    return moved[:, :2]

The tests: driving 4 m forward brings a point 10 m ahead to 6 m, a 90° left turn moves a point ahead to the right, and no motion changes nothing. Moving objects need more (their own motion), which is why temporal models use both ego-motion and a motion model. The same re-framing applies to a plan that arrives late (the Deploying a driving VLA module).

4. Predicting a set of objects

A detector must output "all the cars", but the number varies and there is no natural order. A network with a fixed number of output slots can emit them in any order, so which output should be compared with which ground-truth object? Comparing slot $i$ with object $i$ punishes a perfect answer that lists the objects in a different order.

DETR (2005.12872) treats detection as direct set prediction. It uses a small, fixed number of learned object queries, vectors that attend to the image and each produce one prediction, and a loss computed over the whole set, where bipartite matching forces unique predictions. The matching finds the one-to-one assignment of predictions to ground-truth objects with the lowest total cost, and the loss is then computed on the matched pairs.

def hungarian_match(cost: torch.Tensor) -> list[tuple[int, int]]:
    """Optimal one-to-one assignment of rows to columns minimising total cost.

    Dynamic programming over subsets of columns, exact and fine for the handful of
    objects in a teaching example (it is exponential in the number of columns; real
    detectors use the O(n^3) Hungarian algorithm).

    Args:
        cost: Shape (rows, cols) with rows <= cols.

    Returns:
        (row, column) pairs, one per row.

    Raises:
        ValueError: If there are more rows than columns or more than 16 columns.
    """
    rows, cols = cost.shape
    if rows > cols:
        raise ValueError(f"need rows <= cols, got {rows} x {cols}")
    if cols > 16:
        raise ValueError(f"at most 16 columns supported, got {cols}")
    best: dict[int, tuple[float, list[int]]] = {0: (0.0, [])}
    for row in range(rows):
        step: dict[int, tuple[float, list[int]]] = {}
        for used, (total, chosen) in best.items():
            for col in range(cols):
                if used >> col & 1:
                    continue
                key = used | (1 << col)
                candidate = total + cost[row, col].item()
                if key not in step or candidate < step[key][0]:
                    step[key] = (candidate, [*chosen, col])
        best = step
    _, columns = min(best.values(), key=lambda item: item[0])
    return list(enumerate(columns))


def set_loss(predicted: torch.Tensor, target: torch.Tensor) -> tuple[float, float]:
    """Loss between predicted and target object positions, with and without matching.

    Args:
        predicted: Shape (q, 2): q query outputs, one position each.
        target: Shape (m, 2) with m <= q ground-truth objects.

    Returns:
        The mean distance after optimal matching (unmatched predictions are ignored
        here; a real detector also trains them to say "no object"), and the mean
        distance when targets are paired with predictions in index order.
    """
    cost = torch.cdist(target, predicted)
    matched = hungarian_match(cost)
    matched_loss = sum(cost[r, c].item() for r, c in matched) / len(matched)
    ordered_loss = cost.diagonal().mean().item()
    return matched_loss, ordered_loss

The reference code finds that assignment exactly with a small dynamic program (real detectors use the Hungarian algorithm, which is polynomial; ours is exponential in the number of columns and capped at 16). The tests compare it with brute force on random costs, with SciPy when available, and with a case where a greedy choice blocks a better pairing:

A perfect answer given in a different order has zero loss after matching and a large loss without it. Unmatched queries are trained to predict "no object", which is how the model learns how many objects there are.

Where queries show up in driving. The same idea appears wherever the output is an unordered set: detected agents, map elements, and, in Alpamayo-R1's vision encoder, a fixed set of query vectors that compress many cameras and timesteps into a few tokens (the Deploying a driving VLA module).

Reference implementation (PyTorch)

libs/tutorial/geometry.py; tests are in libs/tutorial/tests/test_geometry.py. Run uv run --group tutorial python -m libs.tutorial.geometry.

Check yourself

1. Which two matrices does projecting a vehicle-frame point to a pixel use, and what does each describe?
2. Why can a pixel not be turned back into a 3D point without more information?
3. A static object was 10 m ahead 0.5 s ago and the car has driven 4 m straight. Where is it now?
4. Why does a set loss need matching, and what trains the unmatched queries?

Quiz

Answer each question; feedback appears as soon as you choose.

Sources

• Carion et al., End-to-End Object Detection with Transformers, 2020 (DETR).
• Li et al., BEVFormer, 2022.