From depth to points

A picture with distances

In Seeing with a Camera, a pixel only told you a direction, so you needed a known plane to find out how far away the ball was. The camera has been measuring that distance all along: img.depth is a 240 × 320 array of floats with one distance per pixel. It now shows in the Images tab next to the colour picture, light where things are near and dark blue where they are far.

Each value is $z$, the distance along the camera's optical axis (not along the ray), in metres. Pixels that saw nothing at all hold inf.

Back-projection

Projection divides by depth: $u = f_x\,x_c / z + c_x$. With $z$ known, run it backwards. The pixel in column $u$ and row $v$ saw the point

$$x_c = \frac{(u - c_x)\,z}{f_x}, \qquad y_c = \frac{(v - c_y)\,z}{f_y}, \qquad z_c = z$$

in the camera's frame, where $f_x, f_y$ are the focal lengths and $(c_x, c_y)$ the principal point from $K$. The camera pose (rotation $R$, position $t$) moves it into the world: $p = R\,p_c + t$.

Every pixel at once

No loops: v, u = np.indices(depth.shape) gives every pixel's row and column, seen = np.isfinite(depth) masks out the misses, and u[seen] lists the columns of the pixels that saw something, row by row. Stack $x_c, y_c, z_c$ into an $(N, 3)$ array. Rotating every row at once is points @ R.T, because each row $p^\top$ becomes $(R\,p)^\top = p^\top R^\top$.

The result is a point cloud: about 77,000 points, one per pixel, that trace every surface the camera saw. viz.cloud(points, colour) draws one in the 3-D view.

Your task

Write depth_to_points(depth, K, T) so it returns an $(N, 3)$ array of world points, one for every finite pixel, in row order.

The program keeps the points over the table, draws them, finds the highest one and moves the gripper 5 cm above it. The grader checks your function on a rendered depth image and a made-up camera, checks top, and checks where the gripper ends up.