PnP Pose from Correspondences

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'

Example:

Constraints:

Test Results