EssentialMatrixConstraint

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

X = symbol_shorthand.X

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.

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