Solving Relative Orientation Using Collinearity Equations

Introduction

Relative orientation is the process of determining the relative rotations and translations of one image with respect to another reference image. This problem always involves a stereo pair of images. In this setup, the camera calibration parameters and a set of corresponding image points (manually identified or automatically matched) are known. The unknowns are the relative position and orientation of the second image with respect to the first. As a result, the relative orientation problem contains five degrees of freedom: the three rotational angles and two independent components of the baseline.

Geometric Constraints

In relative orientation, the first image is treated as the origin of a 3D Cartesian coordinate system—specifically, the undistorted, ideal camera coordinate system. The length of the baseline between the two projection centers is assumed to be constant or normalized.

If reasonable initial estimates of the orientation parameters are available, the collinearity equations can be used directly to solve the problem. The simple form of the equations in direct mode is:

x’ = − M₁(X − X₀) / M₃(X − X₀)
y’ = − M₂(X − X₀) / M₃(X − X₀)
z’ = −1

Where x’ and y’ are the image coordinates in the 3D coordinate system without distortion. If we consider the equations with all possible parameters, the collinearity condition takes the following form:

x'(…) · M₃(ω, φ, κ) · (X − X₀) + M₁(ω, φ, κ) · (X − X₀) = 0
y'(…) · M₃(ω, φ, κ) · (X − X₀) + M₂(ω, φ, κ) · (X − X₀) = 0

Parameters

Each collinearity equation is a nonlinear function that depends on 20 parameters. Altogether, the collinearity condition involves 21 parameters, which can be grouped as follows:

2 parameters from the image observations
10 interior orientation parameters
6 exterior orientation parameters
3 model point coordinates

Solutions

Solving the relative orientation problem can be carried out analytically using the collinearity equations in two common formulations:

Right-sided relative orientation: The rotation and translation parameters of the left image are held fixed, and only the parameters of the right image are treated as unknowns.
Left-sided relative orientation: The rotation and translation parameters of the right image are fixed, and only the orientation and position parameters of the left image are estimated.

Because the relative orientation problem contains five degrees of freedom, one of the parameters must be fixed. This can be achieved by either fixing the baseline length to a constant value, or fixing the largest component of the baseline.

When solving relative orientation using collinearity equations, it is required that initial estimates of the unknowns are available with reasonable accuracy; otherwise, the solution will not converge. To address this, each nonlinear equation is linearized using a first-order Taylor expansion, which enables the use of iterative least-squares adjustment.

Solving with Collinearity Equations

To solve the relative orientation problem using the collinearity condition, the first step is to form the Taylor series expansion of the nonlinear functions around an initial estimate of the parameters. This linearization expresses the change in the image coordinates as a function of small increments in the unknown parameters.

Once the equations are linearized, the solution is iteratively refined. At each iteration, the partial derivatives of the collinearity equations with respect to the unknown parameters are evaluated at the current estimate. These derivatives define the direction in the parameter space that moves the solution toward the optimum. By applying successive corrections, the parameter estimates converge toward values that minimize the residuals between observed and computed image coordinates.

If we assume a one-sided relative orientation where the orientation elements of the second image are estimated while the value of X₀₂ is held constant, the set of unknown parameters becomes: ΔY₀₂, ΔZ₀₂, Δω₂, Δφ₂, and Δκ₂.

These five parameters represent the two remaining components of the baseline and the three rotation angles of the second image, forming the five degrees of freedom required for relative orientation.

Experimentation Using Python
To better understand how the collinearity equations are used to solve relative orientation, we can simulate a practical case using Python.
Simulated Case Definition
In this scenario, we have a stereo pair of images taken by a calibrated camera. We want to find the relative position and orientation of the second image with respect to the first.
Interior Orientation: Known camera parameters (Focal length, Principal Point, and distortion coefficients).
Reference Frame: The first image is placed at the origin with zero rotation (X₀₁ = [0, 0, 0] and ω₁, φ₁, κ₁ = 0).
Initial Estimates: The second image has a baseline mostly along the X-axis (X₀₂ = [1, 0, 0]), and we assume small initial guesses for its rotations and remaining translations.
Observations: A set of corresponding tie points has been identified across both images, and their pixel coordinates have been converted to undistorted, normalized ideal pinhole coordinates. We also have initial 3D coordinates for these tie points obtained via basic ray intersection.

Python Implementation

import numpy as np

# Assuming nPoints represents the number of tie points
# xys_1 and xys_2 contain the normalized 2D image coordinates
# XYZs contains the initial 3D model coordinates
# Image_2_ori contains [Omega, Phi, Kappa] and Image_2_X0 contains [X, Y, Z]

for it in range(0, 100): # Maximum of 100 iterations
nEqu = nPoints * 4 # 4 equations per 3D point (2 per image)
nX = 5 + 3 * nPoints # 5 relative orientation unknowns + 3 coordinates per point

A = np.zeros((nEqu, nX)) # Jacobian matrix (Design matrix)
L = np.zeros((nEqu, 1)) # Misclosure vector (Residuals)

for i in range(0, nPoints):
# Extract current estimates
XYZ_i = XYZs[i, :]
xy_1_i = xys_1[i, :]
xy_2_i = xys_2[i, :]

# Evaluate Collinearity Functions (f1, f2) and their Partial Derivatives
# Note: f_Eval_Direct and Diffs_Eval_Direct are custom functions that evaluate
# the collinearity equations and their symbolic derivatives.
f1_vals_i = f_Eval_Direct(xy_1_i, Image_1_ori, Image_1_X0, XYZ_i, camera_params)
f2_vals_i = f_Eval_Direct(xy_2_i, Image_2_ori, Image_2_X0, XYZ_i, camera_params)

Diffs_F1_vals = Diffs_Eval_Direct(xy_1_i, Image_1_ori, Image_1_X0, XYZ_i, camera_params)
Diffs_F2_vals = Diffs_Eval_Direct(xy_2_i, Image_2_ori, Image_2_X0, XYZ_i, camera_params)

# Populate Misclosure Vector L
L[4 * i, 0] = -f1_vals_i[0] # Image 1 x-residual
L[4 * i + 1, 0] = -f1_vals_i[1] # Image 1 y-residual
L[4 * i + 2, 0] = -f2_vals_i[0] # Image 2 x-residual
L[4 * i + 3, 0] = -f2_vals_i[1] # Image 2 y-residual

# Populate Jacobian Matrix A for 3D point parameters
A[4 * i:4 * i + 2, 5 + 3 * i:5 + 3 * i + 3] = Diffs_F1_vals[:, 15:18]
A[4 * i + 2:4 * i + 4, 5 + 3 * i:5 + 3 * i + 3] = Diffs_F2_vals[:, 15:18]

# Populate Jacobian Matrix A for the 5 relative orientation unknowns (Image 2)
# Omega, Phi, Kappa
A[4 * i + 2:4 * i + 4, 0:3] = Diffs_F2_vals[:, 12:15]
# Y0, Z0 (X0 is fixed to define scale)
A[4 * i + 2:4 * i + 4, 3:5] = Diffs_F2_vals[:, 19:21]

# Solve Normal Equations: (A^T * A) * DeltaX = A^T * L
AtA = A.transpose() @ A
DeltaX = np.linalg.inv(AtA) @ A.transpose() @ L

# Check for convergence
NormDeltaX = np.linalg.norm(DeltaX, 2)
print(f”Iteration: {it}, Norm dx: {NormDeltaX:.6f}”)

if NormDeltaX < 1e-4:
print(f"Relative orientation converged at iteration: {it}")
break

# Apply Corrections to Image 2 parameters (Omega, Phi, Kappa, Y0, Z0)
Image_2_ori[2] += DeltaX[0, 0] # Update Omega
Image_2_ori[1] += DeltaX[1, 0] # Update Phi
Image_2_ori[0] += DeltaX[2, 0] # Update Kappa
Image_2_X0[1] += DeltaX[3, 0] # Update Y0
Image_2_X0[2] += DeltaX[4, 0] # Update Z0

# Apply Corrections to 3D Tie Point Coordinates
for i in range(0, nPoints):
XYZs[i, 0] += DeltaX[5 + 3 * i, 0] # Update X
XYZs[i, 1] += DeltaX[5 + 3 * i + 1, 0] # Update Y
XYZs[i, 2] += DeltaX[5 + 3 * i + 2, 0] # Update Z

Step-by-Step Code Description

Initialization: The outer loop defines the maximum number of iterations. We calculate the size of our matrices based on nPoints. We have 4 equations per point (x and y equations for both images) and 5 + 3n unknowns (5 orientation parameters for the right image, plus 3 spatial coordinates for each point).
Evaluating the Equations: Inside the inner loop, the algorithm evaluates the actual collinearity equations (f_Eval_Direct) to see how close our current geometric model is to the actual 2D coordinates observed on the photos.
Building the Matrices: * The L Vector holds the residuals (the differences between the calculated 2D coordinates and the actual observed 2D coordinates). We negate it because of the Newton iteration formula (A · dx = −F(x₀)).
- The A Matrix (Jacobian) is populated using the partial derivatives of the collinearity equations with respect to every single unknown parameter (Diffs_Eval_Direct).
Solving the Normal Equations: Outside the inner loop, we form the normal equations (AtA) and solve for the correction vector DeltaX using matrix inversion and multiplication. DeltaX tells us exactly how much to adjust our parameters to minimize the errors.
Convergence Check: We calculate the length (norm) of the DeltaX vector. If the changes are extremely small (less than 10⁻⁴), it means the system has found the optimal solution and we break out of the loop.
Applying Corrections: If it hasn’t converged yet, the corrections calculated in DeltaX are added to our current estimates for Image 2’s orientation angles (ω, φ, κ), position (Y₀, Z₀), and the 3D coordinates (X, Y, Z) of all the tie points. The loop then restarts with these refined values.