Triangulation of 3D Points

mediumPROBundle Adjustment for SfMSfMCV: Structure from Motion and SLAM

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$ .

Here are the steps to follow:

For each point pair, build a $4 \times 4$ matrix $A$ from the camera matrices and the 2D points (below), where $P^k$ is the $k$ -th row of $P$ .
Apply Singular Value Decomposition (SVD) to $A$ to find the homogeneous point $\tilde X = (X, Y, Z, W)$ with $A\tilde X \approx 0$ .
Dehomogenize: divide by $W$ to get $(X/W,\ Y/W,\ Z/W)$ .

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}

Return the 3D points as a NumPy array of shape $(N, 3)$ , one row per point pair.

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

Inputs: $P_1, P_2$ are $3 \times 4$ projection matrices; pts1 and pts2 hold $N$ matching image points $(u, v)$ , one per row. Take $P_1 = [I \mid 0]$ , $P_2 = [I \mid (-1, 0, 0)^T]$ , with $x_1 = (0.2, 0.4)$ and $x_2 = (0, 0.4)$ .
Rows of $A$ : $0.2\,P_1^3 - P_1^1 = (-1, 0, 0.2, 0)$ , $0.4\,P_1^3 - P_1^2 = (0, -1, 0.4, 0)$ , $0 \cdot P_2^3 - P_2^1 = (-1, 0, 0, 1)$ , $0.4\,P_2^3 - P_2^2 = (0, -1, 0.4, 0)$ .
Null space: a homogeneous point $\tilde X = (X, Y, Z, W)$ projecting onto both pixels satisfies $A\tilde X = 0$ ; here that unit vector is $(0.1796, 0.3592, 0.8980, 0.1796)$ . With noisy pixels, take the unit $\tilde X$ minimising $\lVert A\tilde X \rVert$ .
Dehomogenise: dividing by $W = 0.1796$ gives $(1, 2, 5)$ . Check: it reprojects to $(0.2, 0.4)$ and $(0, 0.4)$ .
Output: an $N \times 3$ array, here one row $(1, 2, 5)$ . Notice depth comes from disparity: a shift of $0.2$ over a baseline of $1$ gives $Z = 1/0.2 = 5$ . Identical pixels in both views mean parallel rays and $W \to 0$ : a point at infinity.

Constraints

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

Learn this in

3D Gaussian Splatting3D Computer Vision Multi-View Geometry3D Computer Vision 3D Reconstruction Pipelines3D Computer Vision

More from CV: Structure from Motion and SLAM

Map Point Visibility Testhard PnP Pose from Correspondencesmedium Checkerboard Corner Detectionhard Range-Bearing Observationmedium Odometry Pose Updateeasy

Concept primerPREVIEW

Background Knowledge

In Structure from Motion (SfM), triangulation is the process of recovering a 3D point $X$ from its 2D projections $x_1, x_2$ in different camera views, given known camera projection matrices $P_1, P_2$ . Each camera matrix $P \in \mathbb{R}^{3\times 4}$ maps a 3D homogeneous point $X = (X,Y,Z,1)^T$ to an image point $x \sim P X$ , where $x = (u,v,1)^T$ . Because of noise, the back-projected rays from different cameras usually do not intersect exactly; triangulation finds the 3D point that best “fits” the observations under some error measure.

Linear triangulation (DLT) is a standard linear method that avoids solving nonlinear optimization directly. It uses the fact that $x \sim PX$ implies $x \times (PX) = 0$ , giving a homogeneous linear system $A X = 0$ built from the image coordinates and rows of $P_1, P_2$ . The desired 3D point $X$ is then the right singular vector of $A$ corresponding to the smallest singular value (from SVD), i.e., the solution that minimizes $\|A X\|$ subject to $\|X\|=1$ .

Algorithm / General Approach

High-level pattern for linear triangulation from two views:

Use the camera matrices $P_1, P_2$ and image points $x_1 = (u_1, v_1)$ , $x_2 = (u_2, v_2)$ .
For each view, derive two linear equations in the unknown 3D homogeneous point $X$ from $x \sim P X$ .
Stack the four equations into a $4 \times 4$ matrix $A$ .
Solve $A X = 0$ in the least-squares sense using SVD; take the last column of $V$ (from SVD of $A$ ).
De-homogenize $X$ to get Euclidean coordinates $(X/W, Y/W, Z/W)$ .

This is applied independently to each point pair in a batch of correspondences.

Step-by-Step Strategy

Assume:

$P_1, P_2$ are $3 \times 4$ arrays.
$x_1[i] = (u_1, v_1)$ , $x_2[i] = (u_2, v_2)$ are matched points.

Let $P_k^j$ denote the $j$ -th row of $P_k$ .

Extract rows of each camera:

$P_1^1, P_1^2, P_1^3$ from $P_1$
$P_2^1, P_2^2, P_2^3$ from $P_2$

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.

Example

Constraints

Learn this in

More from CV: Structure from Motion and SLAM

Continue the full explanation

Editor locked