PnP Pose from Correspondences

mediumPROPose EstimationPoseCV: Structure from Motion and SLAM

Implement Perspective-n-Point pose estimation using Direct Linear Transform to estimate camera pose (R, t) from 2D-3D correspondences. The goal is to find the rotation R and translation t that align 3D points with their 2D projections.

The Perspective-n-Point problem is a fundamental challenge in Computer Vision, where the relationship between 3D points and their 2D projections is described by the pinhole camera model: $s \begin{pmatrix} u \\ v \\ 1 \end{pmatrix} = K [R | t] \begin{pmatrix} X \\ Y \\ Z \\ 1 \end{pmatrix}$ . To solve this, we follow these steps:

Build a linear system from the given correspondences
Solve for the projection matrix P = K[R|t]
Decompose P to extract R and t.

\begin{pmatrix} u \\ v \\ 1 \end{pmatrix} = K [R | t] \begin{pmatrix} X \\ Y \\ Z \\ 1 \end{pmatrix}

This technique is widely used in Structure from Motion and SLAM applications.

Example

Input

points_3d = [[0,0,0], [1,0,0], [0,1,0], ...]
points_2d = [[320,240], [420,240], [320,140], ...]
K = [[500,0,320], [0,500,240], [0,0,1]]

Output

{'R': rotation_matrix, 't': translation_vector}

Reasoning

DLT builds 2N×12 matrix A from correspondences. SVD gives P in null space. P = K[R|t] → [R|t] = K^-1 @ P Extract R (orthogonalize via SVD) and t.

Constraints

points_3d: World points (N, 3)
points_2d: Image points (N, 2)
K: Camera intrinsic matrix (3, 3)
Return: Dict with 'R' and 't'

Learn this in

Structure from Motion and SLAMCV Study Plan Visual SLAM & Localization3D Computer Vision Camera Models & Calibration3D Computer Vision Multi-View Geometry3D Computer Vision

More from CV: Structure from Motion and SLAM

Map Point Visibility Testhard Triangulation of 3D Pointsmedium Checkerboard Corner Detectionhard Range-Bearing Observationmedium Odometry Pose Updateeasy

Concept primerPREVIEW

Background Knowledge

In PnP, you know a set of 3D points in world coordinates $(X_i, Y_i, Z_i)$ and their 2D projections in the image $(u_i, v_i)$ , plus the camera intrinsics $K$ . You want the camera pose: rotation $R$ and translation $t$ such that the pinhole camera model

s \begin{pmatrix} u \\ v \\ 1 \end{pmatrix} = K [R \mid t] \begin{pmatrix} X \\ Y \\ Z \\ 1 \end{pmatrix}

holds for all correspondences. The matrix $P = K[R|t]$ is a $3\times 4$ projection matrix that maps homogeneous 3D points to homogeneous image points.

The Direct Linear Transform (DLT) is a generic way to estimate $P$ from many linear equations of the form $x \sim P X$ . Each point correspondence gives you two independent linear constraints on the entries of $P$ . Once you estimate $P$ in a least-squares sense, you factor out $K$ to get the extrinsics $[R|t]$ , then project this block back to a valid rotation (orthonormal with determinant $+1$ ) and extract $t$ .

Algorithm / Approach

High-level pattern:

Use each 2D–3D correspondence to derive linear equations in the 12 unknown entries of $P$ .
Stack all equations into a big matrix system $A p = 0$ , where $p$ is a 12D vector of the entries of $P$ .
Solve for $p$ as the right singular vector of $A$ corresponding to the smallest singular value (SVD).
Reshape $p$ into $P$ , then decompose $P$ into intrinsics and extrinsics:
If $K$ is known: compute $M = K^{-1}P$ and split $M = [R' \mid t']$ .
Orthonormalize $R'$ to get a valid $R$ , adjust scale, and then get $t$ .

Step-by-Step Strategy
Normalize data (optional but recommended)

Optionally normalize 2D points to have mean ~0 and average distance ~ $\sqrt{2}$ .
Optionally normalize 3D points to have mean ~0 and average distance ~ $\sqrt{3}$ .
This improves numerical stability; you’ll need to undo this normalization at the end if you apply it to both 2D and 3D.

Set up equations per correspondence For a 3D point $X_i = (X_i, Y_i, Z_i, 1)^T$ and image point $x_i = (u_i, v_i, 1)^T$ , with $x_i \sim P X_i$ ,

\begin{pmatrix} u_i \\ v_i \\ 1 \end{pmatrix} \times (P X_i) = 0.

This cross-product yields 3 equations, but only 2 are independent; usually one discards the third. The common DLT equation form (using rows of $P$ : $p_1^T, p_2^T, p_3^T$ ) is:

\begin{aligned} u_i (p_3^T X_i) - (p_1^T X_i) &= 0 \\ v_i (p_3^T X_i) - (p_2^T X_i) &= 0. \end{aligned}

Each gives a row of the matrix $A$ in terms of the 12 unknowns of $P$ .

Build the linear system

For each correspondence, append two rows to $A$ using the above equations.
You need at least 6 correspondences (12 equations) to solve for 11 degrees of freedom (scale ambiguity), but in practice use more (overdetermined system).

PRO

Continue the full explanation

You're reading the free preview. Unlock the complete walkthrough, the code editor, test runner and reference solution with Premium.

PRO

Editor locked

The code editor is locked for Pro problems. It is only available for free problems. Please upgrade to gain access to the code editor for all problems.

solution.py

Python 3.13

Test results