SfmLevenbergMarquardt

The SfmLevenbergMarquardtOptimizer class is an easy entry point for structure from motion. Together with SfmLevenbergMarquardtParams, it runs CPU Levenberg–Marquardt and lets users choose whether landmarks are eliminated through a Schur complement.

import gtsam
import numpy as np

from gtsam.symbol_shorthand import K, L, X

Build a small structure-from-motion problem¶

We first create two cameras, four landmarks, and their image measurements. The graph stores poses, points, and a shared calibration as separate variables. Priors fix the gauge, while deliberately perturbed initial values give the optimizer something to improve.

poses = [
    gtsam.Pose3(),
    gtsam.Pose3(gtsam.Rot3.Ypr(0.02, -0.01, 0.01), np.array([1.0, 0.0, 0.0])),
]
points = [
    np.array([-1.0, -0.5, 5.0]),
    np.array([0.8, -0.6, 4.5]),
    np.array([-0.7, 0.9, 5.5]),
    np.array([1.1, 0.8, 6.0]),
]
calibration = gtsam.Cal3_S2(500.0, 505.0, 0.0, 320.0, 240.0)
pixel_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)

graph = gtsam.NonlinearFactorGraph()
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), pixel_noise, X(i), L(j), K(0)
            )
        )
graph.addPriorPose3(
    X(0), poses[0], gtsam.noiseModel.Isotropic.Sigma(6, 1e-3)
)
graph.addPriorPoint3(
    L(0), points[0], gtsam.noiseModel.Isotropic.Sigma(3, 1e-3)
)
graph.addPriorCal3_S2(
    K(0),
    calibration,
    gtsam.noiseModel.Diagonal.Sigmas(
        np.array([50.0, 50.0, 0.1, 10.0, 10.0])
    ),
)

initial = gtsam.Values()
for i, pose in enumerate(poses):
    initial.insert(
        X(i),
        pose.retract(np.array([0.01, -0.01, 0.01, 0.02, 0.0, -0.01])),
    )
for j, point in enumerate(points):
    initial.insert(L(j), point + np.array([0.01, -0.02, 0.01]))
initial.insert(K(0), gtsam.Cal3_S2(510.0, 495.0, 0.0, 318.0, 242.0))

Configure and run the optimizer¶

SfmLevenbergMarquardtParams extends the usual LM parameters with a landmark-elimination mode. Here we select Schur elimination, ask CreateReducedOrdering() for the non-landmark ordering expected by that mode, and pass the parameters to SfmLevenbergMarquardtOptimizer. The final graph error confirms that the optimization improved the initial estimate.

params = gtsam.SfmLevenbergMarquardtParams.ceresDefaults()
params.setEliminationMode(gtsam.SfmEliminationMode.Schur)
params.setLinearSolver(
    gtsam.NonlinearOptimizerParams.LinearSolverType.MULTIFRONTAL_SOLVER
)
reduced_ordering = (
    gtsam.SfmLevenbergMarquardtOptimizer.CreateReducedOrdering(graph, initial)
)
complete_ordering = (
    gtsam.SfmLevenbergMarquardtOptimizer.CreateSchurOrdering(
        graph, reduced_ordering
    )
)
params.setOrdering(reduced_ordering)

initial_error = graph.error(initial)
optimizer = gtsam.SfmLevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
final_error = graph.error(result)

assert final_error < initial_error
assert complete_ordering.size() == initial.size()
print("reduced variables:", reduced_ordering.size())
print("error:", initial_error, "->", final_error)

reduced variables: 7
error: 845.7689790759512 -> 3.236514773600142e-27

When to use it¶

Use SfmLevenbergMarquardtOptimizer as a direct entry point for bundle adjustment when the graph contains explicit Point3 or Unit3 landmarks. Full mode solves the joint system; Schur mode removes those landmarks before solving the reduced camera system. Shared calibration remains in the reduced system.

SfmData can build standard SFM graphs, and GeneralSFMFactor explains the reprojection factor used in this example.

Source¶

SfmLevenbergMarquardt.h

AI assistance caveat¶

AI was used to help draft this documentation, and inaccuracies could be present.