Finding the table

The table isn't where you think

The table top is $z = 0$ in the robot's frame, so "keep points with $z > 1$ cm" ought to separate the objects from the table. But that only works if camera.pose is perfect, and no calibration is. From this step on, the pose you're given is off by a degree or so, as real ones are. A 1° error tilts the table in the cloud by 9 mm over half a metre, and here it also sits up to 1.5 cm too high or too low. Look at the grey points in the 3-D view: they float above the real table on one side and sink into it on the other.

So don't assume where the table is: measure it. A plane is $n \cdot p + d = 0$ with $n$ a unit normal; a point's signed distance from it, $n \cdot p + d$, is its height above the table.

RANSAC

Least squares on every point is dragged towards the objects. RANSAC (random sample consensus) ignores them:

1. Pick 3 random points $a, b, c$. Their plane has normal $n = (b - a) \times (c - a)$, normalised (skip it if that's nearly zero: the points are in a line).
2. Count the inliers: points with $|n \cdot (p - a)| <$ threshold (5 mm, a few times the noise).
3. Repeat, keeping the plane with the most inliers.
4. Refit to those inliers. That's pca_normal from step 2 again: the table's normal is its direction of least spread. Then $d = -n \cdot \bar p$, with $\bar p$ the inliers' mean.

Make $n$ point up ($n_z > 0$), so heights above the table come out positive.

How many tries? If a fraction $w$ of the points are table, one sample is all table with probability $w^3$. With $w = 0.8$, 100 tries all miss with odds below $10^{-20}$.

Your task

Write fit_plane(points, threshold, iters) returning (n, d). The program colours the table grey and the objects blue, compares with a plain $z$ threshold, and hovers the fingertips 2 cm above the tallest object, using its height above your plane.

The grader also tests fit_plane on three tilted synthetic tables and on a new image from this camera.