Triangulation of 3D Points

Implement a linear triangulation method to estimate 3D points from two camera views. This technique is crucial in Structure from Motion (SfM) and Bundle Adjustment for reconstructing 3D scenes from 2D images.

The goal is to find the 3D point $X$ that minimizes the reprojection error between the observed 2D points $x_1$ and $x_2$ in two camera views, given the camera matrices $P_1$ and $P_2$ . The ray direction for each 2D point $(u, v)$ can be computed as $P^{-1}(u,v,1)^T$ .

Here are the steps to follow:

Compute the ray directions for each 2D point.
Build a matrix $A$ using the camera matrices and 2D points.
Apply Singular Value Decomposition (SVD) to matrix $A$ .

A = \begin{bmatrix} u_1 P_1^3 - P_1^1 \\ v_1 P_1^3 - P_1^2 \\ u_2 P_2^3 - P_2^1 \\ v_2 P_2^3 - P_2^2 \end{bmatrix}

This technique is widely used in computer vision applications, such as 3D reconstruction and Simultaneous Localization and Mapping (SLAM).

Example:

Input:

P1 = K @ [I | 0]  # First camera at origin
P2 = K @ [R | t]  # Second camera
pts1 = [[u1, v1], ...]
pts2 = [[u2, v2], ...]

Output:

3D points [[X, Y, Z], ...]

Reasoning:

Each 2D point gives 2 equations. 4 equations from 2 views for each point. SVD of A gives X in null space.

Constraints:

P1, P2: Camera projection matrices (3, 4)
pts1, pts2: Corresponding 2D points (N, 2)
Return: 3D points (N, 3)

Example:

Constraints:

Test Results