SmartProjectionFactor

import gtsam
import numpy as np
from gtsam import (
    Values,
    Point2,
    Point3,
    Pose3,
    Rot3,
    NonlinearFactorGraph,
    SmartProjectionParams,
    SmartProjectionFactorPinholeCameraCal3_S2,
    PinholeCameraCal3_S2,
    Cal3_S2,
)
from gtsam.symbol_shorthand import C

SmartProjectionFactor<CAMERA> is a “smart” factor designed for Structure from Motion (SfM) or visual SLAM problems where both camera poses and calibration are being optimized. It implicitly represents a 3D point landmark that has been observed by multiple cameras.

Key characteristics:

Implicit Point: The 3D point is not an explicit variable in the factor graph. Instead, the factor internally triangulates the point based on the current camera estimates whenever needed (e.g., during linearization or error calculation).
Marginalization: When linearized (e.g., to a Hessian factor), it effectively marginalizes out the 3D point, creating a dense factor connecting only the cameras that observed the point.
CAMERA Template: This template parameter must represent a camera model that includes both pose and calibration (e.g., PinholeCameraCal3_S2, PinholeCameraBundler).
Values Requirement: When using this factor, the Values object passed to methods like error or linearize must contain CAMERA objects (not separate Pose3 and Calib objects) associated with the factor’s keys.
Configuration: Its behavior (linearization method, handling of degenerate triangulations) is controlled by SmartProjectionParams.

Use Case: Suitable for Bundle Adjustment or SfM problems where calibration parameters are unknown or need refinement along with camera poses. Alternative: If calibration is known and fixed, use SmartProjectionPoseFactor for better efficiency.

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.

Creating and Adding Measurements¶

Create the factor with a noise model and parameters.
Add measurements (2D points) and the corresponding camera keys one by one or in batches.

smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)
smart_params = SmartProjectionParams() # Use default params (HESSIAN, IGNORE_DEGENERACY)

# Factor type includes the Camera type, e.g., SmartProjectionFactorPinholeCameraCal3_S2
smart_factor = SmartProjectionFactorPinholeCameraCal3_S2(smart_noise, smart_params)

# Add measurements and keys
smart_factor.add(Point2(150, 505), C(0))
smart_factor.add(Point2(470, 495), C(1))
smart_factor.add(Point2(480, 150), C(2))

print(f"Smart factor involves {smart_factor.size()} measurements.")
smart_factor.print("SmartFactor: ")

Smart factor involves 3 measurements.
SmartFactor: SmartProjectionFactor
linearizationMode: 0
triangulationParameters:
rankTolerance = 1
enableEPI = 0
landmarkDistanceThreshold = -1
dynamicOutlierRejectionThreshold = -1
useLOST = 0
noise model

result:
no point, status = 1

SmartFactorBase, z = 
measurement 0, px = 
150
505
noise model = unit (2) 
measurement 1, px = 
470
495
noise model = unit (2) 
measurement 2, px = 
480
150
noise model = unit (2) 
  keys = { c0 c1 c2 }

Evaluating the Error¶

The .error(values) method implicitly triangulates the point based on the CAMERA objects in values and computes the sum of squared reprojection errors.

# Create Values containing CAMERA objects
values = Values()
K = Cal3_S2(500, 500, 0, 320, 240)
pose0 = Pose3(Rot3.Ypr(0.1, -0.1, 0.2), Point3(-1, 0, 0.5))
pose1 = Pose3(Rot3.Ypr(0.0,  0.1, 0.1), Point3( 1, 0, 0.5))
pose2 = Pose3(Rot3.Ypr(-0.1, 0.0, -0.2), Point3( 0, 1, 0.5))

values.insert(C(0), PinholeCameraCal3_S2(pose0, K))
values.insert(C(1), PinholeCameraCal3_S2(pose1, K))
values.insert(C(2), PinholeCameraCal3_S2(pose2, K))

# Triangulate first to see the implicit point
point_result = smart_factor.point(values)
print(f"Triangulated point result:\n{point_result.status}")

if point_result.valid():
   # Calculate error
   total_error = smart_factor.error(values)
   print(f"\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}")
else:
   print("\nTriangulation failed, error calculation depends on degeneracyMode.")

Triangulated point result:
Status.BEHIND_CAMERA

Triangulation failed, error calculation depends on degeneracyMode.