Skip to article frontmatterSkip to article content

GeneralSFMFactor

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.

Open In Colab

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

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
GTSAM Docs
Frobenius Norm Factors
GTSAM Docs
InitializePose3