In VisualISAMExample, the camera calibration K was assumed known and fixed. Real cameras often aren’t calibrated ahead of time, so this notebook solves for K jointly with the poses and landmarks -- a technique called self-calibration (or auto-calibration).
Self-calibration is fundamentally harder than ordinary structure-from-motion: an unknown focal length introduces an extra ambiguity, since a farther, larger scene photographed with a longer lens can look almost identical in the image to a closer, smaller scene photographed with a shorter lens. Left unconstrained, this ambiguity has no unique solution -- so, in addition to the usual pose and landmark priors, we also need a prior on the calibration itself to pin the problem down.
This notebook is a direct transcription of examples/SelfCalibrationExample.cpp; the scene (a 10-meter cube of landmarks, 8 cameras circling it) is regenerated locally rather than imported from the shared SFMdata helper module, to stay a faithful match to the C++ original.
GTSAM Copyright 2010-2026, Georgia Tech Research Corporation, Atlanta, Georgia 30332-0415 All Rights Reserved
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
See LICENSE for the license information
try:
import google.colab
%pip install --quiet gtsam-develop
except ImportError:
passimport math
from gtsam import Cal3_S2
from gtsam.noiseModel import Diagonal, Isotropic
# SFM-specific factors
from gtsam import GeneralSFMFactor2Cal3_S2 # does calibration !
from gtsam import PinholeCameraCal3_S2
# Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
from gtsam import Point2
from gtsam import Point3, Pose3, Rot3
# Inference and optimization
from gtsam import NonlinearFactorGraph, DoglegOptimizer, Values
from gtsam.symbol_shorthand import K, L, X1. Scene setup¶
Same scene shape as the VisualISAMExample notebook -- a 10-meter landmark cube, 8 poses on a circular orbit always facing the center -- regenerated here directly instead of via the shared helper module.
def createPoints() -> list[Point3]:
"""Create the set of ground-truth landmarks"""
return [
Point3(10.0, 10.0, 10.0),
Point3(-10.0, 10.0, 10.0),
Point3(-10.0, -10.0, 10.0),
Point3(10.0, -10.0, 10.0),
Point3(10.0, 10.0, -10.0),
Point3(-10.0, 10.0, -10.0),
Point3(-10.0, -10.0, -10.0),
Point3(10.0, -10.0, -10.0),
]
def createPoses(
init: Pose3 = Pose3(Rot3.Ypr(math.pi / 2, 0, -math.pi / 2), Point3(30, 0, 0)),
delta: Pose3 = Pose3(
Rot3.Ypr(0, -math.pi / 4, 0),
Point3(math.sin(math.pi / 4) * 30, 0, 30 * (1 - math.sin(math.pi / 4))),
),
steps: int = 8,
) -> list[Pose3]:
"""Create the set of ground-truth poses: a circular trajectory, radius 30
at pi/4 intervals, always facing the circle center."""
poses: list[Pose3] = []
poses.append(init)
for i in range(1, steps):
poses.append(poses[i - 1].compose(delta))
return poses
points: list[Point3] = createPoints()
poses: list[Pose3] = createPoses()2. Build the factor graph¶
A prior anchors pose x0. Then, for every pose/landmark pair, we project the ground-truth landmark through the true calibration Kcal to get a simulated pixel measurement, and add a GeneralSFMFactor2Cal3_S2 -- the calibration-aware counterpart to the plain projection factor used in VisualISAMExample. Every one of these factors shares the same calibration key K(0), since there’s only one physical camera whose calibration we’re trying to recover from all the views combined.
graph = NonlinearFactorGraph()
# Add a prior on pose x1.
# 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
poseNoise = Diagonal.Sigmas([0.1, 0.1, 0.1, 0.3, 0.3, 0.3])
graph.addPriorPose3(X(0), poses[0], poseNoise)
# Simulated measurements from each camera pose, adding them to the factor graph
Kcal = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
measurementNoise = Isotropic.Sigma(2, 1.0)
for i, pose in enumerate(poses):
for j, point in enumerate(points):
camera = PinholeCameraCal3_S2(pose, Kcal)
measurement: Point2 = camera.project(point)
# The only real difference with the Visual SLAM example is that here we
# use a different factor type, that also calculates the Jacobian with
# respect to calibration
graph.add(
GeneralSFMFactor2Cal3_S2(
measurement,
measurementNoise,
X(i),
L(j),
K(0),
)
)3. Priors on landmark and calibration¶
As in VisualISAMExample, a prior on landmark l0 fixes the overall scale. Self-calibration adds one more thing to pin down: without some outside information about the calibration, the focal-length/scene-scale ambiguity described above leaves the problem underconstrained. A (loose) prior on K(0) supplies that information.
# Add a prior on the position of the first landmark.
pointNoise = Isotropic.Sigma(3, 0.1)
graph.addPriorPoint3(L(0), points[0], pointNoise) # add directly to graph
# Add a prior on the calibration.
calNoise = Diagonal.Sigmas([500, 500, 0.1, 100, 100])
graph.addPriorCal3_S2(K(0), Kcal, calNoise)4. Initial estimate¶
Every pose and landmark starts from a perturbed guess, just like in VisualISAMExample -- but now the calibration itself also starts from a deliberately wrong guess: fx = fy = 60 and principal point (45, 45), versus the true fx = fy = 50 and principal point (50, 50).
initialEstimate = Values()
initialEstimate.insert(K(0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0))
for i, pose in enumerate(poses):
initialEstimate.insert(
X(i),
pose.compose(
Pose3(Rot3.Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))
),
)
for j, point in enumerate(points):
initialEstimate.insert(L(j), point + Point3(-0.25, 0.20, 0.15))5. Optimize¶
This time we solve with DoglegOptimizer, the trust-region alternative to Levenberg-Marquardt compared against it in DogLegOptimizerExample.
result: Values = DoglegOptimizer(graph, initialEstimate).optimize()
result.print("Final results:\n")Final results:
Values with 17 values:
Value k0: (gtsam::Cal3_S2)
[
50, -1.7844e-16, 50;
0, 50, 50;
0, 0, 1
]
Value l0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10;
10;
10
]
Value l1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-10;
10;
10
]
Value l2: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-10;
-10;
10
]
Value l3: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10;
-10;
10
]
Value l4: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10;
10;
-10
]
Value l5: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-10;
10;
-10
]
Value l6: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-10;
-10;
-10
]
Value l7: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10;
-10;
-10
]
Value x0: (gtsam::Pose3)
R: [
7.88266e-17, -5.33571e-17, -1;
1, 1.69742e-17, 2.13565e-17;
-8.02971e-18, -1, 6.47159e-17
]
t: 30 -4.01306e-18 1.21656e-20
Value x1: (gtsam::Pose3)
R: [
-0.707107, -2.07839e-16, -0.707107;
0.707107, -3.08858e-17, -0.707107;
1.12199e-16, -1, 1.8707e-16
]
t: 21.2132 21.2132 -3.13375e-15
Value x2: (gtsam::Pose3)
R: [
-1, -9.91995e-17, 1.8202e-17;
3.48175e-17, -2.20241e-17, -1;
1.6168e-16, -1, 1.25939e-17
]
t: -1.17337e-15 30 7.10179e-16
Value x3: (gtsam::Pose3)
R: [
-0.707107, -2.56546e-16, 0.707107;
-0.707107, 1.24028e-17, -0.707107;
1.2373e-16, -1, -1.51259e-16
]
t: -21.2132 21.2132 9.19136e-15
Value x4: (gtsam::Pose3)
R: [
-2.6377e-16, -2.31823e-16, 1;
-1, -2.98388e-17, -3.63243e-16;
-4.274e-17, -1, -2.00741e-16
]
t: -30 4.2481e-16 9.38199e-15
Value x5: (gtsam::Pose3)
R: [
0.707107, -1.7636e-16, 0.707107;
-0.707107, -6.52449e-17, 0.707107;
-1.37343e-16, -1, -1.38407e-16
]
t: -21.2132 -21.2132 7.60698e-15
Value x6: (gtsam::Pose3)
R: [
1, -1.16769e-16, 6.68628e-16;
-6.46423e-16, 1.16778e-17, 1;
-9.30176e-17, -1, 4.11323e-17
]
t: -2.24367e-15 -30 2.73581e-15
Value x7: (gtsam::Pose3)
R: [
0.707107, -3.35454e-16, -0.707107;
0.707107, 6.88657e-17, 0.707107;
-2.32754e-16, -1, 2.76909e-16
]
t: 21.2132 -21.2132 -6.87553e-15
Despite starting K(0) more than 10 pixels off in focal length and 5 pixels off in principal point -- on top of every pose and landmark also starting from a wrong guess -- the optimizer recovers a calibration close to the true Cal3_S2(50, 50, 0, 50, 50). Self-calibration is harder than SFM with known calibration, but it isn’t hopeless, provided the graph carries enough independent views and a prior to fix the remaining ambiguity.