Bonus: Where is the camera?

The camera got bumped

Someone knocked the camera stand. The camera has turned by a few degrees and shifted a few centimetres, which is enough to miss every grasp, so camera.pose is switched off. You'll measure the pose again using the robot itself, because its encoders know exactly where the gripper is.

A magenta marker now sits on the grasp point, so arm.ee_position() is its 3-D position $p_i$. The starter shows it to the camera at 16 places spread out in 3-D and finds its pixel $(u_i, v_i)$ at each.

This is eye-to-hand calibration: the camera stays put and the robot carries a marker to positions it knows, so finding the camera's pose (its extrinsics) comes down to matching known 3-D points with pixels. Classic hand–eye calibration solves $AX = XB$ instead. It pairs robot motions $A$ with the camera motions $B$ they cause to find a fixed transform $X$, such as where a camera sits on the gripper, without knowing where the target is. Here the marker is exactly on the grasp point, so the known points are enough.

The direct linear transform

$K$ is known, so undo it first: $\hat x_i = K^{-1}[u_i, v_i, 1]^\top$. What's left is the world-to-camera transform $M = [R_{cw} \mid t_{cw}]$, with $\hat x_i \propto M P_i$ where $P_i = [p_i; 1]$. Writing $m_k$ for the rows of $M$, each point gives two equations that are linear in $M$'s 12 entries:

Stack them into $A\,m = 0$ ($2N \times 12$); the best $m$ is the last row of np.linalg.svd(A)[2]. Then tidy up:

1. Scale $M$ so the rows of $R_{cw}$ have length 1. Negate it if the points come out behind the camera ($z_c < 0$).
2. Noise leaves $R_{cw}$ slightly non-orthogonal: with U, S, Vt = np.linalg.svd(R_cw), use U @ Vt.
3. Invert: $R = R_{cw}^\top$, $t = -R_{cw}^\top t_{cw}$.

Then polish

DLT is exact on perfect data but sensitive to noise when the points fill only part of the view. So the starter's refine() finishes with a few Gauss–Newton steps on the reprojection error. That's the standard recipe: a linear method for the first guess, nonlinear least squares for the last millimetres.

Your task

Implement calibrate(points3d, pixels) so it returns the 4 × 4 T_world_cam. The grader checks it on 12 perfect correspondences (it must be exact), then compares the refined pose_estimate with the real pose: within 3° and 2 cm. The bump changes on every run.