Section 28.2: Point clouds and depth maps

"A depth map is a promise about where the surfaces are; a point cloud is that promise turned into coordinates the arm can act on."

A Patient Embodied AI Agent

Problem First: Why This Representation Exists

Show a robot a perfect, pin-sharp photo of a coffee cup and it still cannot pick the cup up: a 2D image says what something looks like but says nothing about how far away it is. A robot arm planning to grasp a cup needs to know the cup is 0.42 m away, not just that it occupies 80 pixels. A mobile base avoiding a chair leg needs the leg's actual position in the room, not its screen location. Depth maps solve this by attaching a distance measurement to each pixel; point clouds solve it further by giving every sample a metric position in a shared coordinate frame. Figure 28.2A shows this lift from a flat depth image to a sparse, filterable cloud of metric points. Without this step, manipulation and navigation planning cannot reason about reachability, clearance, or collision.

A depth value is not decoration: it is the difference between a gripper that closes on an object and one that closes on air two centimeters short. Figure 28.2.2 traces the geometry step by step, from a single depth pixel through the intrinsics matrix to a metric point in the world frame.

For point clouds and depth maps, audit depth holes, registration error, voxel size (where a voxel is a small 3D cube used to group nearby points for downsampling), outlier removal, normal estimation, and frame transforms. Registration, the process of aligning two point clouds captured from different viewpoints or times into one consistent coordinate frame, is covered in depth in section 29.5; here it matters only as one of several places where a bad frame transform can silently corrupt a cloud. The action evidence is whether those choices changed grasp, collision, or clearance decisions. Treat the representation as a typed state estimate, not as a visualization.

Figure 28.2.1 should be read as the Point clouds and depth maps handoff diagram: sensor evidence, geometric representation, uncertainty, latency, and action consumer are separate failure points.

Mathematical Core

Back-projection converts each valid depth pixel into a 3D point in the camera frame.

The camera intrinsics \(K\) define the ray for each pixel. The transform \(T_{wc}\) moves the point into the world or robot frame, where it can be merged, filtered, and queried by action modules.

Before reading on, consider this: if a depth sensor reports a pixel as invalid because the target surface is transparent glass, and your pipeline silently treats that as zero depth, where does the back-projected point land in the robot's world frame? The answer changes whether the planner believes there is an obstacle at the camera origin or no obstacle at all.

Worked Miniature

Code Fragment 28.2.1 back-projects a 2 by 2 depth map into four 3D points. This tiny array is the same math Open3D applies to thousands of pixels.

# Back-project a tiny depth map into camera-frame points.
# Each pixel becomes one metric sample after applying intrinsics.
import numpy as np

depth = np.array([[1.0, 1.2], [0.9, 1.1]])
fx = fy = 500.0
cx = cy = 0.5
points = []
for v in range(depth.shape[0]):
    for u in range(depth.shape[1]):
        z = depth[v, u]
        x = (u - cx) * z / fx
        y = (v - cy) * z / fy
        points.append((round(x, 4), round(y, 4), round(float(z), 2)))
print(points)

These expected output samples are nearly centered laterally, so the main variation is depth, not horizontal spread. That is the interpretation to carry into planning: the cloud suggests a mostly frontal surface patch whose geometry changes along z by about 30 cm.

Back-projection gives each point a position, but a gripper needs more than position: it needs to know which way the surface faces. Normal estimation assigns a surface orientation vector to each point by fitting a local plane to its nearest neighbors. A gripper that approaches a cup at the wrong angle will slip. The grasp planner needs surface normals to align the finger pad perpendicular to the contact surface. On a legged robot, normals on foothold candidates indicate slope: a 30-degree normal tilt signals a surface the foot may slide on. Without normals, the planner sees geometry but not orientation. This reduces grasping to position-only control and drives up slip failures on real hardware. Adding surface normals to a bin-picking system typically cuts re-grasp attempts from roughly 8 per object to 1 or 2, because the gripper arrives aligned to the contact surface rather than relying on compliance to correct a skewed approach.

The algorithm collects the \(k\) nearest neighbors for each point (typically \(k = 10\) to \(30\)), computes their covariance matrix, and takes the eigenvector (a direction that the covariance matrix leaves unrotated, only rescaled) with the smallest eigenvalue (the amount of spread along that direction) as the normal, flipping it toward the viewpoint to resolve orientation. At surface boundaries the neighbors straddle two surfaces, so the fitted plane mixes both and the normal lands between them. That is why the table flags instability on mixed surfaces.

Consider a specific case. The Intel RealSense D435 reports depth at 848x480 at 90 fps, with a usable range of roughly 0.2 m to 3 m and roughly 2% depth error at 1 m. At 1 m range and fx = fy = 600, a single pixel offset shifts the back-projected point by about 1.7 mm laterally. That is fine for a 5 cm cup but not for a 3 mm connector pin. Legged platforms such as Boston Dynamics' Spot typically use similar structured-light or stereo depth sensors to build local point-cloud terrain maps for foothold selection, commonly at voxel sizes around 2 cm for navigation and sub-centimeter crops for manipulation. The sensor choice, voxel size, and filtering threshold are therefore design decisions tied directly to the tolerance of the consuming action.

Debugging And Evaluation

Evaluate the representation inside the consuming action loop with calibration, frame transform, representation version, latency, selected action, and failure label.

Perturb exactly one geometric assumption, such as depth dropout, scale, occlusion, pose drift, motion, or calibration, then record the action change.