SmartProjectionPoseFactor

import gtsam
from gtsam import (
    Values,
    Point2,
    Point3,
    Pose3,
    Rot3,
    NonlinearFactorGraph,
    SmartProjectionParams,
    SmartProjectionPoseFactorCal3_S2,
    Cal3_S2,
)
from gtsam.symbol_shorthand import X  # Key for Pose3 variable

SmartProjectionPoseFactor<CALIBRATION> is a “smart” factor optimized for visual SLAM problems where the camera calibration is known and fixed, and only the camera poses are being optimized. It inherits from SmartProjectionFactor but simplifies the setup by taking a single shared CALIBRATION object (e.g., Cal3_S2, Cal3Bundler) assumed to be used by all cameras observing the landmark.

Key characteristics:

Implicit Point: Like its base class, the 3D point is not an explicit variable.
Fixed Calibration: Assumes a single, known calibration K for all measurements.
Pose Variables: The factor connects Pose3 variables directly.
Values Requirement: Requires Pose3 objects in the Values container for its keys.
Configuration: Behavior is controlled by SmartProjectionParams.

Use Case: Ideal for visual SLAM or SfM when camera intrinsics are pre-calibrated and assumed constant. Alternative: If calibration also needs optimization, use SmartProjectionFactor.

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, the shared calibration K, and parameters.
Add measurements (2D points) and the corresponding pose keys.

smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)
smart_params = SmartProjectionParams() # Use default params
K = Cal3_S2(500, 500, 0, 320, 240) # Fixed calibration

smart_factor = SmartProjectionPoseFactorCal3_S2(smart_noise, K, smart_params)

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

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

Smart factor involves 3 measurements.
SmartFactorPose: SmartProjectionPoseFactor, z = 
 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 = { x0 x1 x2 }

Evaluating the Error¶

The .error(values) method uses the Pose3 objects from values and the fixed K to triangulate the point and compute the error.

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

values.insert(X(0), pose0)
values.insert(X(1), pose1)
values.insert(X(2), pose2)

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

if point_result.valid():
   print(f"Triangulated point: {point_result.get()}")
   # 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.VALID
Triangulated point: [0.28416823 1.95555615 5.67688675]

Total reprojection error (0.5 * sum(err^2/sigma^2)): 120243.1626

Linearization¶

Linearization works similarly to SmartProjectionFactor, producing a linear factor (default: Hessian) connecting the Pose3 variables.

graph = NonlinearFactorGraph()
graph.add(smart_factor)

# Linearize (default mode is HESSIAN)
linear_factor = smart_factor.linearize(values)

if linear_factor:
    print("\nLinearized Factor (showing HessianFactor structure):")
    # Cast to HessianFactor to print its details
    hessian_factor = gtsam.HessianFactor(linear_factor)
    if hessian_factor:
         hessian_factor.print()
    else:
         print("Linearized factor is not a HessianFactor (check params.linearizationMode)")
else:
    print("Linearization failed (likely due to triangulation degeneracy)")


Linearized Factor (showing HessianFactor structure):

 keys: x0(6) x1(6) x2(6) 
Augmented information matrix: [
	202102, 6373.47, -59741.9, 4386.91, -35145.2, 11091.1, -115337, -44197.1, 12233.6, -8181.39, 19268.4, -7521.18, -81683.2, -724.877, 5044.41, -484.005, 14404.6, -5767.48, 89097.4;
	6373.47, 81115.2, -30292.7, 14717.5, -2404.93, -3343.21, 63410.4, 20584.6, -5335.01, 3787.66, -10602.2, 4111.71, -60743.7, -88128.4, 38992.5, -18242.3, 11072.3, -3393.47, 105458;
	-59741.9, -30292.7, 27634, -6415.45, 10844.3, -1981.98, 10553.8, 5348.27, -1607.72, 998.024, -1760.06, 696.401, 44568.2, 31147.8, -15125.4, 6542.51, -7986, 2832.46, -62376.9;
	4386.91, 14717.5, -6415.45, 2722.09, -996.496, -424.653, 9577.1, 3000.7, -765.229, 551.36, -1601.54, 620.327, -12253.7, -15890.6, 7106.5, -3294.65, 2225.84, -703.855, 20429.2;
	-35145.2, -2404.93, 10844.3, -996.496, 6132.48, -1869.56, 18982.5, 7333.62, -2035.73, 1357.9, -3171.11, 1238.23, 15136.7, 1537.88, -1499.49, 376.242, -2675.09, 1054.42, -17138.8;
	11091.1, -3343.21, -1981.98, -424.653, -1869.56, 777.22, -9389.67, -3428.6, 932.466, -633.633, 1569.05, -611.241, -1827.86, 3981.42, -1495.55, 805.333, 305.895, -169.934, 204.626;
	-115337, 63410.4, 10553.8, 9577.1, 18982.5, -9389.67, 210063, -32012.2, 19847.7, -6615.86, -35357.8, 12992.1, -83063.6, 2877.07, 3675.5, 245.696, 14633.2, -5901.9, 91375.5;
	-44197.1, 20584.6, 5348.27, 3000.7, 7333.62, -3428.6, -32012.2, 79820.6, -31086.7, 15340.5, 5564.44, -1509.55, 71381, -87978.3, 31244.3, -17668.1, -12223.3, 5946.04, -40235;
	12233.6, -5335.01, -1607.72, -765.229, -2035.73, 932.466, 19847.7, -31086.7, 12383.1, -5991.81, -3406.72, 1051.42, -29836.9, 33051, -11561.9, 6625.01, 5124.61, -2447.34, 18485.3;
	-8181.39, 3787.66, 998.024, 551.36, 1357.9, -633.633, -6615.86, 15340.5, -5991.81, 2949.34, 1147.27, -319.227, 13846.5, -16832.1, 5966.68, -3379.52, -2372.04, 1151.02, -7909.48;
	19268.4, -10602.2, -1760.06, -1601.54, -3171.11, 1569.05, -35357.8, 5564.44, -3406.72, 1147.27, 5951.86, -2185.74, 14119.3, -690.008, -543.913, -82.7916, -2486.55, 1005.27, -15442.2;
	-7521.18, 4111.71, 696.401, 620.327, 1238.23, -611.241, 12992.1, -1509.55, 1051.42, -319.227, -2185.74, 806.502, -4768.82, -371.498, 426.945, -95.4627, 842.323, -333.351, 5486.34;
	-81683.2, -60743.7, 44568.2, -12253.7, 15136.7, -1827.86, -83063.6, 71381, -29836.9, 13846.5, 14119.3, -4768.82, 149690, -5708.64, -6412.92, -549.716, -26368.6, 10641.3, -162324;
	-724.877, -88128.4, 31147.8, -15890.6, 1537.88, 3981.42, 2877.07, -87978.3, 33051, -16832.1, -690.008, -371.498, -5708.64, 160163, -64108.9, 32675.7, 347.645, -2041.12, -63443.5;
	5044.41, 38992.5, -15125.4, 7106.5, -1499.49, -1495.55, 3675.5, 31244.3, -11561.9, 5966.68, -543.913, 426.945, -6412.92, -64108.9, 26167.1, -13114.9, 1394.39, 202.071, 34971.1;
	-484.005, -18242.3, 6542.51, -3294.65, 376.242, 805.333, 245.696, -17668.1, 6625.01, -3379.52, -82.7916, -95.4627, -549.716, 32675.7, -13114.9, 6668.86, -37.4944, -372.943, -13620.5;
	14404.6, 11072.3, -7986, 2225.84, -2675.09, 305.895, 14633.2, -12223.3, 5124.61, -2372.04, -2486.55, 842.323, -26368.6, 347.645, 1394.39, -37.4944, 4647.64, -1867.77, 28880.4;
	-5767.48, -3393.47, 2832.46, -703.855, 1054.42, -169.934, -5901.9, 5946.04, -2447.34, 1151.02, 1005.27, -333.351, 10641.3, -2041.12, 202.071, -372.943, -1867.77, 773.191, -10827.4;
	89097.4, 105458, -62376.9, 20429.2, -17138.8, 204.626, 91375.5, -40235, 18485.3, -7909.48, -15442.2, 5486.34, -162324, -63443.5, 34971.1, -13620.5, 28880.4, -10827.4, 240486
]