EssentialMatrixConstraint

EssentialMatrixConstraint is a binary factor connecting two Pose3 variables. It represents a constraint derived from a measured Essential matrix ( $E$ ) between the two camera views corresponding to the poses. The Essential matrix $E$ encapsulates the relative rotation $R$ and translation direction $t$ between two calibrated cameras according to the epipolar constraint:

p_2^T E p_1 = 0

(1)

where $p_1$ and $p_2$ are the homogeneous coordinates of corresponding points in the normalized (calibrated) image planes.

This factor takes the measured $E_{12}$ (from pose 1 to pose 2) and compares it to the Essential matrix predicted from the estimated poses $P_1$ and $P_2$ . The error is computed in the 5-dimensional tangent space of the Essential matrix manifold.

Note: This factor requires known camera calibration, as the Essential matrix operates on normalized image coordinates.

import gtsam
import numpy as np
from gtsam import EssentialMatrixConstraint, EssentialMatrix, Pose3, Rot3, Point3, Values
from gtsam import symbol_shorthand

X = symbol_shorthand.X

Creating an EssentialMatrixConstraint¶

To create the factor, provide:

Keys for the two Pose3 variables (e.g., X(1), X(2)).
The measured gtsam.EssentialMatrix ( $E_{12}$ ).
A 5-dimensional noise model (gtsam.noiseModel).

# Assume we have two poses
pose1 = Pose3(Rot3.Yaw(0.0), Point3(0, 0, 0))
pose2 = Pose3(Rot3.Yaw(0.1), Point3(1, 0, 0))

# Calculate the ground truth Essential matrix between them
gt_E12 = EssentialMatrix.FromPose3(pose1.between(pose2))

# Add some noise (conceptual, E lives on a manifold)
# In practice, E would be estimated from image correspondences
measured_E = gt_E12 # Use ground truth for this example

# Define a noise model (5 dimensional!)
noise_dim = 5
E_noise = gtsam.noiseModel.Isotropic.Sigma(noise_dim, 0.01)

# Create the factor
key1 = X(1)
key2 = X(2)
factor = EssentialMatrixConstraint(key1, key2, measured_E, E_noise)

factor.print("EssentialMatrixConstraint: ")

EssentialMatrixConstraint: EssentialMatrixConstraint(x1,x2)
  measured: R:
 [
	0.995004, -0.0998334, 0;
	0.0998334, 0.995004, 0;
	0, 0, 1
]
d: :1
0
0
isotropic dim=5 sigma=0.01

Evaluating the Error¶

The .error(values) method computes the error vector in the 5D tangent space of the Essential matrix manifold. The error represents the difference between the measured E and the E predicted from the current pose estimates in values.

values = Values()
# Insert values close to ground truth
values.insert(key1, pose1)
values.insert(key2, pose2)

error_vector = factor.error(values)
print(f"Error vector at ground truth: {error_vector}")

# Insert values slightly different
noisy_pose2 = Pose3(Rot3.Yaw(0.11), Point3(1.05, 0.01, -0.01))
values.update(key2, noisy_pose2)

error_vector_noisy = factor.error(values)
print(f"Error vector at noisy pose: {error_vector_noisy}")

Error vector at ground truth: 0.0
Error vector at noisy pose: 1.4069198000486227

GTSAM Docs

Dataset Utilities

GTSAM Docs

EssentialMatrixFactor Variants