Bonus: Let the table fix the camera

Fix the camera, not every height

In step 3 you measured every height from the fitted table. You can go one better and use the table to correct camera.pose itself: then every point comes out right, with no plane to carry around.

The fitted plane $n \cdot p + d = 0$ is where the table appears. You know where it really is: $z = 0$, with normal $\hat z = (0, 0, 1)$. The correction $C$ is the rigid motion that lays the one on the other.

Turning one vector onto another

The smallest rotation that turns the unit vector $n$ onto $\hat z$ turns about their cross product $v = n \times \hat z$. With $c = n \cdot \hat z = n_z$, the cosine of the angle between them,

where $[v]_\times$ is the matrix with $[v]_\times\, p = v \times p$. No trigonometry, and an already level table ($n = \hat z$) gives $v = 0$ and $R = I$.

After that rotation the table is the plane $z + d = 0$, so lift everything by $d$. $C$ is the 4 × 4 matrix with $R$ in its top-left corner and $(0, 0, d)$ as its translation. The camera's view goes into the world as it believed and then gets corrected, so the fixed pose is $C\,T$.

What a plane can't tell you

A flat table looks the same however you slide the camera along it or turn it about the vertical. So the plane pins only three of the pose's six numbers: two tilts and the height. That's enough here, because this camera's error is a tilt and a shift up or down. To find the other three you need something with corners, like the box of step 5, or the hand-eye calibration of Seeing with a Camera.

Your task

Write level_pose(T, n, d), returning the corrected 4 × 4 pose. The program stores it as fixed, rebuilds the cloud with it, checks that the table now lies flat at $z = 0$, and hovers the fingertips 1 cm above the tallest object using nothing but its $z$.