StereoFactor - GTSAM Docs

import gtsam
import numpy as np
from gtsam import Pose3, Point3, StereoPoint2, Rot3, Cal3_S2Stereo, Values
# Need StereoCamera for backprojection/triangulation
from gtsam import StereoCamera 
# The Python wrapper often creates specific instantiations
from gtsam import GenericStereoFactor3D
from gtsam import symbol_shorthand

X = symbol_shorthand.X
L = symbol_shorthand.L

GenericStereoFactor<POSE, LANDMARK> is a factor for handling measurements from a calibrated stereo camera. It relates a 3D LANDMARK (usually Point3) to a StereoPoint2 measurement observed by a stereo camera system defined by a POSE (usually Pose3) and a fixed stereo calibration Cal3_S2Stereo.

StereoPoint2 contains $(u_L, u_R, v)$ , the horizontal pixel coordinates in the left ( $u_L$ ) and right ( $u_R$ ) images, and the vertical pixel coordinate ( $v$ ), which is assumed the same for both images in a rectified stereo setup. Cal3_S2Stereo holds the intrinsic parameters (focal length, principal point) common to both cameras and the stereo baseline (distance between camera centers).

Key features:

Templated: Works with different pose and landmark types.
Fixed Calibration: Assumes the Cal3_S2Stereo object (K_) is known and fixed.
Sensor Offset: Optionally handles a fixed body_P_sensor_ (Pose3) transform.
Cheirality Handling: Can be configured for points behind the camera.

The error is the 3D vector difference:

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

(1)

where projectStereo uses the StereoCamera model, $P$ is the pose, $L$ the landmark, $S$ the optional offset, and $z$ is the measured_ StereoPoint2.

Creating a GenericStereoFactor¶

Instantiate by providing:

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

measured_stereo = StereoPoint2(330, 305, 250) # uL, uR, v
stereo_noise = gtsam.noiseModel.Isotropic.Sigma(3, 1.0) # 1 pixel std dev (ul, ur, v)
pose_key = X(0)
landmark_key = L(1)

# Shared pointer to stereo calibration
K_stereo = Cal3_S2Stereo(500.0, 500.0, 0.0, 320.0, 240.0, 0.1) # fx, fy, s, u0, v0, baseline

# 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 = GenericStereoFactor3D(
    measured_stereo, stereo_noise, pose_key, landmark_key, K_stereo, body_P_sensor=body_P_sensor)
factor_with_offset.print("Factor with offset: ")

# Create factor without sensor offset
factor_no_offset = GenericStereoFactor3D(
    measured_stereo, stereo_noise, pose_key, landmark_key, K_stereo)
factor_no_offset.print("\nFactor without offset: ")

Factor with offset:   keys = { x0 l1 }
  noise model: unit (3) 
Factor with offset: .z(330, 305, 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

Factor without offset:   keys = { x0 l1 }
  noise model: unit (3) 

Factor without offset: .z(330, 305, 250)

Evaluating the Error¶

The error is the 3D difference between the predicted stereo projection and the measurement.

values = Values()

# Example values
pose = Pose3(Rot3.Rodrigues(0.1, -0.2, 0.3), Point3(1, -1, 0.5))
values.insert(pose_key, pose)

# --- Evaluate factor without offset --- 
# Create a StereoCamera object at the current pose
camera_no_offset = StereoCamera(pose, K_stereo)
# Triangulate (backproject) the measurement to get the point in the camera frame
# Depth = fx * b / disparity = 500 * 0.1 / (330 - 305) = 50 / 25 = 2.0
expected_point_camera = camera_no_offset.backproject(measured_stereo) # Point in camera frame
# Transform the point from the camera frame to the world frame
landmark = pose.transformFrom(expected_point_camera) # Point in world frame
print(f"Expected landmark point (no offset): {landmark}")

values.insert(landmark_key, landmark)
error_no_offset = factor_no_offset.error(values)
print(f"\nError (no offset) at expected landmark: {error_no_offset} (Should be near zero)")

# --- Evaluate factor with offset --- 
# Calculate the actual sensor pose in the world
pose_with_offset = pose.compose(body_P_sensor) # world_P_sensor = world_P_body * body_P_sensor
# Create a StereoCamera object at the sensor pose
camera_with_offset = StereoCamera(pose_with_offset, K_stereo)
# Triangulate the measurement from the sensor's perspective
expected_point_offset_cam = camera_with_offset.backproject(measured_stereo) # Point in sensor frame
# Transform the point from the sensor frame to the world frame
landmark_offset = pose_with_offset.transformFrom(expected_point_offset_cam) # Point in world frame
print(f"\nExpected landmark point (offset): {landmark_offset}")

# Update the landmark value in Values for the offset factor calculation
values.update(landmark_key, landmark_offset)
error_with_offset = factor_with_offset.error(values)
print(f"Error (with offset) at recomputed landmark: {error_with_offset} (Should be near zero)")

# --- Evaluate with noisy landmark (using the no-offset factor for simplicity) ---
# Use the original landmark calculated for the no-offset case as the 'ground truth'
noisy_landmark = landmark + Point3(0.1, -0.05, 0.1) 
values.update(landmark_key, noisy_landmark)
error_no_offset_noisy = factor_no_offset.error(values)
print(f"\nError (no offset) at noisy landmark: {error_no_offset_noisy}")

Expected landmark point (no offset): [ 1.54225239 -2.27112649  2.95849821]

Error (no offset) at expected landmark: 48664.883462255115 (Should be near zero)

Expected landmark point (offset): [ 2.89128008 -3.54882535  1.19789333]
Error (with offset) at recomputed landmark: 1783675.2295780657 (Should be near zero)

Error (no offset) at noisy landmark: 54320.22670263611