Fast CPU bundle adjustment: Full and Schur

Created by Codex.

This tutorial solves one bundle-adjustment problem twice with the CPU SfmLevenbergMarquardtOptimizer: first with the fastest measured Full-system configuration, then with explicit Schur elimination. The example includes a calibration shared by every camera, so it also demonstrates that CPU Schur elimination removes Point3 landmarks while keeping poses and calibration in the reduced system.

import numpy as np
import plotly.graph_objects as go

import gtsam
from gtsam.examples import SFMdata
from gtsam.symbol_shorthand import K, L, X

LinearSolver = gtsam.NonlinearOptimizerParams.LinearSolverType

1. Build a bundle-adjustment graph¶

Four camera poses observe eight landmarks. Every projection factor refers to the same calibration variable k0; calibration is therefore optimized globally rather than stored separately in each camera. Priors on the first pose and landmark fix the coordinate frame and scale, while a loose calibration prior regularizes self-calibration.

poses = SFMdata.createPoses()[:4]
points = SFMdata.createPoints()
calibration = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)

graph = gtsam.NonlinearFactorGraph()
measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)
for i, pose in enumerate(poses):
    camera = gtsam.PinholeCameraCal3_S2(pose, calibration)
    for j, point in enumerate(points):
        graph.add(
            gtsam.GeneralSFMFactor2Cal3_S2(
                camera.project(point), measurement_noise, X(i), L(j), K(0)
            )
        )

graph.addPriorPose3(
    X(0), poses[0],
    gtsam.noiseModel.Diagonal.Sigmas(
        np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3])
    ),
)
graph.addPriorPoint3(
    L(0), points[0], gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
)
graph.addPriorCal3_S2(
    K(0), calibration,
    gtsam.noiseModel.Diagonal.Sigmas(
        np.array([500.0, 500.0, 0.1, 100.0, 100.0])
    ),
)
print(f"{graph.size()} factors, {len(poses)} poses, {len(points)} points")

35 factors, 4 poses, 8 points

2. Create one initial estimate¶

Both optimizations start from exactly the same deterministic perturbation. This makes their final objectives directly comparable.

initial = gtsam.Values()
initial.insert(K(0), gtsam.Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0))
pose_delta = gtsam.Pose3(
    gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25),
    gtsam.Point3(0.05, -0.10, 0.20),
)
for i, pose in enumerate(poses):
    initial.insert(X(i), pose.compose(pose_delta))
for j, point in enumerate(points):
    # Keep the fixed Point3 type distinct from a dynamic NumPy vector.
    initial.insertPoint3(
        L(j), point + gtsam.Point3(-0.25, 0.20, 0.15)
    )

initial_error = graph.error(initial)
print(f"Initial error: {initial_error:.6f}")

Initial error: 9805.400745

3. Calculate the ordering once¶

reduced_ordering contains only the four poses and shared calibration. schur_ordering is the complete point-first ordering: naturally ordered landmarks followed by that METIS reduced ordering. The fastest measured CPU path uses this complete ordering with Full mode and MULTIFRONTAL_SOLVER.

reduced_ordering = (
    gtsam.SfmLevenbergMarquardtOptimizer.CreateReducedOrdering(
        graph, initial
    )
)
schur_ordering = (
    gtsam.SfmLevenbergMarquardtOptimizer.CreateSchurOrdering(
        graph, reduced_ordering
    )
)

print(f"Reduced ordering: {reduced_ordering.size()} variables")
print(f"Complete Schur ordering: {schur_ordering.size()} variables")
assert reduced_ordering.size() == len(poses) + 1
assert schur_ordering.size() == initial.size()

Reduced ordering: 5 variables
Complete Schur ordering: 13 variables

4. Fastest measured CPU path: Full + multifrontal¶

Full mode passes the entire linearized graph to the ordinary nonlinear-optimizer solver. Supplying schur_ordering makes MultifrontalSolver eliminate all landmarks first and then factor the camera-and-calibration Schur complement in the same Bayes tree—there is no separately materialized reduced graph or second solver call.

full_params = gtsam.SfmLevenbergMarquardtParams.ceresDefaults()
full_params.setEliminationMode(gtsam.SfmEliminationMode.Full)
full_params.setLinearSolver(LinearSolver.MULTIFRONTAL_SOLVER)
full_params.setOrdering(schur_ordering)

full_optimizer = gtsam.SfmLevenbergMarquardtOptimizer(
    graph, initial, full_params
)
full_result = full_optimizer.optimize()
full_error = graph.error(full_result)
print(f"Full final error: {full_error:.9f}")

Full final error: 0.000172184

5. Explicit Schur mode¶

Schur-mode parameters accept the reduced ordering, not the complete one. With MULTIFRONTAL_SOLVER, the optimizer completes it internally and performs the same landmark-first elimination in one multifrontal factorization. Other solver choices would instead receive an explicitly materialized reduced graph.

schur_params = gtsam.SfmLevenbergMarquardtParams.ceresDefaults()
schur_params.setEliminationMode(gtsam.SfmEliminationMode.Schur)
schur_params.setLinearSolver(LinearSolver.MULTIFRONTAL_SOLVER)
schur_params.setOrdering(reduced_ordering)

schur_optimizer = gtsam.SfmLevenbergMarquardtOptimizer(
    graph, initial, schur_params
)
schur_result = schur_optimizer.optimize()
schur_error = graph.error(schur_result)
print(f"Schur final error: {schur_error:.9f}")

Schur final error: 0.000172184

6. Verify both paths¶

The two modes may take slightly different floating-point paths, but they should converge to the same objective and nearly identical shared calibration. These assertions also make the notebook a useful end-to-end smoke test of the Python wrappers.

assert full_error < initial_error
assert schur_error < initial_error
np.testing.assert_allclose(full_error, schur_error, rtol=1e-7, atol=1e-9)
np.testing.assert_allclose(
    full_result.atCal3_S2(K(0)).vector(),
    schur_result.atCal3_S2(K(0)).vector(),
    rtol=1e-6,
    atol=1e-6,
)
print("Full and Schur agree.")
print("Recovered calibration:", full_result.atCal3_S2(K(0)).vector())

Full and Schur agree.
Recovered calibration: [ 4.99680801e+01  4.66069104e+01 -2.45373588e-07  5.00012934e+01
  4.83109424e+01]

7. Inspect the reconstruction¶

The recovered landmarks should overlay the ground truth. Hover over either point cloud to inspect coordinates.

truth_xyz = np.asarray(points)
result_xyz = np.asarray(
    [full_result.atPoint3(L(j)) for j in range(len(points))]
)
figure = go.Figure()
figure.add_trace(
    go.Scatter3d(
        x=truth_xyz[:, 0], y=truth_xyz[:, 1], z=truth_xyz[:, 2],
        mode="markers", name="Ground truth", marker={"size": 6},
    )
)
figure.add_trace(
    go.Scatter3d(
        x=result_xyz[:, 0], y=result_xyz[:, 1], z=result_xyz[:, 2],
        mode="markers", name="Optimized", marker={"size": 3},
    )
)
figure.update_layout(
    title="CPU SFM reconstruction",
    scene={"aspectmode": "data"},
    margin={"l": 0, "r": 0, "b": 0, "t": 40},
)
figure.show()