Cleaning up a cloud

Real depth is messy

From now on the depth camera behaves like a real one. Every distance carries noise that grows with the square of the distance (about 1.5 mm at 1 m), and a few pixels in a thousand report a "flying pixel" somewhere in mid-air. The raw cloud also holds the floor, the robot and far too many points. Three steps clean it up.

1. Crop

Keep only the points inside the workspace box with corners LO and HI: one boolean mask, crop, already written.

2. Voxel downsampling

Cut space into cubes of side $s$ (voxels) starting from the corner LO. Point $p$ falls in voxel $\lfloor (p - \text{LO}) / s \rfloor$, three whole numbers. Replace the points in each voxel by their mean: 30,000 points become about 4,500, spread evenly and with the noise averaged away. A flying pixel is alone in its voxel, so dropping voxels with fewer than MIN_POINTS points removes it.

np.unique(cells, axis=0, return_inverse=True, return_counts=True) lists the occupied voxels, which voxel each point is in, and how many points each holds. np.add.at(sums, which, points) adds every point into its voxel's row.

3. Normals from neighbours

A surface's normal is the direction it faces. Take a point's neighbours within 2 cm and centre them on their mean $\bar p$. Their scatter matrix $C = \sum_i (p_i - \bar p)(p_i - \bar p)^\top$ has eigenvalues $\lambda_0 \le \lambda_1 \le \lambda_2$, the spread along its three eigenvectors. On a surface the patch spreads in two directions and hardly at all in the third: that eigenvector is the normal. np.linalg.eigh returns eigenvalues in ascending order, so the normal is the first column of the eigenvector matrix.

$\lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2)$ measures flatness: near 0 on a flat face, larger on edges and curved surfaces.

Your task

Write voxel_downsample(points, lo, size, min_points) and pca_normal(neighbours), which returns (normal, flatness).

Two pedestals stand on the table, but only one has a flat top: the other is tilted, and a ball would roll off it. The program finds the points that are high up, flat and facing straight up, and puts the red ball on them. The ball must stay there.