SmartProjectionRigFactor

SmartProjectionRigFactor<CAMERA> is a generalization of SmartProjectionPoseFactor designed for multi-camera systems (rigs). Like other smart factors, it implicitly represents a 3D point landmark observed by multiple cameras.

Key differences/features:

Multi-Camera Rig: Assumes a fixed rig configuration, where multiple cameras (CAMERA instances, which include fixed intrinsics and fixed extrinsics relative to the rig’s body frame) are defined.
Pose Variables: Connects Pose3 variables representing the pose of the rig’s body frame in the world.
Multiple Observations per Pose: Allows multiple measurements associated with the same body pose key, but originating from different cameras within the rig.
Camera Indexing: Each measurement must be associated with both a body pose key and a cameraId (index) specifying which camera in the rig took the measurement.
Fixed Calibration/Extrinsics: The intrinsics and relative extrinsics of the cameras within the rig are assumed fixed.
CAMERA Template: Can be templated on various camera models (e.g., PinholePose, SphericalCamera), provided they adhere to the expected GTSAM camera concepts. Important Note: See the Note on Template Parameter CAMERA below.
Values Requirement: Requires Pose3 objects (representing the body frame) in the Values container.
Configuration: Behavior controlled by SmartProjectionParams. Note: Currently (as of C++ header comment), only supports HESSIAN linearization mode and ZERO_ON_DEGENERACY mode.

Use Case: Ideal for visual SLAM with a calibrated multi-camera rig (e.g., stereo rig, multi-fisheye system) where only the rig’s pose is optimized.

Note on Template Parameter CAMERA: While this factor is templated on CAMERA for generality, the current internal implementation for linearization has limitations. It implicitly assumes that traits<CAMERA>::dimension matches the optimized variable dimension (Pose3::dimension, which is 6). Consequently:

It works correctly with CAMERA types where dimension == 6, such as PinholePose<CALIBRATION> or SphericalCamera.
Using CAMERA types where dimension != 6, such as PinholeCamera<CALIBRATION> (dim = 6 + CalDim), will cause compilation errors.
Recommendation: For standard pinhole cameras in a fixed rig, use PinholePose<CALIBRATION> as the CAMERA type when defining the CameraSet for this factor.

If you are using the factor, please cite:

L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, “Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors”, Int. Conf. on Robotics and Automation (ICRA), 2014.

import gtsam
from gtsam import (
    Values,
    Point2,
    Point3,
    Pose3,
    Rot3,
    SmartProjectionParams,
    LinearizationMode,
    DegeneracyMode,
    # Use PinholePose variant for wrapping
    SmartProjectionRigFactorPinholePoseCal3_S2,
    PinholePoseCal3_S2,
    Cal3_S2,
)
from gtsam.symbol_shorthand import X  # Key for Pose3 variable (Body Pose)

Constructor¶

You create a SmartProjectionRigFactor by providing:

A noise model for the 2D pixel measurements (typically noiseModel.Isotropic).
A CameraSet<CAMERA> object defining the fixed rig configuration. Each CAMERA in the set contains its fixed intrinsics and its fixed pose relative to the rig’s body frame (body_P_camera).
Optionally, SmartProjectionParams to configure linearization and degeneracy handling. Remember the current restrictions (HESSIAN, ZERO_ON_DEGENERACY).

# Define the Camera Rig (using PinholePose)
K = Cal3_S2(500, 500, 0, 320, 240)
body_P_cam0 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, 0, 0))
cam0 = PinholePoseCal3_S2(body_P_cam0, K)
body_P_cam1 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, -0.1, 0))  # Baseline 0.1m
cam1 = PinholePoseCal3_S2(body_P_cam1, K)

rig_cameras = gtsam.CameraSetPinholePoseCal3_S2()
rig_cameras.push_back(cam0)
rig_cameras.push_back(cam1)

# Noise model and parameters
noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)
smart_params = SmartProjectionParams(
    linMode=LinearizationMode.HESSIAN, degMode=DegeneracyMode.ZERO_ON_DEGENERACY
)

# Create the Factor
smart_factor = SmartProjectionRigFactorPinholePoseCal3_S2(
    noise_model, rig_cameras, smart_params
)
smart_factor.print("SmartFactorRig: ")

SmartFactorRig: SmartProjectionRigFactor: 
 SmartProjectionFactor
linearizationMode: 0
triangulationParameters:
rankTolerance = 1
enableEPI = 0
landmarkDistanceThreshold = -1
dynamicOutlierRejectionThreshold = -1
useLOST = 0
noise model

result:
no point, status = 1

SmartFactorBase, z = 
  keys = { }

`add(measurement, poseKey, cameraId)`¶

This method adds a single 2D measurement (Point2) associated with a specific camera in the rig and a specific body pose.

measurement: The observed pixel coordinates.
poseKey: The key (Symbol or Key) of the body’s Pose3 variable at the time of observation.
cameraId: The integer index of the camera within the CameraSet (provided during construction) that captured this measurement.

# --- Use Pre-calculated Valid Measurements ---
# These measurements were calculated offline using:
# gt_landmark = Point3(1.0, 0.5, 5.0)
# gt_pose0 = Pose3()
# gt_pose1 = Pose3(Rot3.Ypr(0.1, 0.0, 0.0), Point3(0.5, 0, 0))
# And the rig defined above.

z00 = Point2(400.0, 290.0)    # Measurement from Body Pose X(0), Camera 0
z01 = Point2(350.0, 290.0)    # Measurement from Body Pose X(0), Camera 1
z10 = Point2(372.787, 297.553) # Measurement from Body Pose X(1), Camera 0
z11 = Point2(323.308, 297.674) # Measurement from Body Pose X(1), Camera 1
# --------------------------------------------

# 3. Add pre-calculated measurements
smart_factor.add(z00, X(0), 0)
smart_factor.add(z01, X(0), 1)
smart_factor.add(z10, X(1), 0)
smart_factor.add(z11, X(1), 1)

print(f"Smart factor involves {smart_factor.size()} measurements from {len(smart_factor.keys())} unique poses.")
# smart_factor.print("SmartFactorRig (with pre-calculated measurements): ") # Optional

Smart factor involves 2 measurements from 2 unique poses.

Inherited and Core Methods¶

`error(Values values)`¶

Inherited from SmartFactorBase. Calculates the total reprojection error for the landmark. Internal Process:

Retrieves the body Pose3 estimates for all relevant keys from the values object.
For each measurement, calculates the corresponding camera’s world pose using the body pose and the fixed rig extrinsics (world_P_sensor = world_P_body * body_P_camera).
Triangulates the 3D landmark position using these calculated camera poses and the stored 2D measurements.
Reprojects the triangulated point back into each calculated camera view.
Computes the sum of squared differences between the reprojections and the original measurements, weighted by the noise model.
Handles degenerate cases (e.g., failed triangulation) based on the degeneracyMode set in SmartProjectionParams.

# Assuming smart_factor created and measurements added above

values = Values()
pose0 = Pose3() # Body at origin
pose1 = Pose3(Rot3.Ypr(0.1, 0.0, 0.0), Point3(0.5, 0, 0)) # Body moved
values.insert(X(0), pose0)
values.insert(X(1), pose1)

# Need to check triangulation first, as error calculation depends on it
point_result = smart_factor.point(values)
print(f"Triangulation Result Status: {point_result.status}")

total_error = smart_factor.error(values)
print(f"Total reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}")
# Note: If degenerate and DegeneracyMode is ZERO_ON_DEGENERACY, error will be 0.

Triangulation Result Status: Status.VALID
Total reprojection error (0.5 * sum(err^2/sigma^2)): 1316.4717

`point(Values values)`¶

Inherited from SmartProjectionFactor. Performs the internal triangulation based on the body poses in values and the fixed rig geometry. Returns a TriangulationResult object which contains:

The triangulated Point3 (if successful).
A status indicating whether the triangulation was VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, or FAR_POINT.

# Assuming smart_factor and values from the previous cell
point_result = smart_factor.point(values)
print(f"Triangulation Result Status: {point_result.status}")

if point_result.valid():
    triangulated_point = point_result.get()
    print(f"Triangulated Point: {triangulated_point}")
else:
    print("Triangulation did not produce a valid point.")

Triangulation Result Status: Status.VALID
Triangulated Point: [0.94370846 0.79793704 7.63497051]

`cameras(Values values)`¶

Inherited from SmartFactorBase. Computes and returns a CameraSet<CAMERA> containing the effective cameras corresponding to each measurement. For each measurement i associated with body pose key k and camera ID cid:

Retrieves the body pose world_P_body = values.atPose3(k).
Retrieves the fixed relative pose body_P_camera = rig_cameras.at(cid).pose().
Computes the camera’s world pose world_P_camera = world_P_body * body_P_camera.
Creates a CAMERA object using this world_P_camera and the fixed intrinsics rig_cameras.at(cid).calibration(). The returned CameraSet contains these calculated cameras, one for each measurement added via add().

# Assuming smart_factor and values from previous cells

calculated_cameras = smart_factor.cameras(values)

# Print the world pose of each calculated camera
print(f"Pose of camera for measurement 0 (Body X(0), Cam 0):\n{calculated_cameras.at(0).pose()}\n")
print(f"Pose of camera for measurement 1 (Body X(0), Cam 1):\n{calculated_cameras.at(1).pose()}\n")
print(f"Pose of camera for measurement 2 (Body X(1), Cam 0):\n{calculated_cameras.at(2).pose()}\n")
print(f"Pose of camera for measurement 3 (Body X(1), Cam 1):\n{calculated_cameras.at(3).pose()}\n")

Pose of camera for measurement 0 (Body X(0), Cam 0):
R: [
	1, 0, 0;
	0, 1, 0;
	0, 0, 1
]
t: 0.1   0   0


Pose of camera for measurement 1 (Body X(0), Cam 1):
R: [
	1, 0, 0;
	0, 1, 0;
	0, 0, 1
]
t:  0.1 -0.1    0


Pose of camera for measurement 2 (Body X(1), Cam 0):
R: [
	0.995004, -0.0998334, 0;
	0.0998334, 0.995004, 0;
	0, 0, 1
]
t:     0.5995 0.00998334          0


Pose of camera for measurement 3 (Body X(1), Cam 1):
R: [
	0.995004, -0.0998334, 0;
	0.0998334, 0.995004, 0;
	0, 0, 1
]
t:   0.609484 -0.0895171          0

`linearize(Values values)`¶

Inherited from SmartProjectionFactor. Computes the linear approximation (GaussianFactor) of the factor at the linearization point defined by values. For SmartProjectionRigFactor, due to current implementation limitations, this must be configured via SmartProjectionParams to use LinearizationMode.HESSIAN. The resulting RegularHessianFactor connects the unique body pose keys involved in the measurements.

# Assuming smart_factor and values from previous cells

# Check if triangulation succeeded before linearizing
if not smart_factor.point(values).valid():
    print("Cannot linearize: triangulation failed or degenerate.")
else:
    linear_factor = smart_factor.linearize(values)

    if linear_factor:
        print("\nLinearized Factor (HessianFactor structure):")
        linear_factor.print("Linear Factor: ")
    else:
        print("\nLinearization failed (potentially due to triangulation degeneracy and params setting).")

# Note: The printed Hessian is often zero when ZERO_ON_DEGENERACY is used and triangulation fails.


Linearized Factor (HessianFactor structure):
Linear Factor: 
 keys: x0(6) x1(6) 
Augmented information matrix: [
	255621, 1454.13, -31747.6, 636.066, -33103.6, 3605.16, -254669, 22279.1, 15195.9, 2671.95, 33001.7, -3605.16, -5437.65;
	1454.13, 9642.56, -1187.49, 1253.63, -198.336, -75.3949, -2405.75, -9411.71, 1088.32, -1227.56, 322.499, 75.3949, -653.552;
	-31747.6, -1187.49, 4048.22, -209.638, 4112.44, -437.73, 31729.4, -1770.15, -1992, -201.969, -4112.82, 437.73, 740.416;
	636.066, 1253.63, -209.638, 163.769, -83.6702, -3.45048, -757.87, -1182.15, 167.803, -154.598, 99.6018, 3.45048, -94.317;
	-33103.6, -198.336, 4112.44, -83.6702, 4287, -466.758, 32981.3, -2875.28, -1968.94, -344.734, -4273.93, 466.758, 704.833;
	3605.16, -75.3949, -437.73, -3.45048, -466.758, 51.9764, -3582.21, 409.075, 204.351, 50.0313, 464.082, -51.9764, -70.5256;
	-254669, -2405.75, 31729.4, -757.87, 32981.3, -3582.21, 253816, -21248.6, -15238.8, -2538.55, -32892.2, 3582.21, 5479.25;
	22279.1, -9411.71, -1770.15, -1182.15, -2875.28, 409.075, -21248.6, 11385.4, 332.508, 1463.29,

2742.9, -409.075, 142.514;
	15195.9, 1088.32, -1992, 167.803, -1968.94, 204.351, -15238.8, 332.508, 1007.53, 29.6019, 1975.86, -204.351, -387.999;
	2671.95, -1227.56, -201.969, -154.598, -344.734, 50.0313, -2538.55, 1463.29, 29.6019, 188.241, 327.577, -50.0313, 23.48;
	33001.7, 322.499, -4112.82, 99.6018, -4273.93, 464.082, -32892.2, 2742.9, 1975.86, 327.577, 4262.53, -464.082, -710.727;
	-3605.16, 75.3949, 437.73, 3.45048, 466.758, -51.9764, 3582.21, -409.075, -204.351, -50.0313, -464.082, 51.9764, 70.5256;
	-5437.65, -653.552, 740.416, -94.317, 704.833, -70.5256, 5479.25, 142.514, -387.999, 23.48, -710.727, 70.5256, 2632.94
]

Other Inherited Methods¶

The factor also inherits standard methods from NonlinearFactor and SmartFactorBase:

keys(): Returns a KeyVector containing the unique body pose keys involved.
measured(): Returns a Point2Vector containing all the added 2D measurements.
dim(): Returns the dimension of the error vector (2 * number of measurements).
size(): Returns the number of measurements added.
print(s, keyFormatter): Prints details about the factor.
equals(other, tol): Compares two factors for equality.

# Assuming smart_factor from previous cells
print(f"Keys: {[gtsam.DefaultKeyFormatter(k) for k in smart_factor.keys()]}")
print(f"Number of Measurements (size): {smart_factor.size()}")
print(f"Dimension (dim): {smart_factor.dim()}")
print(f"Measurements: {smart_factor.measured()}")

Keys: ['x0', 'x1']
Number of Measurements (size): 2
Dimension (dim): 8
Measurements: [array([400., 290.]), array([350., 290.]), array([372.787, 297.553]), array([323.308, 297.674])]

GTSAM Docs

SmartProjectionPoseFactor

GTSAM Docs

GenericStereoFactor

SmartProjectionRigFactor

Constructor¶

add(measurement, poseKey, cameraId)¶

Inherited and Core Methods¶

error(Values values)¶

point(Values values)¶

cameras(Values values)¶