Skip to article frontmatterSkip to article content

EssentialMatrixFactor Variants

This header defines several factors related to the Essential matrix (EE), which encodes the relative rotation and translation direction between two calibrated cameras. They are primarily used in Structure from Motion (SfM) problems where point correspondences between views are available but the 3D structure and/or camera poses/calibration are unknown.

The core constraint is the epipolar constraint: p2TEp1=0p_2^T E p_1 = 0, where p1p_1 and p2p_2 are corresponding points in normalized (calibrated) image coordinates.

Factors defined here:

  • EssentialMatrixFactor: Constrains an unknown EssentialMatrix variable using a single point correspondence (p1,p2)(p_1, p_2).
  • EssentialMatrixFactor2: Constrains an EssentialMatrix and an unknown inverse depth variable using a point correspondence.
  • EssentialMatrixFactor3: Like Factor2, but incorporates an additional fixed extrinsic rotation (useful for camera rigs).
  • EssentialMatrixFactor4<CALIBRATION>: Constrains an EssentialMatrix and a shared CALIBRATION variable using a point correspondence given in pixel coordinates.
  • EssentialMatrixFactor5<CALIBRATION>: Constrains an EssentialMatrix and two unknown CALIBRATION variables (one for each camera) using a point correspondence given in pixel coordinates.

Open In Colab

import gtsam
import numpy as np
from gtsam import (EssentialMatrix, Point2, Point3, Rot3, Unit3, Pose3, Cal3_S2, EssentialMatrixFactor,
                   EssentialMatrixFactor2, EssentialMatrixFactor3, EssentialMatrixFactor4Cal3_S2,
                   EssentialMatrixFactor5Cal3_S2, Values)
from gtsam import symbol_shorthand

E = symbol_shorthand.E
K = symbol_shorthand.K
D = symbol_shorthand.D # For inverse depth

1. EssentialMatrixFactor

This factor involves a single EssentialMatrix variable. It takes a pair of corresponding points (pA,pB)(p_A, p_B) in normalized (calibrated) image coordinates and penalizes deviations from the epipolar constraint pBTEpA=0p_B^T E p_A = 0. The error is pBTEpAp_B^T E p_A.

# Assume normalized coordinates
pA_calibrated = Point2(0.5, 0.2)
pB_calibrated = Point2(0.4, 0.25)

# Noise model on the epipolar error (scalar)
epipolar_noise = gtsam.noiseModel.Isotropic.Sigma(1, 0.01)

# Key for the unknown Essential Matrix
keyE = E(0)

factor1 = EssentialMatrixFactor(keyE, pA_calibrated, pB_calibrated, epipolar_noise)
factor1.print("Factor 1: ")

# Evaluate error (requires an EssentialMatrix value)
values = Values()
# Example: E for identity rotation, translation (1,0,0)
example_E = EssentialMatrix(Rot3(), Unit3(1, 0, 0))
values.insert(keyE, example_E)
error1 = factor1.error(values)
print(f"\nError for Factor 1: {error1}")
Factor 1:   keys = { e0 }
isotropic dim=1 sigma=0.01
  EssentialMatrixFactor with measurements
  (0.5 0.2   1)' and ( 0.4 0.25    1)'

Error for Factor 1: 12.499999999999995

2. EssentialMatrixFactor2

This factor involves an EssentialMatrix variable and a double variable representing the inverse depth of the 3D point corresponding to the measurement pAp_A in the first camera’s frame. It assumes the measurement pBp_B is perfect and calculates the reprojection error of the point (reconstructed using pAp_A and the inverse depth) in the first camera image, after projecting it into the second camera and back. It requires point correspondences (pA,pB)(p_A, p_B), which can be provided in either calibrated or pixel coordinates (if a calibration object K is provided). The error is a 2D reprojection error in the first image plane (typically in pixels if K is provided).

# Assume pixel coordinates and known calibration
K_cal = Cal3_S2(500, 500, 0, 320, 240)
pA_pixels = Point2(480, 288) # Corresponds to (0.5, 0.2) calibrated
pB_pixels = Point2(464, 312) # Corresponds to (0.4, 0.25) calibrated

# Noise model on the 2D reprojection error (pixels)
reprojection_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)

# Key for inverse depth
keyD = D(0)

factor2 = EssentialMatrixFactor2(keyE, keyD, pA_pixels, pB_pixels, reprojection_noise)
factor2.print("\nFactor 2: ")

# Evaluate error (requires E and inverse depth d)
values.insert(keyD, 0.2) # Assume inverse depth d = 1/Z = 1/5 = 0.2
error2 = factor2.error(values)
print(f"\nError for Factor 2: {error2}")

Factor 2:   keys = { e0 d0 }
  noise model: unit (2) 
  EssentialMatrixFactor2 with measurements
  (480 288   1)' and (464 312)'

Error for Factor 2: 412.82000000000016

3. EssentialMatrixFactor3

This is identical to EssentialMatrixFactor2 but includes an additional fixed Rot3 representing the rotation from the ‘body’ frame (where the Essential matrix is defined) to the ‘camera’ frame (where the measurements are made). iRc: Rotation from camera frame to body frame (inverse of body-to-camera). The Essential matrix EbodyE_{body} is transformed to the camera frame before use: Ecamera=RcRb⋅EbodyE_{camera} = R_{cRb} \cdot E_{body}.

body_R_cam = Rot3.Yaw(0.05) # Example fixed rotation

factor3 = EssentialMatrixFactor3(keyE, keyD, pA_pixels, pB_pixels, body_R_cam, reprojection_noise)
factor3.print("\nFactor 3: ")

# Evaluate error (uses same E and d from values)
error3 = factor3.error(values)
print(f"\nError for Factor 3: {error3}")

Factor 3:   keys = { e0 d0 }
  noise model: unit (2) 
  EssentialMatrixFactor2 with measurements
  (480 288   1)' and (464 312)'
  EssentialMatrixFactor3 with rotation [
	0.99875, -0.0499792, 0;
	0.0499792, 0.99875, 0;
	0, 0, 1
]

Error for Factor 3: 413.0638991792357

4. EssentialMatrixFactor4<CALIBRATION>

This factor involves an EssentialMatrix variable and a single unknown CALIBRATION variable (e.g., Cal3_S2) that is assumed to be shared by both cameras. It takes point correspondences (pA,pB)(p_A, p_B) in pixel coordinates. The error is the algebraic epipolar error (K−1pB)TE(K−1pA) (K^{-1} p_B)^T E (K^{-1} p_A) .

Note: Recovering calibration from 2D correspondences alone is often ill-posed. This factor typically requires strong priors on the calibration.

# Key for the unknown shared Calibration
keyK = K(0)

factor4 = EssentialMatrixFactor4Cal3_S2(keyE, keyK, pA_pixels, pB_pixels, epipolar_noise)
factor4.print("\nFactor 4: ")

# Evaluate error (requires E and K)
values.insert(keyK, K_cal) # Use the known K for this example
error4 = factor4.error(values)
print(f"\nError for Factor 4: {error4}")

Factor 4:   keys = { e0 k0 }
isotropic dim=1 sigma=0.01
  EssentialMatrixFactor4 with measurements
  (480 288)' and (464 312)'

Error for Factor 4: 11.520000000000007

5. EssentialMatrixFactor5<CALIBRATION>

Similar to Factor4, but allows for two different unknown CALIBRATION variables, one for each camera (KAK_A and KBK_B). It takes point correspondences (pA,pB)(p_A, p_B) in pixel coordinates. The error is the algebraic epipolar error (KB−1pB)TE(KA−1pA) (K_B^{-1} p_B)^T E (K_A^{-1} p_A) .

Note: Like Factor4, this is often ill-posed without strong priors.

# Keys for potentially different calibrations
keyKA = K(0) # Can reuse keyK if they are actually the same
keyKB = K(1)

factor5 = EssentialMatrixFactor5Cal3_S2(keyE, keyKA, keyKB, pA_pixels, pB_pixels, epipolar_noise)
factor5.print("\nFactor 5: ")

# Evaluate error (requires E, KA, KB)
# values already contains E(0) and K(0)
values.insert(keyKB, K_cal) # Assume KB is also the same known K
error5 = factor5.error(values)
print(f"\nError for Factor 5: {error5}")

Factor 5:   keys = { e0 k0 k1 }
isotropic dim=1 sigma=0.01
  EssentialMatrixFactor5 with measurements
  (480 288)' and (464 312)'

Error for Factor 5: 11.520000000000007
GTSAM Docs
EssentialMatrixConstraint
GTSAM Docs
Frobenius Norm Factors