6D Pose Estimation

6D pose estimation reports where an object sits and how it is rotated, as 3D position plus 3D orientation for grasping and assembly.

Eight 2D to 3D correspondences solve rotation and translation to 6 mm and 3 degrees, then depth refinement tightens translation to 3 mm inside a 20 mm gripper clearance.

Why Does This Exist?

A box says the drill is in view. The gripper needs the drill handle axis within degrees and millimeters to close without collision. 6D pose estimation exists to deliver that: three numbers for translation $t$ plus three for rotation $R$ , turning a known CAD model into an actionable grasp frame. Without rotation, every symmetric or articulated tool becomes a gamble, and without millimeters the fingers hit the part instead of the handle.

This page covers the correspondence-to-pose pipeline and its refinement step. If joint-based pose is what you know, pose estimation builds the keypoint picture this page extends from people to rigid objects. The robust fitting that rejects bad matches is RANSAC, and the box that starts the whole pipeline is object detection.

Think of It Like This

Fitting a key from its photo

You hold a key photo against the lock, sliding and turning your mental model until the teeth align with the pins. Translation is the slide, rotation is the turn, and the click is the fit. The model aligns rendered model points to observed pixels the same way.

The analogy stops at keys: objects also hide half their teeth behind clutter, so the estimator must decide which teeth it can even see before turning anything.

How It Actually Works

Correspondences, then the solve

A network predicts 2D locations of known 3D model keypoints, or dense 2D-to-3D correspondences. Perspective-n-Point with RANSAC then solves for the rotation $R$ and translation $t$ that best project model points onto those observations. Here $R$ is a $3 \times 3$ rotation matrix orienting the object frame in the camera frame, and $t$ is a 3-vector giving the object origin's position in meters in that same camera frame. A refinement step such as ICP on depth, or render-and-compare on RGB, tightens the fit. Symmetric objects need special losses that accept any equivalent rotation rather than punishing correct-but-rotated answers. The projection underneath follows the pinhole camera model.

Worked example: 8 matches, 6 mm, 3 degrees

A mug 100 mm wide is viewed with 60 percent occlusion, leaving 8 reliable correspondences with 2-pixel noise. PnP plus RANSAC rejects 2 outliers, solves on the remaining 6, and lands translation within 6 mm and rotation within 3 degrees. ICP on depth pulls translation to 3 mm. That residual fits inside a 20 mm gripper clearance, so the grasp proceeds. With only 3 correspondences the solve turns unstable and error triples, which is why visibility gating precedes grasping.

Code

This snippet solves translation from one correspondence given a known rotation, the single-point version of what PnP does with many:

1import numpy as np2
3
4def translation_from_match(K: np.ndarray, R: np.ndarray, X: np.ndarray, x: np.ndarray, depth: float) -> np.ndarray:5    cam = R @ X  # model point rotated into camera orientation6    ray = np.linalg.inv(K) @ np.array([x[0], x[1], 1.0])  # pixel to unit ray7    t = depth * ray - cam  # scale ray by depth, subtract rotated point8    return np.round(t, 2)9
10
11K = np.array([[500.0, 0, 320.0], [0, 500.0, 240.0], [0, 0, 1.0]])12print(translation_from_match(K, np.eye(3), np.array([0.0, 0.0, 0.0]), np.array([320.0, 240.0]), 0.8))13# -> [0.  0.  0.8]

Here $K$ is the $3 \times 3$ camera intrinsic matrix with focal length 500 and principal point $(320, 240)$ , $R$ is the known rotation, $X$ the 3D model point, $x$ the observed pixel, and $depth$ the measured distance along the ray. Centered pixels at 0.8 m give pure forward translation. Real solves use many points plus robust rejection.

Watch Out For

Symmetry punished as error

A textureless cylinder looks identical after any spin about its axis, but naive rotation loss punishes all but one answer. The symptom is unstable training that never converges on bottles and shafts. Use symmetry-aware losses that score the closest equivalent rotation or the closest-point distance instead of raw angle.

Trusting monocular depth at face value

One RGB image fixes direction but not distance: a small mug nearby and a large mug far away project identically. The symptom is correct rotation with translation that is off by factors, not millimeters. Gate grasps on measured depth or a known-size prior, and never execute from RGB alone.

The Quick Version

6D pose is 3D position $t$ plus 3D orientation $R$ for a known object model.
Correspondences plus robust PnP give the first solve, near 6 mm and 3 degrees on 8 matches.
Depth refinement tightens translation to about 3 mm for robot grasp targets.
Occlusion needs visibility gating; with only 3 points the solve turns unstable.
Symmetry needs losses that accept equivalent rotations.
Monocular RGB alone cannot fix absolute distance, so gate on depth.