TriangulationFactor - GTSAM Docs

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

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.

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