Pick by shape

Colour isn't enough

Every ball now has a twin: a cube or a can of the same colour and the same 5 cm width. world.balls() is switched off, and finding colours in the picture would pick the twins as often as the balls. Their shape tells them apart, and the cloud has it.

A sphere is a linear fit

A sphere with centre $c$ and radius $r$ holds the points with $\|p - c\|^2 = r^2$. Multiply it out and move the unknowns to one side:

That's linear in the four unknowns $(c_x, c_y, c_z, k)$: each point $(x, y, z)$ gives one row of a system $A\,u = b$, with row $[2x, 2y, 2z, 1]$ in $A$ and $x^2 + y^2 + z^2$ in $b$. Least squares (np.linalg.lstsq) solves it in one go, then $r = \sqrt{k + \|c\|^2}$. It works with only the near half of the ball in view, and its centre is about a millimetre from the truth, where the bounding box of step 4 fell a centimetre short.

Ball or not?

Any set of points fits some sphere. How well it fits is the RMS misfit, the typical distance of the points from the fitted surface: $\sqrt{\tfrac1N \sum_i (\|p_i - c\| - r)^2}$. A ball's points lie on its sphere to within the sensor's noise. A cube's corners and a can's rim stick out from any sphere by millimetres, and a cube's fitted radius comes out too big. So a ball has $r$ close to BALL_R and a small misfit.

The program prints both numbers for every object. Look at them before you choose your thresholds.

Your task

Write fit_sphere(points), returning (centre, radius, rms), and is_ball(points), returning True or False. The program then looks, picks a ball at its fitted centre (at that centre's height above the table), drops it in the cup and looks again, until no balls are left.

The grader also tries is_ball on every object in two new arrangements. For the bonus, never close the gripper over a twin.