ProjectionFactor - GTSAM Docs

import gtsam
import numpy as np
from gtsam import Pose3, Point3, Point2, Rot3, Cal3_S2, Values
# The Python wrapper often creates specific instantiations
from gtsam import GenericProjectionFactorCal3_S2
from gtsam import symbol_shorthand

X = symbol_shorthand.X
L = symbol_shorthand.L

GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> is a versatile factor for monocular camera measurements. It models the reprojection error between the predicted projection of a 3D LANDMARK (usually Point3) onto the image plane of a camera defined by POSE (usually Pose3) and CALIBRATION (e.g., Cal3_S2, Cal3Bundler, Cal3DS2), and a measured 2D pixel coordinate measured_.

Key features:

Templated: Works with different pose, landmark, and calibration types.
Fixed Calibration: Assumes the CALIBRATION object (K_) is known and fixed (passed as a shared pointer).
Sensor Offset: Optionally handles a fixed body_P_sensor_ (Pose3) transform between the pose variable’s frame (body) and the camera’s sensor frame.
Cheirality Handling: Can be configured to throw an exception or return a large error if the landmark projects behind the camera.

The error is the 2D vector difference:

\text{error}(P, L) = \text{project}(P \cdot S, L) - z

(1)

where $P$ is the pose variable, $L$ is the landmark variable, $S$ is the optional body_P_sensor transform, project is the camera projection function including calibration, and $z$ is the measured_ point.

Creating a GenericProjectionFactor¶

Instantiate by providing:

The 2D measurement (Point2).
The noise model (typically 2D isotropic).
The key for the pose variable.
The key for the landmark variable.
A shared_ptr to the fixed calibration object.
(Optional) The fixed Pose3 sensor offset body_P_sensor.
(Optional) Cheirality handling flags.

measured_pt2 = Point2(330, 250)
pixel_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # 1 pixel std dev
pose_key = X(0)
landmark_key = L(1)

# Shared pointer to calibration
K = Cal3_S2(500.0, 500.0, 0.0, 320.0, 240.0)

# Optional sensor pose offset
body_P_sensor = Pose3(Rot3.Ypr(-np.pi/2, 0, -np.pi/2), Point3(0.1, 0, 0.2))

# Create factor with sensor offset
factor_with_offset = GenericProjectionFactorCal3_S2(
    measured_pt2, pixel_noise, pose_key, landmark_key, K, body_P_sensor)
factor_with_offset.print("Factor with offset: ")

# Create factor without sensor offset (implicitly identity)
factor_no_offset = GenericProjectionFactorCal3_S2(
    measured_pt2, pixel_noise, pose_key, landmark_key, K)
factor_no_offset.print("\nFactor without offset: ")

Factor with offset: GenericProjectionFactor, z = [
	330;
	250
]
  sensor pose in body frame:  R: [
	6.12323e-17, 6.12323e-17, 1;
	-1, 3.7494e-33, 6.12323e-17;
	-0, -1, 6.12323e-17
]
t: 0.1   0 0.2
  keys = { x0 l1 }
  noise model: unit (2) 

Factor without offset: GenericProjectionFactor, z = [
	330;
	250
]
  keys = { x0 l1 }
  noise model: unit (2)

Evaluating the Error¶

The error is the difference between the predicted projection and the measurement.

values = Values()

# Example values
pose = Pose3(Rot3.Rodrigues(0.1, -0.2, 0.3), Point3(1, -1, 0.5))
landmark = Point3(4.0, 2.0, 3.0)

values.insert(pose_key, pose)
values.insert(landmark_key, landmark)

# Evaluate factor without offset
error_no_offset = factor_no_offset.error(values)
print(f"Error (no offset): {error_no_offset}")

# Evaluate factor with offset
error_with_offset = factor_with_offset.error(values)
print(f"Error (with offset): {error_with_offset}")

# Manually verify projection (no offset case)
cam_no_offset = gtsam.PinholeCameraCal3_S2(pose, K)
predicted_no_offset = cam_no_offset.project(landmark)
manual_error = predicted_no_offset - measured_pt2
print(f"Manual error calculation (no offset): {manual_error}")
assert np.allclose(factor_no_offset.unwhitenedError(values), manual_error)

Error (no offset): 1175689.2145311693
Error (with offset): 50751.576015003826
Manual error calculation (no offset): [1370.63962025  687.55033305]