TriangulationFactor - GTSAM Docs

TriangulationFactor<CAMERA> is a unary factor that constrains a single unknown 3D point (Point3) based on a 2D measurement from a known camera. It’s essentially the inverse of a projection factor where the camera pose and calibration are fixed, and only the 3D point is variable.

Key characteristics:

Unary Factor: Connects only to a Point3 variable key.
Known Camera: The CAMERA object (e.g., PinholeCameraCal3_S2, StereoCamera) containing the fixed pose and calibration is provided during factor construction.
Measurement: Takes a 2D measurement (Point2 for monocular, StereoPoint2 for stereo).
Error: Calculates the reprojection error: $\text{error}(L) = \text{camera.project}(L) - z$

Use Case: Useful in triangulation scenarios where multiple camera views with known poses observe an unknown landmark. A NonlinearFactorGraph containing only TriangulationFactors (one for each view) can be optimized to find the maximum likelihood estimate of the 3D landmark position.

import gtsam
import numpy as np
from gtsam import Point3, Point2, Pose3, Rot3, Cal3_S2, PinholeCameraCal3_S2, Values, NonlinearFactorGraph
# The Python wrapper often creates specific instantiations
from gtsam import TriangulationFactorCal3_S2
from gtsam import symbol_shorthand

L = symbol_shorthand.L

Creating a TriangulationFactor¶

Instantiate by providing:

The known CAMERA object.
The 2D measurement.
The noise model for the measurement.
The key for the unknown Point3 landmark.
(Optional) Cheirality handling flags.

# Known camera parameters
K = Cal3_S2(500.0, 500.0, 0.0, 320.0, 240.0)
pose = Pose3(Rot3.Ypr(0.1, -0.1, 0.2), Point3(1, 0, 0.5))
camera = PinholeCameraCal3_S2(pose, K)

# Measurement observed by this camera
measured_pt2 = Point2(450.5, 300.2)
pixel_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)
landmark_key = L(0)

# Factor type includes the Camera type
factor = TriangulationFactorCal3_S2(camera, measured_pt2, pixel_noise, landmark_key)
factor.print("TriangulationFactor: ")

TriangulationFactor: TriangulationFactor,camera.pose R: [
	0.990033, -0.117578, -0.0775207;
	0.0993347, 0.97319, -0.207445;
	0.0998334, 0.197677, 0.97517
]
t:   1   0 0.5
camera.calibration[
	500, 0, 320;
	0, 500, 240;
	0, 0, 1
]
z[
	450.5;
	300.2
]
  keys = { l0 }
  noise model: unit (2)

Evaluating the Error¶

The error is the reprojection error given an estimate of the landmark’s Point3 position.

values = Values()

# Estimate for the landmark position
landmark_estimate = Point3(3.0, 0.5, 2.0)
values.insert(landmark_key, landmark_estimate)

error = factor.error(values)
print(f"Reprojection error for estimate {landmark_estimate}: {error}")

# Calculate expected projection
expected_projection = camera.project(landmark_estimate)
manual_error = expected_projection - measured_pt2
print(f"Expected projection: {expected_projection}")
print(f"Manual error calculation: {manual_error}")
assert np.allclose(factor.unwhitenedError(values), manual_error)

Reprojection error for estimate [3.  0.5 2. ]: 314012.75623020524
Expected projection: [1225.10768109  467.55726116]
Manual error calculation: [774.60768109 167.35726116]

Usage in Triangulation¶

Multiple TriangulationFactors, one for each known camera view, can be added to a graph to solve for the landmark position.

# Create a second camera and measurement
pose2 = Pose3(Rot3.Ypr(-0.1, 0.1, -0.1), Point3(-1, 0, 0.5))
camera2 = PinholeCameraCal3_S2(pose2, K)
measured_pt2_cam2 = Point2(180.0, 190.0)
factor2 = TriangulationFactorCal3_S2(camera2, measured_pt2_cam2, pixel_noise, landmark_key)

# Create graph and add factors
triangulation_graph = NonlinearFactorGraph()
triangulation_graph.add(factor)
triangulation_graph.add(factor2)

# Optimize (requires an initial estimate)
initial_estimate = Values()
initial_estimate.insert(landmark_key, Point3(2, 0, 5)) # Initial guess

optimizer = gtsam.LevenbergMarquardtOptimizer(triangulation_graph, initial_estimate)
result = optimizer.optimize()

print("\nOptimized Landmark Position:")
result.print()
print(f"Final Error: {triangulation_graph.error(result):.4f}")


Optimized Landmark Position:
Values with 1 values:
Value l0: (class Eigen::Matrix<double,-1,1,0,-1,1>)
[
	-12446.1;
	-55075.8;
	2.39319e+06
]

Final Error: 7855.8598