GeneralSFMFactor

import gtsam
import numpy as np
from gtsam import (GeneralSFMFactorCal3_S2, GeneralSFMFactor2Cal3_S2,
                   PinholeCameraCal3_S2, Pose3, Point3, Point2, Cal3_S2, Values)
from gtsam import symbol_shorthand

X = symbol_shorthand.X
L = symbol_shorthand.L
K = symbol_shorthand.K
C = symbol_shorthand.C # For Camera variable

This header defines factors for Structure from Motion (SfM) problems where camera calibration might be unknown or optimized alongside pose and structure.

GeneralSFMFactor<CAMERA, LANDMARK>:

A binary factor connecting a CAMERA variable and a LANDMARK variable.
Represents the reprojection error of the LANDMARK into the CAMERA view, compared to a 2D measured_ pixel coordinate.
The CAMERA type encapsulates both pose and calibration (e.g., PinholeCamera<Cal3Bundler>).
Error: camera.project(landmark) - measured

GeneralSFMFactor2<CALIBRATION>:

A ternary factor connecting a Pose3 variable, a Point3 landmark variable, and a CALIBRATION variable.
This explicitly separates the camera pose and calibration into different variables.
Error: PinholeCamera<CALIBRATION>(pose, calibration).project(landmark) - measured

These factors are core components for visual SLAM or SfM systems where calibration is refined or initially unknown.

1. `GeneralSFMFactor<CAMERA, LANDMARK>`¶

Connects a combined Camera variable (pose+calibration) and a Landmark. Requires the Values object to contain instances of the specific CAMERA type (e.g., PinholeCameraCal3_S2).

measured_pt = Point2(320, 240) # Measurement in pixels
sfm_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)
camera_key = C(0)
landmark_key = L(0)

# Note: The factor type name includes the Calibration, e.g., GeneralSFMFactorCal3_S2
factor1 = GeneralSFMFactorCal3_S2(measured_pt, sfm_noise, camera_key, landmark_key)
factor1.print("GeneralSFMFactor: ")

# Evaluate error - requires a Camera object in Values
values = Values()
camera_pose = Pose3() # Identity pose
calibration = Cal3_S2(500, 500, 0, 320, 240) # fx, fy, s, u0, v0
camera = PinholeCameraCal3_S2(camera_pose, calibration)
landmark = Point3(0, 0, 5) # Point 5m in front of camera

values.insert(camera_key, camera)
values.insert(landmark_key, landmark)

error1 = factor1.error(values)
print(f"\nError for GeneralSFMFactor: {error1}") # Should be [0, 0] if landmark projects to measured_pt

GeneralSFMFactor:   keys = { c0 l0 }
  noise model: unit (2) 
GeneralSFMFactor: .z[
	320;
	240
]

Error for GeneralSFMFactor: 0.0

2. `GeneralSFMFactor2<CALIBRATION>`¶

Connects separate Pose3, Point3 (Landmark), and Calibration variables.

pose_key = X(0)
calib_key = K(0)
# landmark_key = L(0) # Re-use from above

# Note: The factor type name includes the Calibration, e.g., GeneralSFMFactor2Cal3_S2
factor2 = GeneralSFMFactor2Cal3_S2(measured_pt, sfm_noise, pose_key, landmark_key, calib_key)
factor2.print("GeneralSFMFactor2: ")

# Evaluate error - requires Pose3, Point3, Cal3_S2 objects in Values
values2 = Values()
values2.insert(pose_key, camera_pose)
values2.insert(landmark_key, landmark)
values2.insert(calib_key, calibration)

error2 = factor2.error(values2)
print(f"\nError for GeneralSFMFactor2: {error2}")

GeneralSFMFactor2:   keys = { x0 l0 k0 }
  noise model: unit (2) 
GeneralSFMFactor2: .z[
	320;
	240
]

Error for GeneralSFMFactor2: 0.0

1. GeneralSFMFactor<CAMERA, LANDMARK>¶

2. GeneralSFMFactor2<CALIBRATION>¶

1. `GeneralSFMFactor<CAMERA, LANDMARK>`¶

2. `GeneralSFMFactor2<CALIBRATION>`¶