"Everyone celebrates the depth map. Nobody thanks the depth map for the collision that never happened."
An Underappreciated AI Agent
The Big Picture
The previous six sections built the depth toolkit: how sensors measure range, how a depth map becomes a metric point cloud, how frames are calibrated and clouds are registered, which 3D representation to store, and how uncertain every number really is. This section is where that toolkit earns its keep. Two domains dominate the demand for real-time 3D: robots, which must not hit things, must know where they are, and must pick things up; and augmented reality, which must place virtual content so convincingly into a live scene that a rendered mug sits behind your real coffee cup and gets occluded by your hand. Both reduce to the same three geometric questions asked hundreds of times per second: where is free space, where is the surface, and where am I relative to both. We trace how depth flows into a navigation costmap, into a grasp, and into an AR occlusion test, and we confront the latency and uncertainty budgets that separate a demo from a shipping product.
This section assumes you can turn a depth image into a point cloud in a metric frame (Sections 40.2 and 40.3), choose a representation such as an occupancy voxel grid (Section 40.5), and reason about per-pixel depth error (Section 40.6). The pose-tracking that ties frames together over time is the subject of Chapter 52, and we lean on it here rather than rebuild it. No robotics background is assumed; each application is developed from its geometric query.
Robot navigation: from depth to free space
A mobile robot's first job is to not collide. The classical machinery for this is the costmap: a 2D or 3D grid where each cell holds a traversal cost derived from occupancy. Depth is the primary input. Each frame, the robot projects its depth image into 3D, drops points that fall outside a height band (the floor below the wheels, the ceiling above the sensor), and marks the remaining points as obstacles in the grid. Crucially it also marks the space along each ray, in front of the hit as free: a depth measurement is not just evidence of a surface at range \(z\), it is evidence of emptiness everywhere between the sensor and \(z\). This ray-casting step is what lets a robot clear a costmap cell after a person walks away, and it is why a raw obstacle-only map drifts into paralysis.
Occupancy is fused over time in log-odds, exactly the recursive Bayesian update introduced in Chapter 4. A cell seen occupied many times becomes confidently solid; a transient flicker from sensor noise decays away. The depth uncertainty of Section 40.6 sets the update weights directly: a stereo camera's error grows as \(z^2\), so a hit at 8 m should nudge the map far less than a hit at 1 m. Ignore that and a robot brakes for phantom walls at the edge of its range. The code block below builds a minimal top-down occupancy grid from a single depth frame, the operation a navigation stack runs every cycle.
import numpy as np
def depth_to_occupancy(depth, fx, fy, cx, cy, cell=0.05, grid_m=6.0,
floor=0.05, ceil=1.8):
"""Project a metric depth image into a top-down occupancy grid.
depth: HxW array of metric depth (meters), 0 = invalid.
Returns an NxN uint8 grid: 1 = obstacle, 0 = unknown/free."""
H, W = depth.shape
us, vs = np.meshgrid(np.arange(W), np.arange(H))
z = depth.ravel()
valid = z > 0
# Back-project pixels to camera-frame 3D points (Section 40.2).
x = (us.ravel() - cx) * z / fx
y = (vs.ravel() - cy) * z / fy
x, y, z = x[valid], y[valid], z[valid]
# Keep only points within the robot's collision height band.
band = (y > -ceil) & (y < -floor) # y points down in camera frame
x, z = x[band], z[band]
# Rasterize the ground-plane (x, z) footprint into grid cells.
n = int(grid_m / cell)
gx = ((x + grid_m / 2) / cell).astype(int)
gz = (z / cell).astype(int)
keep = (gx >= 0) & (gx < n) & (gz >= 0) & (gz < n)
grid = np.zeros((n, n), np.uint8)
grid[gz[keep], gx[keep]] = 1
return grid
# Synthetic frame: a wall at 2 m fills the lower half of the image.
depth = np.zeros((120, 160), np.float32)
depth[60:, :] = 2.0
occ = depth_to_occupancy(depth, fx=120, fy=120, cx=80, cy=60)
print("occupied cells:", int(occ.sum()))
The height-band filter in that function is not a detail, it is the whole trick that turns a wall of points into a floor plan a planner can use. Points near the ground are the floor and must be ignored; points at wheel or torso height are what will actually stop the robot.
Manipulation: grasping from a point cloud
Where navigation asks "where is free space", manipulation asks "where is the surface, precisely". A robot arm picking parts from a bin needs the object's local geometry to millimeter accuracy so it can place a gripper on a graspable patch. The modern pipeline segments the target's point cloud, estimates surface normals (recall that a normal is only meaningful with the connectivity or neighborhood structure discussed in Section 40.5), and either fits a known CAD model by registration (Section 40.4) or runs a learned grasp network that proposes 6-DoF gripper poses directly on raw points. This is where a robot's exteroceptive depth stream meets the proprioceptive and tactile senses of Chapter 57: vision plans the grasp, touch confirms it.
Key Insight
Robotics and AR are the same geometry problem viewed from opposite sides. A robot uses depth to decide where it may move matter (free space, graspable surface, self-localization). AR uses depth to decide where it may draw matter (occlusion, placement, anchoring). Both need the identical three primitives: a free-space model, a surface model, and a pose. Build those three well and a navigation stack and an AR engine share most of a codebase.
Augmented reality: occlusion, placement, and anchoring
An AR system's credibility rests on depth. Three jobs depend on it. Placement needs plane detection: the system finds horizontal and vertical surfaces so a virtual object rests on the real table instead of floating. Occlusion needs a per-pixel depth of the real scene: before drawing a virtual pixel, the renderer compares the virtual fragment's depth against the sensed real depth, and if the real surface is nearer, the virtual pixel is discarded. This single test is what makes a rendered character walk convincingly behind a real sofa. Anchoring needs pose: the virtual content must stay locked to its world position as the user moves, which is the tracking half of SLAM again.
Modern phone AR frameworks (Apple's ARKit, Google's ARCore) fuse a sparse visual-inertial pose with a depth stream, from a dedicated time-of-flight sensor on high-end devices or from monocular depth estimation elsewhere, to produce both a coarse scene mesh and a per-frame environment depth map. The monocular route connects directly to Chapter 41, where a single camera yields dense depth with no active emitter at all, and to the neural scene reconstructions of Chapter 51.
Practical Example: an AR tape measure that respects your walls
Consider a home-improvement app that lets a user measure a room by tapping corners on the phone screen. Each tap is a 2D pixel; the app must turn it into a 3D world point. It ray-casts the tap through the camera model into the fused environment depth, reads the metric range at that pixel, and back-projects to a world coordinate anchored by ARKit's pose. Two measured corners give a Euclidean distance. The failure modes are pure Section 40.6 material: on a dark or glossy wall the time-of-flight return is weak and depth is noisy, so the app snaps the tap to the nearest detected plane instead of trusting a single pixel, and it refuses to report a measurement when the plane fit residual is large. The product lesson is that the depth number is never used naked; it is always regularized by a surface prior and gated by its own uncertainty.
The shared constraint: real time on a power budget
What unites both domains is a brutal latency budget. A robot avoiding a walking person and an AR headset stabilizing an anchor both need the perceive-to-act loop closed in tens of milliseconds; a 200 ms lag turns a smooth overlay into visible swim and turns obstacle avoidance into a collision. This forces the whole pipeline (projection, fusion, planning, rendering) onto an embedded processor with a fixed power envelope, the territory of Chapter 59. It is why costmaps are 2D when 2D suffices, why voxel grids are truncated to a few meters around the agent, and why depth is often downsampled before fusion. The engineering art is spending the millisecond and milliwatt budget on the geometry that actually changes the next action.
Library Shortcut
The hand-rolled projection above is instructive but partial: a real navigation map needs ray-cast free-space clearing and probabilistic fusion. OctoMap provides exactly this as a mature octree occupancy library. Inserting a full cloud with automatic ray-casting and log-odds update is two calls, tree.insertPointCloud(cloud, origin) then a probability query, replacing roughly 150 lines of grid ray-tracing, hit/miss bookkeeping, and octree memory management. You supply the registered cloud and sensor origin; the library handles adaptive resolution, unknown-versus-free distinction, and serialization. For the point-cloud front end (voxel downsampling, normal estimation, plane and cluster segmentation) Open3D collapses each into a single call, which is the subject of this chapter's lab.
Exercise
Extend depth_to_occupancy to also mark free space. For each valid pixel, before setting the obstacle cell, walk the grid cells along the ray from the sensor origin \((n/2, 0)\) to the hit cell and set them to a distinct "free" value (say 2) where they are still unknown. Then feed two consecutive frames in which a simulated obstacle moves one meter sideways, and confirm that the cells it vacated flip from occupied back toward free. Which decays faster with a naive overwrite versus a log-odds update, and why does that matter for a robot deciding whether a doorway is now passable?
Self-Check
1. Why does a robot mark space in front of a depth hit as free, and what goes wrong if it only ever marks obstacles?
2. In AR occlusion, exactly which two depth values are compared per pixel, and what is drawn when the real one is smaller?
3. A stereo camera's depth error grows with \(z^2\). Give one concrete way this should change how a distant measurement updates a navigation costmap versus a near one.
Lab 40
load, register, downsample, and segment point clouds with Open3D.
Bibliography
Robot mapping and navigation
The reference octree occupancy-mapping system: probabilistic, ray-cast, memory-efficient, and the practical default behind the library shortcut in this section.
Introduces the layered costmap navigation stack that turns depth and laser scans into traversal costs; the conceptual basis for the occupancy-grid pipeline here.
Grasping and manipulation from depth
A landmark showing that grasps can be learned directly on depth-derived point clouds with synthetic supervision, closing the depth-to-manipulation loop.
Detects 6-DoF grasp candidates on raw, unsegmented clouds, illustrating the surface-precision query that manipulation places on depth.
Augmented reality and dense reconstruction
Newcombe, R. A., et al. (2011). KinectFusion: Real-Time Dense Surface Mapping and Tracking. ISMAR.
The system that made real-time depth-camera reconstruction practical, seeding modern AR occlusion and scene meshing via truncated signed-distance volumes.
Apple (2023). ARKit Developer Documentation: Scene Depth and Plane Detection.
The production reference for how a shipping AR platform fuses visual-inertial pose with depth for placement, occlusion, and anchoring.
Point-cloud tooling
Zhou, Q.-Y., Park, J., & Koltun, V. (2018). Open3D: A Modern Library for 3D Data Processing.
The library behind Lab 40: one-call voxel downsampling, normal estimation, registration, and segmentation for the depth pipelines in this chapter.
Rusu, R. B., & Cousins, S. (2011). 3D is Here: Point Cloud Library (PCL). ICRA.
The foundational open-source point-cloud toolkit whose filtering, segmentation, and registration modules underpin most robotics depth stacks.
What's Next
In Chapter 41, we drop the active emitter entirely and ask a single ordinary camera for dense metric depth. Foundation models like Depth Anything and Metric3D now deliver zero-shot depth good enough to feed the very costmaps, grasps, and occlusion tests you just built, turning any phone or webcam into a 3D sensor.