Extrinsics & Pose Estimation: The PnP Problem

"Show me four landmarks I recognize and I will tell you exactly where I am standing. Show me three and I will give you four equally confident answers, one of which has me inside the wall."

A P3P Solver With Multiple Personalities

The calibration of Section 12.3 ended with a quiet bonus: alongside $K$ and distortion, it returned a rotation and translation for every board view. Those were extrinsics, computed as a byproduct. This section makes them the main event, because the question "where is the camera?" is asked far more often than "what is the camera?": intrinsics change when the lens does, extrinsics change every time anything moves. The tool that answers it is the Perspective-n-Point (PnP) problem, and its inputs come from sources you have already built: known 3D geometry (a calibration board, a CAD model, a map of triangulated landmarks) matched to pixels by the detection and matching machinery of Chapter 10. The illustration below shows the idea in miniature: a camera fixing its own position from a few landmarks it recognizes.

1. Extrinsics: The Camera's Place in the World Basic

The extrinsic parameters are a rigid transform from world coordinates to camera coordinates:

$$\mathbf{X}_{\text{cam}} = R\, \mathbf{X}_{\text{world}} + \mathbf{t},$$

where $R$ is a $3 \times 3$ rotation matrix and $\mathbf{t}$ a translation vector, together six degrees of freedom (DOF: three of rotation, three of translation), the same rigid transforms you manipulated in 2D in Chapter 5, promoted to 3D. Composing with the intrinsics gives the full projection $P = K[R\,|\,t]$ from Section 12.1. One subtlety repays memorizing: $\mathbf{t}$ is not the camera's position. The transform maps world points into the camera frame, so the camera center $C$ (the point that maps to the origin) is

$$C = -R^\top \mathbf{t}.$$

Forgetting this and plotting $\mathbf{t}$ as the camera position is the single most common bug in homemade pose visualizers; the camera traces a mirror-warped path and everyone blames the solver. Figure 12.4.1 lays out the two frames and the transform between them.

In code, rotations travel as Rodrigues vectors: a 3-vector whose direction is the rotation axis and whose length is the angle in radians. Picture spinning a globe: you grab it along one axis and twist by some angle, and those two facts (which way the axis points, how far you turned) are exactly the direction and length of the vector. A vector $[0, 0, \pi/2]$ therefore means "turn 90 degrees about the $Z$ axis," and the zero vector $[0, 0, 0]$ means no rotation at all. This is the rvec in every OpenCV pose function, compact (3 parameters for 3 DOF, no orthogonality constraints to maintain during optimization) and convertible to and from a matrix with cv2.Rodrigues.

2. The Perspective-n-Point Problem Intermediate

PnP is the formal statement of "where am I, given landmarks?": given $n$ world points $\mathbf{P}_i$, their observed pixels $\mathbf{p}_i$, and the intrinsics $K$, find $R, \mathbf{t}$ minimizing the reprojection error

$$E(R, \mathbf{t}) = \sum_{i=1}^{n} \left\lVert \mathbf{p}_i - \pi(K, R, \mathbf{t}, \mathbf{P}_i) \right\rVert^2 .$$

The solver landscape has three tiers, and OpenCV's flags argument selects among them:

• Minimal: P3P. Three points yield a quartic with up to four geometrically valid poses; a fourth point picks the winner. P3P matters not for everyday use but inside RANSAC, where you want to hypothesize poses from the smallest possible sample (SOLVEPNP_P3P, SOLVEPNP_AP3P).
• Closed-form for many points: EPnP. Expresses the $n$ points as combinations of four control points and solves linearly in $O(n)$, fast and accurate for $n \gtrsim 6$ (SOLVEPNP_EPNP). Four is not arbitrary: any 3D point has unique barycentric coordinates with respect to four non-coplanar reference points, so EPnP reduces estimating $n$ unknown camera-frame positions to estimating just the four control points, which is why the cost stays linear in $n$ no matter how many correspondences you feed it.
• Iterative refinement. Levenberg-Marquardt (the nonlinear least-squares solver introduced in Section 12.3) on the reprojection error from an initial guess, the most accurate finish and the default (SOLVEPNP_ITERATIVE); planar targets get dedicated solvers (SOLVEPNP_IPPE, SOLVEPNP_IPPE_SQUARE) that exploit the flat geometry.

A useful mental model: the calibrated camera converts each observed pixel into a ray (the back-projection of Section 12.1), so PnP is the problem of rigidly moving a known constellation of 3D points until each point impales its own ray. Three rays almost pin the constellation; more rays overdetermine it and average out the pixel noise.

3. solvePnP in Practice Basic

The code below estimates the pose of a checkerboard from a single image, using the $K$ and distortion calibrated in Section 12.3. It then converts the Rodrigues vector to a matrix, computes the camera center with the formula from Subsection 1, and draws the world axes into the image, the canonical "is my pose right?" sanity check: the axes should sit on the board's corner and point along its edges.

# Estimate a calibrated board's pose from one image with solvePnP, then run
# the two standard conversions: Rodrigues vector to rotation matrix, and
# C = -R^T t for the camera center. drawFrameAxes gives the visual sanity check.
import cv2
import numpy as np

# K, dist: from Section 12.3's calibration. objp: board corner coordinates.
img = cv2.imread("board_pose.jpg")
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ok, corners = cv2.findChessboardCornersSB(gray, (9, 6),
                                          flags=cv2.CALIB_CB_EXHAUSTIVE)

ok, rvec, tvec = cv2.solvePnP(objp, corners, K, dist)   # default: ITERATIVE

R, _ = cv2.Rodrigues(rvec)              # 3-vector -> 3x3 rotation matrix
C = -R.T @ tvec                         # camera center in WORLD coordinates
angle = np.degrees(np.linalg.norm(rvec))

print(f"distance to board origin: {np.linalg.norm(tvec):.3f} m")
print(f"rotation angle: {angle:.1f} deg about axis {np.round(rvec.ravel()/np.linalg.norm(rvec), 2)}")
print(f"camera center in board frame: {np.round(C.ravel(), 3)} m")
# distance to board origin: 0.428 m
# rotation angle: 28.4 deg about axis [-0.91  0.4   0.1 ]
# camera center in board frame: [ 0.083 -0.144  0.392] m

cv2.drawFrameAxes(img, K, dist, rvec, tvec, length=3 * 0.024)   # 3 squares long
cv2.imwrite("board_axes.jpg", img)

4. When Correspondences Lie: PnP Meets RANSAC Intermediate

A checkerboard's corners come with guaranteed identities, but most real correspondences come from the feature matching of Chapter 10, and some fraction of matches are simply wrong. Least-squares solvers average over all input, so a single gross outlier can drag the pose meters off target. The remedy is the same RANSAC hypothesize-and-verify loop that chapter used for homographies: sample a minimal set (here, P3P's three or four points), solve, count how many other correspondences reproject within a pixel threshold, repeat, keep the consensus winner, and refine on its inliers. The demonstration below manufactures the failure and the rescue with synthetic data, so every quantity is known exactly.

# Manufacture a PnP problem with known ground truth, corrupt a quarter of the
# correspondences, then compare plain solvePnP against solvePnPRansac. RANSAC's
# hypothesize-and-verify loop is what survives the gross outliers.
import cv2
import numpy as np

rng = np.random.default_rng(7)
K = np.array([[1480., 0., 960.], [0., 1480., 540.], [0., 0., 1.]])

# Ground-truth pose and a cloud of 120 world points, 4 to 8 m ahead.
rvec_true = np.array([0.10, -0.30, 0.05])
tvec_true = np.array([0.20, -0.10, 0.50])
pts3d = rng.uniform([-1, -1, 4], [1, 1, 8], (120, 3)).astype(np.float32)

img_pts, _ = cv2.projectPoints(pts3d, rvec_true, tvec_true, K, None)
img_pts = img_pts.reshape(-1, 2) + rng.normal(0, 0.5, (120, 2))   # pixel noise

bad = rng.choice(120, 30, replace=False)                  # 25% wrong matches
img_pts[bad] = rng.uniform([0, 0], [1920, 1080], (30, 2))

ok, rvec_ls, tvec_ls = cv2.solvePnP(pts3d, img_pts, K, None)
ok, rvec_rs, tvec_rs, inliers = cv2.solvePnPRansac(
    pts3d, img_pts, K, None, reprojectionError=2.0, iterationsCount=300)

err_ls = np.linalg.norm(tvec_ls.ravel() - tvec_true)
err_rs = np.linalg.norm(tvec_rs.ravel() - tvec_true)
print(f"plain solvePnP   translation error: {err_ls:.3f} m")
print(f"solvePnPRansac   translation error: {err_rs:.3f} m  ({len(inliers)} inliers)")
# plain solvePnP   translation error: 1.176 m
# solvePnPRansac   translation error: 0.004 m  (90 inliers)

Two parameters do most of the work. reprojectionError (the inlier threshold, in pixels) should be 2 to 8 px: too tight rejects honest points under noise, too loose admits near-outliers that bias the refit. iterationsCount buys confidence against high outlier rates; with 25% contamination, 300 iterations of 4-point samples make a clean sample overwhelmingly likely. After RANSAC, a final cv2.solvePnPRefineLM on the inliers polishes the answer; production trackers do all three steps every frame.

5. Markers: ArUco, IPPE & the Planar Flip Intermediate

When you control the environment, you can skip natural features entirely and install fiducial markers: high-contrast binary squares whose corners are detectable in one pass and whose IDs are encoded in the pattern. The encoding is concrete: the inner grid of black and white cells reads as a string of bits (white = 1, black = 0), so a marker is literally a small barcode that says both "I am a marker" and "I am marker number 23", with redundant bits for error detection. OpenCV's ArUco module (a contrib citizen graduated into the main library; the object-oriented ArucoDetector API is current since 4.7) detects markers and identifies them; pose then comes from solvePnP on the four corners with the purpose-built SOLVEPNP_IPPE_SQUARE solver, which replaced the deprecated estimatePoseSingleMarkers helper.

# Detect ArUco markers with the modern object-oriented ArucoDetector, then
# recover each marker's 6-DOF pose from its four corners with the planar
# SOLVEPNP_IPPE_SQUARE solver. The object-point order is part of the contract.
import cv2
import numpy as np

aruco = cv2.aruco
detector = aruco.ArucoDetector(aruco.getPredefinedDictionary(aruco.DICT_4X4_50),
                               aruco.DetectorParameters())

frame = cv2.imread("workbench.jpg")
corners, ids, _ = detector.detectMarkers(frame)

MARKER = 0.05                     # printed marker side, meters
h = MARKER / 2                    # IPPE_SQUARE requires EXACTLY this corner order:
obj = np.array([[-h,  h, 0], [ h,  h, 0],          # top-left, top-right,
                [ h, -h, 0], [-h, -h, 0]], np.float32)   # bottom-right, bottom-left

for c, marker_id in zip(corners, ids.ravel()):
    ok, rvec, tvec = cv2.solvePnP(obj, c.reshape(-1, 2), K, dist,
                                  flags=cv2.SOLVEPNP_IPPE_SQUARE)
    cv2.drawFrameAxes(frame, K, dist, rvec, tvec, MARKER * 0.75)
    print(f"marker {marker_id}: {np.linalg.norm(tvec):.3f} m from camera")
# marker  7: 0.612 m from camera
# marker 23: 0.841 m from camera

Planar targets carry a geometric trap worth knowing by name: the planar pose ambiguity. Viewed from far away or nearly head-on, a small flat square projects almost identically under two different tilts (mirror images about the viewing direction), and noise decides which one the solver returns, so the estimated orientation can flip frame to frame even while the corners are tracked perfectly. SOLVEPNP_IPPE_SQUARE returns the better-fitting solution, but when the two reprojection errors are close, only physics can save you: use bigger markers, mount several on a rigid board (aruco.GridBoard), or fuse over time. The story below is what the flip looks like in production.

# Production pose in two calls: RANSAC finds the inlier set, then a final
# Levenberg-Marquardt refinement polishes the pose on those inliers only.
ok, rvec, tvec, inliers = cv2.solvePnPRansac(pts3d, pts2d, K, dist,
                                             reprojectionError=3.0)
rvec, tvec = cv2.solvePnPRefineLM(pts3d[inliers], pts2d[inliers],
                                  K, dist, rvec, tvec)