6D Object Pose Estimation

To grasp an object, a robot needs its full pose: where it is and how it is turned, six numbers in 3D. If the robot has a model of the object, it can find that pose by aligning the model to the sensor data. The grasp, defined once on the model, then moves with it.

Starting guess: angle error Starting guess: position error Clutter points

Model-based pose estimation

The model points $\mathbf{m}_i$ are aligned to scene points by finding the rotation $R$ and translation $\mathbf{t}$ that bring each model point close to its nearest scene point, the same idea as ICP:

$$\min_{R,\,\mathbf{t}}\ \sum_i \min_j \big\lVert R\,\mathbf{m}_i + \mathbf{t} - \mathbf{s}_j \big\rVert^2 \qquad T_{\text{grasp}}^{\text{world}} = T_{\text{object}}^{\text{world}}\; T_{\text{grasp}}^{\text{object}}$$

This page works in 2D, so the pose has three numbers. In 3D it has six, which is where the name comes from.

What to notice

From a good starting guess, the model locks onto the part in a few iterations and the grasp lands on its handle. From a poor guess, or with lots of clutter, it can lock onto the wrong spot, a local minimum. That is why real systems first make a rough guess with a learned detector, or with feature matching, and then refine with this kind of alignment. Methods like FoundationPose combine learned networks with exactly this refine step.