SelfCalibrationFactor

Created by Codex.

Estimate two focal lengths from a fundamental matrix and known principal points.

Authors: Frank Dellaert, et al. (see THANKS for the full author list)

See LICENSE for the license information

try:
    import google.colab
    %pip install --quiet gtsam-develop
except ImportError:
    pass

import gtsam
import numpy as np

from gtsam import symbol_shorthand

C = symbol_shorthand.C
K = symbol_shorthand.K
P = symbol_shorthand.P
S = symbol_shorthand.S
X = symbol_shorthand.X

Mathematical idea¶

For fundamental matrix $F$ and calibration matrices $K_i(f_i),K_j(f_j)$ , the induced essential matrix is

E(f_i,f_j)=K_j(f_j)^{\mathsf T}F K_i(f_i).

(1)

A valid essential matrix has singular values $(\sigma,\sigma,0)$ . The factor penalizes departure from the two equal nonzero singular-value constraints, thereby estimating the focal lengths. SVD means singular value decomposition.

Model¶

The factor implements the Fetzer focal-length constraint. Its two scalar variables are f_i and f_j; the fundamental matrix and principal points are fixed constructor data. The residual is a two-vector measuring deviation from the equal-nonzero-singular-values condition of an essential matrix.

F = np.array([[0.0, -1.0e-3, 0.1],
              [1.0e-3, 0.0, -0.2],
              [-0.1, 0.2, 0.0]])
factor = gtsam.SelfCalibrationFactor(
    K(0), K(1), F, np.array([320.0, 240.0]), np.array([320.0, 240.0])
)
print("focal keys:", factor.keys())
print("residual dimension:", factor.dim())

focal keys: [7710162562058289152, 7710162562058289153]
residual dimension: 2

Practical notes¶

Keep focal-length variables away from zero; the implementation floors tiny magnitudes for numerical safety, not as a modeling prior.
Add positive focal priors or a suitable parameterization in a larger problem.
Respect the convention x_j^T F x_i = 0; swapping image order requires transposing F and swapping the associated metadata.

Source¶

SelfCalibrationFactor.h