Introduction

Apex Solver is a high-performance Rust library for nonlinear least squares optimization on Lie groups, aimed at SLAM, bundle adjustment and computer vision.

It bridges the gap between theoretical robotics and practical implementation:

• Manifold-aware optimization — full Lie group support (SO(2), SO(3), SE(2), SE(3), SE₂(3), SGal(3), Sim(3), ℝⁿ) with analytic Jacobians via the apex-manifolds crate.
• Three optimization algorithms — Levenberg–Marquardt (with Ceres-compatible Nielsen and Marquardt damping policies), Gauss–Newton and Dog Leg behind a unified Optimizer trait.
• Four sparse linear solvers — sparse Cholesky and QR, plus explicit and iterative (matrix-free PCG) Schur complement solvers for large-scale bundle adjustment.
• ~40 factors — pose, visual, IMU preintegration, LiDAR, GNSS, range and bearing, motion models and marginalization, each documented with its error and Jacobian in the Factor Reference.
• 15 robust loss functions — Huber, Cauchy, Tukey, Welsch, Barron and more, with correct Triggs correction of residuals and Jacobians.
• Zero-copy sparse linear algebra — parameters live in contiguous nalgebra storage viewed by faer without copying; unsafe is forbidden crate-wide.
• Ecosystem I/O — G2O, TORO and BAL file formats via the apex-io crate.
• Live visualization — integrated Rerun support for watching poses, landmarks, cost curves and sparsity patterns during a solve.

Where to go next

• Installation & Quick Start — solve your first pose graph.
• Problem Construction — variables, factors, keys and gauge freedom.
• Factor Reference — every factor’s error and Jacobian.
• Optimizers — choosing and tuning LM, GN and Dog Leg.
• Linear Solvers — Cholesky, QR and the Schur family.

Installation & Quick Start

Installation

[dependencies]
apex-solver = "1.5.0"

Optional feature for live Rerun visualization:

[dependencies]
apex-solver = { version = "1.5.0", features = ["visualization"] }

Solving a pose graph from a G2O file

use apex_solver::core::problem::Problem;
use apex_solver::factors::pose::BetweenFactor;
use apex_solver::{G2oLoader, JacobianMode, ManifoldType};
use apex_solver::optimizer::levenberg_marquardt::{LevenbergMarquardt, LevenbergMarquardtConfig};
use nalgebra::dvector;
use std::collections::HashMap;

fn main() -> Result<(), Box<dyn std::error::Error>> {
    // Load pose graph from G2O file
    let graph = G2oLoader::load("data/odometry/3d/sphere2500.g2o")?;

    // Create optimization problem
    let mut problem = Problem::new(JacobianMode::Sparse);
    let mut var_keys = HashMap::new();

    // Add SE3 poses as variables -- returns stable VarKey handles
    for (&id, vertex) in &graph.vertices_se3 {
        let quat = vertex.pose.rotation_quaternion();
        let trans = vertex.pose.translation();
        let se3_data = dvector![trans.x, trans.y, trans.z, quat.w, quat.i, quat.j, quat.k];
        let key = problem.add_variable(ManifoldType::SE3, se3_data);
        var_keys.insert(id, key);
    }

    // Add between factors (relative pose constraints) using VarKey handles
    for edge in &graph.edges_se3 {
        let k_from = var_keys[&edge.from];
        let k_to = var_keys[&edge.to];
        problem.add_residual_block(
            &[k_from, k_to],
            Box::new(BetweenFactor::new(edge.measurement.clone())),
            None,  // Optional: add HuberLoss for robustness
        );
    }

    // Configure and run optimizer
    let config = LevenbergMarquardtConfig::new()
        .with_max_iterations(100)
        .with_cost_tolerance(1e-6)
        .with_compute_covariances(true);  // Enable uncertainty estimation

    let mut solver = LevenbergMarquardt::with_config(config);
    let result = solver.optimize(&mut problem)?;

    println!("Status: {:?}", result.status);
    println!("Initial cost: {:.3e}", result.initial_cost);
    println!("Final cost: {:.3e}", result.final_cost);
    println!("Iterations: {}", result.iterations);

    Ok(())
}

Result:

Status: CostToleranceReached
Initial cost: 1.280e+05
Final cost: 2.130e+01
Iterations: 5

Solving a BAL bundle adjustment problem

The bundle_adjustment binary solves BAL datasets with the iterative Schur solver and self-calibration:

cargo run --release --bin bundle_adjustment -- \
    data/bundle_adjustment/trafalgar/problem-257-65132-pre.txt

Programmatically the same shape is: SE(3) pose variable per camera, Rn(3) variable per landmark, optional Rn(3) intrinsics per camera (mark_for_elimination on the point keys), ProjectionFactor with a HuberLoss. See Problem Construction next.

Problem Construction

Variables and keys

Variables are stored in a slotmap-backed arena. add_variable returns a stable, generational VarKey; keep the key and pass it wherever a factor needs the variable. There are no string lookups on the hot path.

#![allow(unused)]
fn main() {
use apex_solver::{ManifoldType};
use nalgebra::dvector;

let pose = problem.add_variable(ManifoldType::SE3,
    dvector![tx, ty, tz, qw, qx, qy, qz]);
let point = problem.add_variable(ManifoldType::RN, dvector![x, y, z]);
}

Supported manifold types: SO2, SO3, SE2, SE3, SE23, SGal3, Sim3, RN. Parameters are stored in the manifold’s representation size (e.g. 7 doubles for SE(3)); optimization happens in the tangent space (6 DOF for SE(3)) via a right-perturbation retraction.

That distinction runs through everything downstream: Jacobians have tangent width, the step is applied with $⊞$ rather than addition, and the linear system is built in tangent coordinates. See From Factor Graph to Linear System.

Residual blocks

A residual block binds one factor to the variables it reads:

#![allow(unused)]
fn main() {
problem.add_residual_block(&[k_from, k_to], Box::new(between_factor), loss);
}

• The factor implements Factor::linearize(params, residual, jacobian) — see Factors.
• loss is an optional Box<dyn LossFunction>; see Robust Loss Functions.
• To weight the block by its measurement uncertainty, use add_residual_block_with_noise and pass a NoiseModel. Without one the block is unweighted, which is only correct when every residual in the graph is already in comparable units.
• Use try_add_residual_block for a Result-returning registration that runs the factor’s validate_variables hook — shape mismatches are caught at registration time instead of during parallel evaluation.

Gauge freedom

A pose graph is unconstrained up to a global rigid transform. Anchor it by fixing DOFs of one pose:

#![allow(unused)]
fn main() {
for dof in 0..6 {
    problem.fix_variable(pose_keys[0], dof);
}
}

Alternatively, register a PriorFactor on the first pose.

Schur landmarks

Schur-complement solvers eliminate landmark blocks. Mark the variables to eliminate:

#![allow(unused)]
fn main() {
problem.mark_for_elimination(point_key);
}

Every Schur solver (ExplicitSparseSchur, ExplicitDenseSchur, ImplicitSparseSchur) can also classify automatically from manifold type and size when the ordering is explicitly opted in:

#![allow(unused)]
fn main() {
use apex_solver::linalg::SchurOrdering;

let ordering = SchurOrdering::default().with_auto_detect(true);
}

Auto-detection is off by default: Rn(3) is ambiguous between 3-D landmarks and self-calibration intrinsics ([focal, k1, k2]), and eliminating intrinsics as landmarks silently corrupts the Schur complement.

What elimination buys, and which Schur solver to pick, is in Linear Solvers & the Schur Complement.

Factor Reference

A factor is one measurement’s contribution to the objective: given the variables it connects, it produces a residual vector and its Jacobian. Apex Solver ships about forty of them, grouped by the sensor they model.

$$\underset{\mathbf{x}}{\min }\frac{1}{2}\sum _i\Vert {\mathbf{r}}_i({\mathbf{x}}_i){\Vert }_{{\Sigma }_i}^2$$

The contract

#![allow(unused)]
fn main() {
pub trait Factor: Send + Sync {
    fn linearize(
        &self,
        params: &[&[f64]],
        residual: &mut [f64],
        jacobian: Option<faer::mat::MatMut<'_, f64>>,
    );
    fn residual_dim(&self) -> usize;
    fn jacobian_shape(&self) -> (usize, usize);
    fn validate_variables(&self, variables: &[&dyn ManifoldVariable]) -> Result<(), String> { Ok(()) }
    fn whitens_internally(&self) -> bool { false }
}
}

linearize writes into caller-owned buffers and allocates nothing — it runs once per factor per iteration, in parallel across disjoint buffers. params arrives as one zero-copy &[f64] slice per connected variable, in the order the block was registered. jacobian is column-major with shape jacobian_shape(), whose columns are the minimal (tangent) dimensions, not the parameter dimensions: an SE3 block is 7 parameters but 6 columns.

Four conventions worth knowing

Poses are body-in-world. T_wb places the body in the world, so pose.translation() is the body’s world position and a body-frame direction is $R^{\top }(\mathbf{p}-\mathbf{t})$. Some camera factors take a world-to-camera pose instead; each says so.

Jacobians are with respect to the right perturbation. For a Lie group variable the solver updates $X\leftarrow X⊞\delta =X\cdot \mathrm{Exp}(\delta )$, so every Jacobian column is $\partial \mathbf{r}/\partial \delta$ at $\delta =0$. Take these derivatives from apex-manifolds — every group reports the Jacobians of its own operations (act, compose, log, right_plus, right_minus) through optional output arguments. Re-deriving a block such as $[R\mid -R[\mathbf{p}{]}_{\times }]$ by hand creates a second copy of a convention that can silently drift from the group’s.

A factor either whitens internally or takes a NoiseModel — never both. Most return the raw residual and let the noise model supplied at registration whiten it. A few cannot, because their weighting is bound up with the measurement itself; they report whitens_internally() == true and must be registered with NoiseModel::null(). Registering one with a noise model is rejected rather than silently double-weighted.

validate_variables is the real shape check. [profile.test] inherits release, so the debug_assert!s factors also carry are compiled out of cargo test. Every factor implements this hook, so a mismatched registration is a typed error instead of a panic inside the parallel assembly.

Writing your own

Implement the trait, then test it in this order — the order matters:

1. A hand-computed residual. Pick a configuration whose answer you can work out on paper and assert it. This is the only test that pins your convention; everything below is self-referential without it.
2. Finite differences on the Jacobian, away from the solution. Perturb through the manifold’s right_plus, not the raw parameters. Evaluate at a point where the residual is not near zero: residual comparisons pass through $J_r^{-1}$, which tends to the identity as the residual tends to zero, so a Jacobian checked only at the truth can be badly wrong and still pass.
3. A solved graph. tests/factor_coverage.rs has one scenario per factor, and a guard test that reads these modules’ pub use lines and fails if an exported factor has no scenario.

Step 1 is not optional. A finite-difference test compares a Jacobian against its own residual, so a residual that is self-consistently wrong passes it — which is exactly how a BearingRangeFactor that rotated the wrong way, and a GpsAsyncFactor missing gravity, both survived in this repo.

Index

Factors are addressed by their module path — factors::visual::StereoFactor, factors::imu::se23::ImuFactor — so an import says which sensor a measurement came from. apex_solver::prelude re-exports only the handful nearly every program needs.

Noise Models

A factor produces a raw residual in measurement units — metres, pixels, radians. A noise model converts it into a statistically comparable quantity so that a 2 cm range error and a 0.5 px reprojection error can be summed in one objective. This is the piece that makes the least-squares problem a maximum likelihood estimate rather than an arbitrary weighted sum.

What it stores

NoiseModel (core::noise) holds the square-root information, not the covariance. Writing $\Omega ={\Sigma }^{-1}$ for the information matrix and $\Omega =S^{\mathsf{T}}S$ for its factor, whitening is

$$\stackrel{=Sr,J}{r}=SJ$$

so that

$$\Vert \tilde{r}{\Vert }^2=r^{\mathsf{T}}\Omega r$$

which is the Mahalanobis distance. Storing $S$ rather than $\Sigma$ means whitening is a multiply, with no factorization on the hot path.

Three representations, chosen by what the measurement actually needs:

Diagonal covers the block-diagonal $\Omega$ that most SLAM graphs use. Reach for Dense only when residual components are genuinely correlated.

Constructing one

#![allow(unused)]
fn main() {
use apex_solver::core::noise::NoiseModel;

// From standard deviations — the common case. sqrt_info_i = 1/σ_i.
let noise = NoiseModel::from_sigmas(&[0.05, 0.05, 0.02])?;

// From square-root information directly (1/σ per row).
let noise = NoiseModel::from_diagonal_sqrt_info(&[20.0, 20.0, 50.0])?;

// From a full information matrix Ω (symmetric PSD).
let noise = NoiseModel::from_information(omega)?;

// From an already-factored S.
let noise = NoiseModel::from_sqrt_info(s)?;

// Unweighted.
let noise = NoiseModel::null();
}

Register it with the block:

#![allow(unused)]
fn main() {
problem.add_residual_block_with_noise(&[key_a, key_b], factor, loss, noise);
}

Rank-deficient and indefinite information

Real datasets carry information matrices that are not positive definite. A g2o file may encode zero roll/pitch information for a planar robot, giving a rank-deficient $\Omega$; floating-point round-trips can leave slightly negative eigenvalues.

from_information handles this by clamping negative eigenvalues to zero — the nearest-PSD projection — and taking the symmetric square root $S=V\sqrt{{\Lambda }^{+}}{V}^{\mathsf{T}}$, which satisfies $S^{\mathsf{T}}S=\Omega$. Clamped directions whiten to zero, i.e. the measurement stops constraining them.

That clamp is silent, and on a genuinely ill-formed $\Omega$ it deletes real constraints rather than empty ones. When the difference matters, use the reporting form:

#![allow(unused)]
fn main() {
let (noise, repair) = NoiseModel::from_information_reporting(omega)?;
if repair.is_material() {
    // `repair.clamped_directions`, `repair.min_eigenvalue` and the scale it is
    // judged against tell you whether this was floating-point noise or a
    // measurement whose Ω is wrong.
}
}

InformationRepair reports how many directions were clamped, the smallest eigenvalue before clamping, and the largest eigenvalue it is judged against — relative_indefiniteness is the ratio that distinguishes round-off from a broken measurement.

RepairStrategy then decides what to do with a materially-repaired edge:

This is not hypothetical: the cubicle pose-graph dataset ships indefinite $\Omega$ on some edges, and switching those to unit weight moves the unweighted cost by a factor of ~7. Whichever you choose, say which objective you are reporting.

Whiten internally, or take a noise model — never both

Most factors return a raw residual and let the registered NoiseModel whiten it. Some cannot: their weighting is inseparable from the measurement — GICP’s combined point covariance, a smart factor’s internal elimination, an IMU preintegration’s propagated information matrix. Those whiten as they go and report Factor::whitens_internally() == true.

Pairing an internally-whitened factor with a non-null noise model whitens twice and silently over-weights the block. The two conventions are indistinguishable by inspecting a residual, so registration rejects the combination outright rather than letting it through:

#![allow(unused)]
fn main() {
// Rejected at registration — the factor already applied its information.
problem.try_add_residual_block_with_noise(&keys, gicp_factor, None, dense_noise)?;

// Correct.
problem.try_add_residual_block_with_noise(&keys, gicp_factor, None, NoiseModel::null())?;
}

Where whitening happens in the pipeline

Whitening runs after the factor linearizes and before the robust loss — see From Factor Graph to Linear System. The ordering is not incidental: a robust loss decides whether a residual is an outlier, and that is a question about standard deviations, not raw units. A 3 px reprojection error is an outlier only relative to the measurement’s σ.

Choosing σ

The noise model is a modelling decision, not a tuning knob, and getting it wrong biases the estimate rather than just slowing convergence:

• Too small σ (over-confident) — the factor dominates and pulls the solution toward that measurement, over-riding better-informed ones.
• Too large σ (under-confident) — the factor is effectively ignored.
• Relative scale is what matters. Multiplying every σ in the graph by a constant leaves the optimum unchanged; it only rescales the cost. What changes the answer is the ratio between different sensors’ models.

For a robust loss to work as intended, σ should reflect the inlier distribution — the loss then handles the tail. Inflating σ to “cover” outliers and also applying a robust loss down-weights inliers twice.

Robust Loss Functions

Outliers dominate the ordinary least-squares objective. Apex Solver ships 15 robust kernels — Huber, Cauchy, Tukey, Welsch, Fair, GemanMcClure, Andrews, Trimmed, L1, Lp, Barron variants and more — all in apex_solver::core::loss_functions.

Usage

#![allow(unused)]
fn main() {
use apex_solver::core::loss_functions::HuberLoss;

problem.add_residual_block(
    &[k_pose, k_landmark],
    Box::new(projection_factor),
    Some(Box::new(HuberLoss::new(1.345)?)),
);
}

What the solver actually computes

For a squared residual norm $s=\Vert \mathbf{r}{\Vert }^2$, the robust objective is

$$F(\mathbf{x})=\frac{1}{2}\sum _i{\rho }_i\left(s_i(\mathbf{x})\right)$$

Minimizing $F$ is equivalent to an iteratively reweighted least-squares problem whose residual and Jacobian are

$$\stackrel{=\rho \prime (s)r,J}{\mathbf{r}}=\sqrt{{\rho }^{\prime }(s)}\left(\mathbf{J}-\frac{{\rho }^{\prime \prime }(s)}{2{\rho }^{\prime }(s)}\alpha \mathbf{r}{\mathbf{r}}^{\top }\mathbf{J}\right),$$

with $\alpha$ from the Triggs correction. Apex Solver applies exactly this correction in-place during block evaluation, and reports the cost as $\frac{1}{2}\rho (s)$ — not as the norm of the corrected residual. The two are different functions; conflating them silently corrupts every reported cost and trust-region ratio on robust problems.

From Factor Graph to Linear System

This chapter follows one optimizer iteration end to end: how the factor graph is turned into J and r, how each factor’s contribution is whitened, robustified and scattered into the global system, and how the solved step gets back onto the manifold. It describes what the code in core/ and linearizer/ actually does.

The graph

A Problem holds two arenas (core::problem):

• Variables — the unknowns, each a manifold element (SE3, SO3, Rn, …), stored in a SlotMap and addressed by a stable VarKey.
• Residual blocks — the factors, each a ResidualBlock holding one boxed Factor, the VarKeys it connects, an optional NoiseModel, and an optional robust LossFunction.

This is the factor graph: variables are nodes, residual blocks are the (hyper-)edges connecting them. There is no separate graph structure.

#![allow(unused)]
fn main() {
let pose_i = problem.add_variable(ManifoldType::SE3, pose_i_params);
let pose_j = problem.add_variable(ManifoldType::SE3, pose_j_params);

problem.add_residual_block(
    &[pose_i, pose_j],           // the edge's endpoints
    Box::new(BetweenFactor::new(measurement)),
    Some(Box::new(HuberLoss::new(1.0)?)),
);
}

Each block is assigned a residual_row_start_idx — its offset in the stacked residual vector — and each variable a column offset in the tangent space. Those two offsets are what turn a graph into a matrix.

What the objective actually is

Stacking every block’s residual gives $r(x)$, and the problem is

$$\underset{x}{\min }f(x)=\frac{1}{2}\sum _i{\rho }_i\left(\Vert {\tilde{r}}_i(x){\Vert }^2\right)$$

where ${\tilde{r}}_i$ is the whitened residual of block $i$ and ${\rho }_i$ its robust loss (identity when none is set). The three sections below are exactly the three transformations between “what the factor computes” and “what the solver sees”.

Step 1 — the factor linearizes itself

Every factor implements one method (factors::Factor):

#![allow(unused)]
fn main() {
fn linearize(
    &self,
    params: &[&[f64]],                         // one slice per connected variable
    residual: &mut [f64],                      // output, length residual_dim()
    jacobian: Option<faer::mat::MatMut<'_, f64>>, // output, jacobian_shape()
);
}

Given the current parameter values, it writes

$$r_i=h_i(x)\ominus z_i\text{and}{J}_i={\frac{\partial r_i}{\partial \delta }|}_{\delta =0}$$

where $h_i$ is the predicted measurement, $z_i$ the observation, and $\ominus$ whatever difference the measurement space requires — subtraction for a Euclidean measurement, between + log for a pose measurement.

The Jacobian is with respect to the tangent space at the current estimate, not the parameter vector. For an SE3 variable stored as 7 parameters the block has 6 columns, and it is the derivative of the residual with respect to a local perturbation $\delta$ applied as $x⊞\delta$. This is what makes the linear system well-posed on a manifold: the 7-parameter quaternion representation is over-parameterised and would give a singular $J^{\mathsf{T}}J$.

Factors must obtain these derivatives from the manifold, not re-derive them. Every group in apex-manifolds reports the Jacobians of its own operations (act, compose, log, right_plus, between) through optional output arguments. A hand-written [R | −R[p]ₓ] is a second copy of a convention that can silently drift from the group’s — factors::common deliberately contains no manifold derivatives for this reason.

The buffers are pre-allocated and reused across iterations; linearize never allocates.

Step 2 — whitening by the noise model

A measurement with covariance $\Sigma$ must be weighted by its information. The NoiseModel stores the square-root information ${\Sigma }^{-1/2}$ and applies it (core::noise):

$${\stackrel{i=\Sigma i-1/2ri,J}{r}}_i={\Sigma }_i^{-1/2}{J}_i$$

so that $\Vert {\tilde{r}}_i{\Vert }^2=r_i^{\mathsf{T}}{\Sigma }_i^{-1}{r}_i$ — the Mahalanobis distance. A Diagonal model is a per-row scale; a Dense model is a triangular multiply.

Whitening happens before the robust loss, in both the block-evaluation path and the assembly path, and the two are documented to keep the same ordering. The loss must see the whitened norm, because “is this residual an outlier?” is a question about standard deviations, not raw units.

A factor either whitens internally or takes a NoiseModel — never both. Some factors’ weighting is inseparable from the measurement (GICP’s combined point covariance, a smart factor’s internal elimination, an IMU preintegration’s information matrix), so they whiten as they go and report whitens_internally() == true. Registering one of those with a non-null noise model would whiten twice and silently over-weight the block; the two conventions are indistinguishable by inspecting a residual, so registration rejects the combination outright.

Step 3 — the robust loss becomes a reweighting

A robust loss $\rho$ turns the problem into a different least-squares problem rather than changing the solver. core::corrector implements Ceres’s corrector: given $s=\Vert \tilde{r}{\Vert }^2$ and the loss derivatives ${\rho }^{\prime }(s),{\rho }^{\prime \prime }(s)$,

$$\sqrt{{\rho }_1}=\sqrt{{\rho }^{\prime }(s)},{\alpha }^2=\frac{{\rho }^{\prime \prime }(s)}{{\rho }^{\prime }(s)}$$

$$\hat{r}=\sqrt{{\rho }_1}\stackrel{,J^=\rho 1J}{r}+\Vert {\stackrel{\parallel \alpha (J}{r}}^{\mathsf{T}}{\stackrel{)r}{r}}^{\mathsf{T}}$$

so that $\Vert \hat{r}{\Vert }^2\approx \rho (\Vert \tilde{r}{\Vert }^2)$ and the gradient is correct. The rank-one term is what makes this more than naive iteratively-reweighted least squares: it carries the curvature of $\rho$, so the Gauss-Newton model of the robustified problem stays accurate.

Ordering matters and is load-bearing: the Jacobian correction reads the un-corrected residual, so correct_jacobian_in_place runs before correct_residual_in_place. Reversing them silently changes the rank-one term.

After this step the solver sees an ordinary least-squares problem in $\hat{r},\hat{J}$ — every optimizer and linear solver downstream is unaware that a robust loss was involved at all.

Step 4 — scattering into the global system

Each block has produced a small dense $\text{rows}\times \text{cols}$ Jacobian, where cols is the sum of its variables’ DOFs. Assembly places those numbers into the global $J$ (linearizer::cpu):

• block $i$’s rows go at its residual_row_start_idx,
• the sub-block for variable $v$ goes at $v$’s column offset.

The result is block-sparse: a row touches only the variables its factor connects, which is precisely the graph’s adjacency.

The sparsity pattern is symbolic and built once. SymbolicStructure computes the pattern and a value permutation before the iteration loop, and every subsequent iteration only refills values in that fixed layout — the pattern is a property of the graph, not of the estimate. Recomputing it per iteration is the single most common way to make a solver slow.

Assembly is parallelised over blocks with rayon into disjoint row ranges, so no two threads write the same row. This is why the visiting order is load-bearing: values are pushed positionally against the symbolic pattern’s pairs, so any disagreement between the two orders would permute $J$’s rows against $r$ — a wrong system that still factorizes cleanly.

Fixed variables (gauge freedom)

A pose-graph or BA problem is invariant under a global transformation, so $J^{\mathsf{T}}J$ is singular unless something is pinned. fix_variable holds individual tangent components fixed:

#![allow(unused)]
fn main() {
for dof in 0..6 {
    problem.fix_variable(first_pose, dof);   // anchor the gauge
}
}

Fixed components are excluded from the column space, so they never enter the linear system and cannot move.

Step 5 — solve, and return to the manifold

The optimizer now has $\hat{J}$ and $\hat{r}$, and forms

$$g={\hat{J}}^{\mathsf{T}}\hat{r},H\approx {\hat{J}}^{\mathsf{T}}\hat{J}$$

Which linear system gets solved is the optimizer’s business (Optimizers) and how it is solved is the backend’s (Linear Solvers). Both produce a tangent-space step $h$.

The step is not added to the parameters. It is applied through the manifold’s right-plus:

$$x\leftarrow x⊞h=x\cdot \exp (h)$$

for each variable’s own group, using the same right convention the Jacobians were taken with. Mixing conventions here — taking a left-Jacobian and applying a right update — produces a step that is wrong by a factor of the adjoint, and it still converges, just slowly and to a slightly different place.

The loop

build symbolic structure          once, from the graph's adjacency
─── per iteration ────────────────────────────────────────────────
  for each residual block (parallel):
      factor.linearize(params) ──────────► r_i, J_i     tangent-space
      noise.whiten(...)        ──────────► r̃_i, J̃_i     Mahalanobis
      corrector.correct(...)   ──────────► r̂_i, Ĵ_i     robustified
      scatter into J, r at (row_start, col_offsets)
  ─────────────────────────────────────────────────────────────────
  solve for h   (Gauss-Newton / LM / Dog Leg × linear backend)
  x ← x ⊞ h     per-variable right-plus
  check convergence

Every iteration repeats steps 1–5 at the new estimate: the residuals, the Jacobians and the robust weights are all recomputed, while the sparsity pattern and every cached symbolic structure are reused.

Why this is efficient

• Symbolic once, numeric many. The pattern of $J$, of $J^{\mathsf{T}}J$, and the permutation linking them are computed once and reused, so an iteration is a parallel value gather plus a parallel product.
• Blocks are independent. Linearization is embarrassingly parallel; only the scatter needs disjoint ranges, which the row layout guarantees.
• No allocation in the hot path. Factor buffers, the assembly arena and the solver workspaces are allocated once and reused.
• Manifold parameters stay contiguous. They live in nalgebra storage that faer views without copying, so no conversion happens between assembly and the linear solve.
• Structure is exploited, not discovered. The Schur partition, the elimination groups and the chunk layout all come from the graph’s adjacency, which is known before the first iteration.

Optimizers

This chapter documents what the three optimizers in src/optimizer/ actually do — the equations as implemented, the defaults as coded, and the deviations from textbook forms with the reason the code gives for each. Every formula names the module it comes from.

The problem all three solve

Given residual blocks $r_i$ over manifold variables, Problem defines

$$\underset{x}{\min }f(x)=\frac{1}{2}\Vert r(x){\Vert }_2^2=\frac{1}{2}\sum _i\Vert r_i(x){\Vert }^2$$

Variables live on manifolds, so the step is applied with $⊞$ rather than addition: $x\leftarrow x⊞h$, with the per-group right-plus from apex-manifolds. The Jacobian $J$ is therefore taken with respect to the tangent space, and every matrix below has tangent dimension, not parameter dimension. An SE3 variable contributes 6 columns, not 7.

Two quantities recur:

$$g=J^{\mathsf{T}}r\text{(gradient)},H\approx J^{\mathsf{T}}J\text{(Gauss-Newton Hessian)}$$

H drops the ${\sum }_i{r}_i{\nabla }^2{r}_i$ term of the true Hessian, which is the Gauss-Newton approximation and is accurate when residuals are small or nearly linear (gauss_newton.rs).

Sign convention. LinearSolver::get_gradient returns $+J^{\mathsf{T}}r$, not the right-hand side $-J^{\mathsf{T}}r$ (linalg/mod.rs). Each optimizer negates it before forming the linear system. tests/linear_solver_contract.rs pins this for every backend, because getting it backwards produces a step that ascends.

Gauss-Newton

optimizer/gauss_newton.rs. Solves the normal equations exactly and takes the full step, with no step-size control at all (gauss_newton.rs):

$$(J^{\mathsf{T}}J)h=-J^{\mathsf{T}}r,x\leftarrow x⊞h$$

There is no damping, no trust region, and no line search. Convergence is quadratic near a solution where the Gauss-Newton approximation holds, and the method diverges when started far away or when $J^{\mathsf{T}}J$ is ill-conditioned. Use it when you already know the problem is well-conditioned and the initial guess is close; otherwise use LM.

Options

The schur_* fields (schur_variant, schur_preconditioner, schur_cg_max_iterations, schur_cg_tolerance, schur_cg_q_tolerance) exist on all three configs and are forwarded to the linear solver — see Linear Solvers.

Levenberg–Marquardt

optimizer/levenberg_marquardt.rs. The default optimizer, and the one for_bundle_adjustment() presets.

The damped system

$$(J^{\mathsf{T}}J+\lambda D)h=-J^{\mathsf{T}}r$$

D is not the identity. It is the clamped diagonal of the Hessian (linalg/mod.rs):

$$D_{jj}=\mathrm{clamp}((J^{\mathsf{T}}J{)}_{jj},d_{\min },d_{\max }),d_{\min }=10^{-6},d_{\max }=10^{32}$$

This is Ceres’s LevenbergMarquardtStrategy: damping each column in proportion to its own curvature makes the step invariant to a rescaling of the parameters, which uniform $\lambda I$ is not. Damping::identity sets $d_{\min }=d_{\max }=1$ and recovers plain $\lambda I$ exactly.

The lower clamp matters: without it a column with no curvature would receive no damping at all, leaving that direction unconstrained.

Step quality

After solving, the step is scored by the gain ratio. Predicted reduction comes from the quadratic model (optimizer/mod.rs):

$${\Delta }_{\text{pred}}=-h^{\mathsf{T}}g-\frac{1}{2}{h}^{\mathsf{T}}Hh$$

evaluated with the un-damped $H$ — hessian_vec_product is documented to never carry $\lambda D$, because damping in the model would corrupt $\rho$ (linalg/mod.rs). Then (optimizer/mod.rs):

$$\rho =\frac{f(x)-f(x⊞h)}{{\Delta }_{\text{pred}}}$$

with two guards the textbook ratio does not have:

• ${\Delta }_{\text{pred}}<-10^{-15}$ → $\rho =-1$. A negative predicted reduction means the model says the step increases cost, so it is rejected outright rather than producing a misleading positive ratio.
• $|{\Delta }_{\text{pred}}|\le 10^{-15}$ → $\rho =1$ if the actual reduction was positive, else $0$.

The step is accepted when $\rho >$min_relative_decrease, default 1e-3 (levenberg_marquardt.rs), matching Ceres. This is not the textbook $\rho >0$.

Damping update — two rules

Selected by damping_update (levenberg_marquardt.rs).

Nielsen (default, what Ceres uses). On an accepted step:

$$\lambda \leftarrow \lambda \cdot \max \left(\frac{1}{3},1-(2\rho -1{)}^3\right),\nu \leftarrow {\nu }_0$$

On a rejected step $\lambda \leftarrow \lambda \nu$ and $\nu \leftarrow 2\nu$, so consecutive failures escalate geometrically. Reads damping_nu; ignores damping_increase_factor, damping_decrease_factor, min_step_quality, good_step_quality.

Marquardt. Three bands:

$$\lambda \leftarrow \{\begin{array}{ll}\lambda \cdot \text{decrease}& \rho \ge \text{good\_step\_quality}\\ & \\ \lambda \cdot \text{increase}& \rho \le \text{min\_step\_quality}\text{or rejected}\\ & \\ \lambda & \text{otherwise}\end{array}$$

A rejected step always increases $\lambda$ whatever $\rho$ was — the code notes that leaving it unchanged would recompute the identical step next iteration and stall (levenberg_marquardt.rs). Afterwards $\lambda$ is clamped to [damping_min, damping_max].

Options

Note the scaling default differs between optimizers: LM defaults to false, Dog Leg to true. Enable it for LM on mixed-scale problems solved with Cholesky or QR.

for_bundle_adjustment() preset

levenberg_marquardt.rs overrides: ImplicitSparseSchur linear solver, SchurJacobi preconditioner, λ₀ = 1e-3 (the plain default is 1e-4), max_iterations = 20, cost_tolerance = 1e-6, parameter_tolerance = 1e-8, gradient_tolerance = 1e-10. The solver choice is justified by the measured table in that comment; see Benchmarks.

Dog Leg

optimizer/dog_leg.rs. Powell’s dog leg over a spherical trust region of radius $\Delta$, interpolating between the steepest-descent and Gauss-Newton directions.

Cauchy point

The unconstrained minimiser along $-g$ (dog_leg.rs):

$$\alpha =\frac{g^{\mathsf{T}}g}{g^{\mathsf{T}}Hg},p_c=-\alpha g$$

with a guard: if $|g^{\mathsf{T}}Hg|\le 10^{-15}$ then $\alpha =1$.

Step selection — three cases

From dog_leg.rs, given $\Delta$:

Case 1 — GN step fits. If $\Vert h_{gn}\Vert \le \Delta$, take $h=h_{gn}$ (StepType::GaussNewton).

Case 2 — Cauchy point already outside. If $\Vert p_c\Vert \ge \Delta$, take the steepest-descent direction scaled to the boundary (StepType::SteepestDescent):

$$h=\frac{\Delta }{\Vert d_{sd}\Vert }{d}_{sd}$$

Case 3 — interpolate. Otherwise walk from $p_c$ toward $h_{gn}$ to the boundary. With $v=h_{gn}-p_c$, solve $\Vert p_c+\beta v{\Vert }^2={\Delta }^2$, i.e. $a{\beta }^2+2b\beta +c=0$ with

$$a=v^{\mathsf{T}}v,b=p_c^{\mathsf{T}}v,c=\Vert p_c{\Vert }^2-{\Delta }^2$$

The implementation does not use the textbook quadratic root. It uses Ceres’s cancellation-avoiding branch (dog_leg.rs), with $d^2=b^2-ac$:

$$\beta =\{\begin{array}{ll}\frac{-b+d}{a}& b\le 0\\ & \\ \frac{-c}{b+d}& b>0\end{array}$$

Choosing by the sign of $b$ avoids catastrophic cancellation in the subtraction. Fallbacks: $\beta =1$ when $d^2<0$ (geometrically impossible, handled anyway) or $|a|<10^{-15}$ (degenerate $v$). Finally $\beta$ is clamped to $[0,1]$ and $h=p_c+\beta v$ (StepType::DogLeg).

Trust-region update

dog_leg.rs. Growth is Ceres’s rule, not a fixed doubling:

$$\Delta \leftarrow \min ({\Delta }_{\max },\max (\Delta ,3\Vert h\Vert ))\text{when }\rho >\text{good\_step\_quality}$$

$$\Delta \leftarrow \max ({\Delta }_{\min },\frac{1}{2}\Delta )\text{when }\rho <\text{poor\_step\_quality}$$

and $\Delta$ is left unchanged in between.

$\Vert h\Vert$ is measured in the scaled space when Jacobi scaling is on — which is Dog Leg’s default. The code documents why: the radius bounds the scaled step, so feeding the un-scaled norm would compare two different units and grow $\Delta$ by an arbitrary factor (dog_leg.rs).

Step reuse

On a rejected step Dog Leg caches the GN step, Cauchy point, gradient and $\alpha$, and reuses them next iteration (dog_leg.rs). Only $\Delta$ changed, so the expensive linear solve does not have to be redone. The cache is invalidated on any accepted step, because the parameters moved. Controlled by enable_step_reuse (default true).

Options

Jacobi column scaling

When use_jacobi_scaling is on, columns are equilibrated before the solve (optimizer/mod.rs):

$$S_{jj}=\frac{1}{1+\Vert J_{:,j}{\Vert }_2},\tilde{J}=JS$$

The solve happens in the scaled space and the step is mapped back with $h=S\tilde{h}$. The $1+$ in the denominator keeps the scaling finite for a structurally empty column.

Ordering matters and the code is explicit about it: the predicted reduction is computed before un-scaling (levenberg_marquardt.rs), because the solver’s cached gradient and Hessian are the scaled ones and all three vectors must live in the same space. The predicted reduction is a value of the quadratic model and is invariant under the change of variables, so it is equally valid for the un-scaled step.

Convergence criteria

All three optimizers share check_convergence (optimizer/mod.rs), evaluated in this order.

Always checked, before anything else:

1. InvalidNumericalValues — any of new cost, step norm or gradient norm is NaN or infinite.
2. Timeout — elapsed ≥ timeout.
3. MaxIterationsReached — iteration ≥ max_iterations.

Only after an accepted step (a rejected step cannot terminate the solve):

4. GradientToleranceReached — $\Vert g\Vert <{\epsilon }_g$.
5. From iteration 1 onward, ParameterToleranceReached: $\Vert h\Vert \le {\epsilon }_x(\Vert x\Vert +{\epsilon }_x)$ — a relative test, so it scales with the parameter magnitude.
6. From iteration 1 onward, CostToleranceReached: $|f_k-f_{k+1}|/\max (f_k,10^{-10})<{\epsilon }_f$.
7. MinCostThresholdReached — optional $f<\tau$ cutoff.
8. TrustRegionRadiusTooSmall — LM and Dog Leg only.

LM additionally reports StalledNoProgress after max_consecutive_rejected_steps consecutive rejections (levenberg_marquardt.rs): damping has grown until every trial step is negligible, so the remaining budget cannot change the cost.

Output

All three return SolverResult<T> (optimizer/mod.rs):

Note that several statuses are successful terminations, not failures: Converged, CostToleranceReached, GradientToleranceReached, ParameterToleranceReached and StalledNoProgress are all treated as converged by the benchmark harness. MaxIterationsReached means the budget ran out with the solve still improving.

Linear Solvers

Every optimizer iteration reduces to one linear solve. This chapter documents the backends in src/linalg/: what each one computes, the equations as implemented, and how to configure them. Formulas name the module they come from.

Two traits define the contract (linalg/mod.rs):

get_hessian() returning None is not a failure. A backend that eliminates straight from $J$ never materialises $J^{\mathsf{T}}J$, and callers must degrade to hessian_vec_product rather than error — that is what makes the matrix-free paths expressible.

JacobianMode picks the assembly: Sparse (default) or Dense (below ~500 DOF). LinearSolverType then picks the algorithm within that mode, and the optimizer rejects mismatched combinations (levenberg_marquardt.rs).

Normal equations

For the non-Schur backends, $H=J^{\mathsf{T}}J$ and $g=J^{\mathsf{T}}r$ are formed once per iteration (sparse/normal_eq.rs). The sparsity of $J^{\mathsf{T}}$, of $H$, and the value permutation linking them are cached across iterations, so each evaluation is a parallel value gather plus faer’s parallel sparse product. The cache is keyed on $J$’s sparsity, which must not change between iterations.

Sparse Cholesky

SparseCholeskySolver — $L{L}^{\mathsf{T}}$ factorization of $H$ (or $H+\lambda D$). Symbolic factorization cached, numeric pass repeated. The default for pose graphs.

Sparse QR

SparseQRSolver — factorizes $J$ directly rather than forming $H$, which squares the condition number. More robust on ill-conditioned problems, at a higher per-iteration cost.

Dense Cholesky / QR

DenseCholeskySolver, DenseQRSolver — for JacobianMode::Dense. Both route their SPD solves through one shared solve_spd helper (dense/cholesky.rs) so the factorization and error mapping exist in exactly one place.

The Schur complement

Why

In bundle adjustment most variables are landmarks. Ladybug has 1,723 cameras and 156,502 landmarks: 15,507 retained DOF against 469,506 eliminated. Landmarks are mutually unconnected — no factor touches two of them — so the landmark block of the Hessian is block-diagonal and can be eliminated cheaply, leaving a system in the cameras alone.

The algebra

Partition the variables into an eliminated set (e, Ceres’s “group 0”) and a retained set (k, “group 1”). The system becomes (linalg/schur/partition.rs):

$$\left[\begin{array}{cc}{H}_{kk}& H_{ke}\\ & \\ H_{ke}^{\mathsf{T}}& H_{ee}\end{array}\right]\left[\begin{array}{c}{\delta }_k\\ \\ {\delta }_e\end{array}\right]=\left[\begin{array}{c}{g}_k\\ \\ g_e\end{array}\right]$$

Eliminating ${\delta }_e$ gives the reduced system:

$$\underset{S}{\underbrace{(H_{kk}-H_{ke}{H}_{ee}^{-1}{H}_{ke}^{\mathsf{T}})}}{\delta }_k=g_k-H_{ke}{H}_{ee}^{-1}{g}_e$$

and back-substitution recovers the eliminated half:

$${\delta }_e=H_{ee}^{-1}(g_e-H_{ke}^{\mathsf{T}}{\delta }_k)$$

Nothing here mentions cameras or points — the partition is agnostic to what the variables mean, which is why the same code covers classic BA, inverse-depth parameterisations, LiDAR features and marginalization.

The precondition, and how it is enforced

The elimination is exact only when $H_{ee}$ is block-diagonal, i.e. the eliminated variables are mutually unconnected. Violating it yields a wrong step with no other symptom, so it is checked rather than assumed:

• Solvers that form $H$ call SchurPartition::verify_block_diagonal (partition.rs) — one pass over the eliminated columns’ nonzeros.
• The matrix-free path never forms $H$, so it checks against $J$’s rows instead: a residual row touching two eliminated variables is the same violation (sparse/schur/implicit.rs, ensure_structure).

Both report a typed error naming the two offending variables.

Damping under Schur

$\lambda D$ is applied to both blocks before elimination — damping the reduced system instead would not be the same problem (levenberg_marquardt.rs, and explicit.rs):

$$S=(H_{kk}+\lambda D_k)-H_{ke}(H_{ee}+\lambda D_e{)}^{-1}{H}_{ke}^{\mathsf{T}}$$

Landmark block inversion

$H_{ee}^{-1}$ is formed blockwise. Every Schur solver shares one regularized-retry policy, EliminatedBlocks::invert_in_place (partition.rs), so a landmark is regularized identically regardless of which solver eliminated it.

Choosing what to eliminate

SchurOrdering (linalg/schur/ordering.rs) classifies variables. Manual marks always apply:

#![allow(unused)]
fn main() {
problem.mark_for_elimination(landmark_key);
}

Auto-classification by manifold type and DOF is available but off by default:

#![allow(unused)]
fn main() {
let ordering = SchurOrdering::new().with_auto_detect(true);
}

The default is off for a specific reason (ordering.rs): Rn(3) is also how self-calibration represents intrinsics [focal, k1, k2]. Auto-detecting “any Rn(3) is a landmark” would silently eliminate intrinsic blocks and corrupt $S$. Turn it on only when you know every Rn(3) in the problem is a landmark.

The four Schur solvers

ExplicitSchurVariant::Sparse

Forms $H=J^{\mathsf{T}}J$, builds $S$ explicitly, factorizes with sparse Cholesky. The most accurate: it solves the reduced system exactly, and is the reference the other paths are measured against.

$S$ is accumulated into a dense ${\text{kept\_dof}}^2$ buffer and filtered back to sparse (explicit.rs). That buffer is the current scaling limit — 1.9 GB at Ladybug’s 15,507 retained DOF.

ExplicitSchurVariant::Chunked

Algebraically identical to Sparse — the benchmark confirms bit-identical RMSE on all four datasets — but builds $S$ chunk by chunk directly from $J$, never forming $J^{\mathsf{T}}J$, $J^{\mathsf{T}}$, or the value permutation. On the largest BAL problem those account for ~24 GB of the ~32 GB needed before elimination can start.

Per chunk $c$ (the rows sharing one eliminated variable), with $E=J[{\text{rows}}_c,e]$ and $F=J[{\text{rows}}_c,k]$:

$$S-=(E^{\mathsf{T}}F{)}^{\mathsf{T}}(E^{\mathsf{T}}E{)}^{-1}(E^{\mathsf{T}}F),g_{\text{red}}-=(E^{\mathsf{T}}F{)}^{\mathsf{T}}(E^{\mathsf{T}}E{)}^{-1}(E^{\mathsf{T}}{r}_c)$$

This trades time for memory and is slower in wall clock — 1.2×–2.1× the Sparse variant. The chunk sweep is inherently serial: reading rows out of a column-major $J$ without building a CSR copy relies on one forward-only cursor per retained column, which forces chunks to be visited in increasing row order. The $J^{\mathsf{T}}J$ path it replaces is fully parallel, so on a many-core machine the lost parallelism outweighs the saved work. Requires each eliminated variable’s rows to be contiguous, which Problem::group_rows_for_elimination arranges.

ExplicitSchurVariant::Iterative

Forms $S$ explicitly like Sparse, then solves it with PCG instead of Cholesky. Carries the same ${\text{kept\_dof}}^2$ memory cost while solving less exactly, so it buys nothing on memory — but it measures 0.5×–1.3× Sparse on time.

ImplicitSparseSchur — the matrix-free path

Forms neither $S$ nor $J^{\mathsf{T}}J$. Writing $J=[E\mid F]$, PCG only ever needs $S$’s action on a vector, and expanding that action so it reads $J$ alone gives the whole solver (implicit.rs):

$$\begin{array}{rl}y& =Fv\\ & \\ t& =E^{\mathsf{T}}y\\ & \\ u& =(E^{\mathsf{T}}E+\lambda D_e{)}^{-1}t\\ & \\ y& \leftarrow y-Eu\\ & \\ Sv& =F^{\mathsf{T}}y+\lambda D_{k}v\end{array}$$

Four passes over $J$’s nonzeros, no intermediate matrix. This is why it exists: $J^{\mathsf{T}}J$ for BA carries a dense block for every pair of cameras sharing a landmark — fill-in $J$ does not have — so cost scales with $\mathrm{nnz}(J)$ rather than $\mathrm{nnz}(J^{\mathsf{T}}J)$. Ceres states the same cost model for ITERATIVE_SCHUR.

Everything else it needs also comes from $J$ (linalg/schur/jacobian_ops.rs): the gradient $J^{\mathsf{T}}r$, the damping diagonal $\mathrm{diag}(J^{\mathsf{T}}J{)}_j=\Vert J_{:,j}{\Vert }^2$, and the $E^{\mathsf{T}}E$ blocks as column dot products.

get_hessian() returns None here; the quadratic model is served exactly as $J^{\mathsf{T}}(Jv)$.

This is what for_bundle_adjustment() selects.

ExplicitDenseSchur

The same explicit construction over a dense Hessian, for JacobianMode::Dense. Targets a few thousand DOF — a dense $J^{\mathsf{T}}J$ for the smallest BAL dataset would be ~485k × 485k.

Preconditioners

SchurPreconditioner applies to both PCG paths. The three match Ceres’s options for ITERATIVE_SCHUR:

SchurJacobi is the true one — it includes the elimination correction, so it preconditions $S$ rather than the unreduced retained block:

$$S_{ii}=(F^{\mathsf{T}}F+\lambda D{)}_{ii}-\sum _{j\in \text{visible}(i)}(F^{\mathsf{T}}E{)}_{ij}{H}_{ee,jj}^{-1}(F^{\mathsf{T}}E{)}_{ij}^{\mathsf{T}}$$

summed over the eliminated blocks visible to $i$ — captured by a visibility index cached per sparsity pattern, so this costs $O(\text{observations})$ rather than $O(\text{kept}\times \text{eliminated})$. Each block is inverted with the shared retry policy; a block that will not invert falls back to identity, because a preconditioner is a convergence aid and must not fail the solve.

PCG and the forcing sequence

Both PCG paths share one loop (linalg/schur/pcg.rs), starting from $x=0$ — every caller solves for a Newton step, not a general system that would benefit from a warm start.

Two stopping rules, and the second matters more. The residual rule is

$$\Vert r\Vert <{\epsilon }_r\cdot \max (\Vert b\Vert ,1)$$

but CG minimises the quadratic model $\phi (x)=\frac{1}{2}{x}^{\mathsf{T}}Ax-b^{\mathsf{T}}x$, and that value is available for free from vectors already on hand:

$$\phi (x)=-\frac{1}{2}{x}^{\mathsf{T}}(b+r)\text{since }Ax=b-r$$

A trust-region optimizer does not need the system solved accurately; it needs a step that reduces the model. Insisting on a small residual on an ill-conditioned $S$ burns iterations long after the model stops moving — measured on Ladybug, every solve ran the full iteration cap without the residual rule ever firing. So, following Ceres, the solve also stops on the forcing sequence with $Q_i=-x^{\mathsf{T}}(b+r)$:

$$i\cdot \frac{Q_i-Q_{i-1}}{Q_i}<\eta$$

The $\eta /i$ shape tightens the requirement as iterations accumulate, so early Newton steps are cheap and approximate while later ones — where the optimizer is close and needs accuracy — are solved harder.

$\eta$ defaults to 1e-2, deliberately tighter than Ceres’s 1e-1, because 1e-1 measured 9.7% worse RMSE on Ladybug and that loss did not recover with more optimizer iterations. The sweep is tabulated at DEFAULT_ETA (pcg.rs). Set with_schur_cg_q_tolerance(0.0) to disable the rule and get an exact solve — what the cross-solver agreement tests do.

PcgResult::termination reports which rule fired: Residual, QuadraticModel, MaxIterations (the step is truncated — worth logging) or Breakdown.

Configuration

#![allow(unused)]
fn main() {
use apex_solver::linalg::{LinearSolverType, ExplicitSchurVariant, SchurPreconditioner};
use apex_solver::optimizer::levenberg_marquardt::LevenbergMarquardtConfig;

// The BA preset: matrix-free, Schur-Jacobi, forcing sequence on.
let config = LevenbergMarquardtConfig::for_bundle_adjustment();

// Exact reduced solve instead, when the step must not depend on a tolerance.
let exact = LevenbergMarquardtConfig::for_bundle_adjustment()
    .with_linear_solver_type(LinearSolverType::ExplicitSparseSchur)
    .with_schur_variant(ExplicitSchurVariant::Sparse);

// Memory-bound: never form JᵀJ, accept the slower serial sweep.
let low_memory = LevenbergMarquardtConfig::for_bundle_adjustment()
    .with_linear_solver_type(LinearSolverType::ExplicitSparseSchur)
    .with_schur_variant(ExplicitSchurVariant::Chunked);

// Tune the iterative paths.
let tuned = LevenbergMarquardtConfig::for_bundle_adjustment()
    .with_schur_preconditioner(SchurPreconditioner::SchurJacobi)
    .with_schur_cg_params(200, 1e-6)   // max iterations, residual tolerance
    .with_schur_cg_q_tolerance(1e-2);  // forcing sequence; 0.0 disables
}

Mark what to eliminate on the problem itself:

#![allow(unused)]
fn main() {
let point = problem.add_variable(ManifoldType::RN, position);
problem.mark_for_elimination(point);
}

Which to use

Measured numbers for each are in Benchmarks.

Covariance estimation

Covariance::compute re-linearizes the problem at a point, forms a clean $H=J^{\mathsf{T}}J$ and inverts via sparse Cholesky or dense SVD:

#![allow(unused)]
fn main() {
use apex_solver::linalg::covariance::{Covariance, CovarianceAlgorithm, CovarianceOptions};

let cov = Covariance::compute(
    CovarianceOptions::new(CovarianceAlgorithm::SparseCholesky),
    &problem,
    &variables,
)?;
let block = cov.block(key); // dof × dof marginal in tangent space
}

The covariance is a property of the problem at a point — it never consults the optimizer’s last linear system.

Pose & Priors

factors::pose — relative constraints and anchors. No sensor model: these express graph topology.

Throughout, $⊞$ is the right perturbation $X⊞\delta =X\mathrm{Exp}(\delta )$, $\boxminus$ its inverse $Y\boxminus X=\mathrm{Log}(X^{-1}Y)$, and $J_r$, $J_l$ the right and left Jacobians of the group. The identity used everywhere below is

$$\mathrm{Log}\left(X\mathrm{Exp}(\mathbf{ϵ})\right)\approx \mathrm{Log}(X)+J_r^{-1}(\mathrm{Log}(X))\mathbf{ϵ}$$

BetweenFactor<T>

A relative measurement between two variables of the same Lie group. Odometry edges and loop closures are both this factor, the latter usually with a robust loss.

Blocks $[T_i,T_j]$ — any T: LieGroup. Residual $\mathrm{dof}(T)$, Jacobian $\mathrm{dof}(T)\times 2\mathrm{dof}(T)$.

Error

With measurement ${\tilde{T}}_{ij}$,

$$\mathbf{r}=\mathrm{Log}\left({\tilde{T}}_{ij}^{-1}{T}_i^{-1}{T}_j\right)$$

zero exactly when the estimated relative transform equals the measured one.

Jacobian

Write $A=T_i^{-1}{T}_j$. Perturbing the second variable, ${\tilde{T}}^{-1}A\mathrm{Exp}({\delta }_j)$, gives directly

$$\frac{\partial \mathbf{r}}{\partial {\delta }_j}=J_r^{-1}(\mathbf{r})$$

Perturbing the first, $T_i\leftarrow T_i\mathrm{Exp}({\delta }_i)$, moves the inverse to the left of $A$, and it has to be commuted through:

$${\stackrel{-1Exp(-\delta i)A=T}{T}}^{-1}A\mathrm{Exp}(-{\mathrm{Ad}}_{A^{-1}}{\delta }_i)$$

so

$$\frac{\partial \mathbf{r}}{\partial {\delta }_i}=-J_r^{-1}(\mathbf{r}){\mathrm{Ad}}_{T_j^{-1}{T}_i}$$

For SE(3), ${\mathrm{Ad}}_T=\left[\begin{array}{cc}R& [\mathbf{t}{]}_{\times }R\\ 0& R\end{array}\right]$ and

$$J_r^{-1}(\mathbf{\rho },\mathbf{\theta })=\left[\begin{array}{cc}{J}_r^{-1}(\mathbf{\theta })& Q_r\\ 0& J_r^{-1}(\mathbf{\theta })\end{array}\right],J_r^{-1}(\mathbf{\theta })=I+\frac{1}{2}[\mathbf{\theta }{]}_{\times }+\left(\frac{1}{{\theta }^2}-\frac{1+\cos \theta }{2\theta \sin \theta }\right)[\mathbf{\theta }{]}_{\times }^2$$

The implementation never writes these forms out: it chains the Jacobians that between, compose and log report, so it stays correct for every group.

#![allow(unused)]
fn main() {
use apex_solver::factors::pose::BetweenFactor;

problem.add_residual_block_with_noise(
    &[key_i, key_j],
    Box::new(BetweenFactor::new(measured_relative_pose)),
    Some(Box::new(HuberLoss::new(1.345)?)),
    NoiseModel::from_sigmas(&[0.05, 0.05, 0.05, 0.01, 0.01, 0.02])?,
);
}

PriorFactor<T>

Anchors one variable to a known value in the tangent space. Prefer this to Problem::fix_variable: fixed indices are honoured when the step is applied, but the linear system still treats those coordinates as free, so variables sharing factors with a “fixed” one are systematically under-corrected.

Blocks $[X]$. Residual $\mathrm{dof}(T)$, Jacobian square.

Error and Jacobian

$$\mathbf{r}=\mathrm{Log}\left(T_{\text{prior}}^{-1}X\right),\frac{\partial \mathbf{r}}{\partial \delta }=J_r^{-1}(\mathbf{r})$$

This is a BetweenFactor with an identity measurement and a constant first argument, and is implemented as exactly that — the two share one chain and cannot drift apart. Because the residual never touches the parameters, there is no quaternion double-cover ambiguity.

EuclideanPriorFactor

$$\mathbf{r}=\mathbf{x}-{\mathbf{x}}_{\text{prior}},\frac{\partial \mathbf{r}}{\partial \mathbf{x}}=I_n$$

Registration rejects any manifold but Rn — on a Lie group this would difference raw parameters, which is not a tangent-space error. Use it to anchor velocities, biases and intrinsics.

PoseRotationPrior / PoseTranslationPrior

Constrain one half of an SE(3) pose and leave the other free.

$$\text{Rotation:}\mathbf{r}=\mathrm{Log}\left(R_{\text{meas}}^{\top }R\right),\frac{\partial \mathbf{r}}{\partial (\delta \mathbf{\rho },\delta \mathbf{\theta })}=\left[0_{3\times 3}\right|J_r^{-1}(\mathbf{r})]$$

$$\text{Translation:}\mathbf{r}=\mathbf{t}-\tilde{\mathbf{t}},\frac{\partial \mathbf{r}}{\partial (\delta \mathbf{\rho },\delta \mathbf{\theta })}=\left[R\right|0_{3\times 3}]$$

The translation block is $R$, not $I$, because the right perturbation moves the origin by $R\delta \mathbf{\rho }$ in the world frame.

These are not a decomposition of PriorFactor<SE3>: the translation rows here are the world-frame difference, whereas the prior’s are the coupled SE(3) logarithm $\mathbf{\rho }$, which mixes rotation into translation through $J_l^{-1}(\mathbf{\theta })$. Stacking both does not reproduce the full prior.

PoseRotationPrior is the usual rotation-only loop-closure initializer; PoseTranslationPrior is how a GNSS position fix attaches to an SE(3) pose.

Visual

factors::visual — everything that goes through a camera.

All projection factors share a smooth cheirality penalty. A landmark behind the camera cannot be projected, but returning zero would make an invalid configuration look optimal, so they emit

$$r={\Pi }_{\text{base}}+{\Pi }_{\text{scale}}\max (z_{\text{min}}-z_{\text{cam}},0),\frac{\partial r}{\partial z_{\text{cam}}}=-{\Pi }_{\text{scale}}$$

with ${\Pi }_{\text{base}}=10^4$, ${\Pi }_{\text{scale}}=10^3$ — large enough to dominate any plausible in-image residual, small enough not to ill-condition the normal equations, and carrying a gradient that pushes the point back in front.

Two derivatives recur throughout. For a world-to-camera pose acting directly, ${\mathbf{p}}_c=R\mathbf{p}+\mathbf{t}$:

$$\frac{\partial {\mathbf{p}}_c}{\partial (\delta \mathbf{\rho },\delta \mathbf{\theta })}=\left[R\right|-R[\mathbf{p}{]}_{\times }],\frac{\partial {\mathbf{p}}_c}{\partial \mathbf{p}}=R$$

For an inverted pose, $\mathbf{q}=T^{-1}\mathbf{p}$, a right perturbation gives $\mathbf{q}\leftarrow \mathrm{Exp}(-\delta )\mathbf{q}=\mathbf{q}-\delta \mathbf{\rho }+[\mathbf{q}{]}_{\times }\delta \mathbf{\theta }$:

$$\frac{\partial \mathbf{q}}{\partial (\delta \mathbf{\rho },\delta \mathbf{\theta })}=[-I|[\mathbf{q}{]}_{\times }],\frac{\partial \mathbf{q}}{\partial \mathbf{p}}=R^{\top }$$

Both come from SE3::act, which reports them itself.

ProjectionFactor<CAM, OP>

Monocular reprojection over $N$ observations of one pose, generic over the camera model and over which blocks are optimized.

Blocks $[\text{pose}?,\text{landmarks}?,\text{intrinsics}?]$, selected at compile time by OP: OptimizationConfig — BundleAdjustment, SelfCalibration, OnlyPose, OnlyLandmarks, OnlyIntrinsics, PoseAndIntrinsics, LandmarksAndIntrinsics. Unused blocks cost nothing; the const-generic flags remove their columns entirely.

Residual $2N$, Jacobian $2N\times (6\text{P}+3N\text{L}+d_{\kappa }\text{I})$.

Error

$${\mathbf{p}}_{c,k}=R{\mathbf{p}}_k+\mathbf{t},{\mathbf{r}}_k=\pi ({\mathbf{p}}_{c,k};\mathbf{\kappa })-{\tilde{\mathbf{u}}}_k\in {\mathbb{R}}^2$$

Jacobian

$$\frac{\partial {\mathbf{r}}_k}{\partial (\delta \mathbf{\rho },\delta \mathbf{\theta })}=\frac{\partial \pi }{\partial {\mathbf{p}}_c}[R\mid -R[{\mathbf{p}}_k{]}_{\times }],\frac{\partial {\mathbf{r}}_k}{\partial {\mathbf{p}}_k}=\frac{\partial \pi }{\partial {\mathbf{p}}_c}R,\frac{\partial {\mathbf{r}}_k}{\partial \mathbf{\kappa }}=\frac{\partial \pi }{\partial \mathbf{\kappa }}$$

The camera model supplies $\partial \pi /\partial {\mathbf{p}}_c$ (2×3) and $\partial \pi /\partial \mathbf{\kappa }$ (2×$d_{\kappa }$) analytically. For a pinhole model,

$$\frac{\partial \pi }{\partial {\mathbf{p}}_c}=\frac{1}{z}\left[\begin{array}{ccc}{f}_x& 0& -f_{x}x/z\\ 0& f_y& -f_{y}y/z\end{array}\right]$$

Nine models are available (Pinhole, RadTan, KannalaBrandt, DoubleSphere, Eucm, Ucm, Fov, …).

ExtrinsicProjectionFactor<CAM>

Reprojection with the camera-to-body transform as a variable. On a real rig the extrinsics are the least trustworthy part of the setup and are shared by every observation from that camera, so they belong in the graph where many observations determine them.

Blocks $[T_{WB},T_{BC},{\mathbf{p}}_w]$ — residual 2, Jacobian $2\times 15$. Both poses read the way a calibration file does: body-in-world, camera-in-body.

Error

$$T_{WC}=T_{WB}{T}_{BC},{\mathbf{p}}_c=T_{WC}^{-1}{\mathbf{p}}_w,\mathbf{r}=\pi ({\mathbf{p}}_c)-\tilde{\mathbf{u}}$$

Jacobian

With $D=\frac{\partial \pi }{\partial {\mathbf{p}}_c}[-I\mid [{\mathbf{p}}_c{]}_{\times }]$ (2×6),

$$\frac{\partial \mathbf{r}}{\partial {\delta }_{WB}}=D{\mathrm{Ad}}_{T_{BC}^{-1}},\frac{\partial \mathbf{r}}{\partial {\delta }_{BC}}=D,\frac{\partial \mathbf{r}}{\partial {\mathbf{p}}_w}=\frac{\partial \pi }{\partial {\mathbf{p}}_c}{R}_{WC}^{\top }$$

The ${\mathrm{Ad}}_{T_{BC}^{-1}}$ factor is $\partial T_{WC}/\partial T_{WB}$, which compose reports directly. The extrinsics block gets $D$ unchanged because $\partial T_{WC}/\partial T_{BC}=I$.

TimeOffsetProjectionFactor<CAM>

Reprojection with the camera-to-IMU clock offset as a variable. The two clocks differ by a constant lag $t_d$; unmodelled it is indistinguishable from an extrinsic rotation error at constant angular rate, and 20 ms at 1 m/s is 2 cm of position error.

Blocks $[{\text{SE}}_2(3)\text{state},T_{BC},{\mathbf{p}}_w,t_d]$ — residual 2, Jacobian $2\times 19$.

Error

A first-order expansion of the state to the exposure instant,

$$\mathbf{p}(t+t_d)=\mathbf{p}+\mathbf{v}{t}_d,R(t+t_d)=R{R}_d,R_d=\mathrm{Exp}(\mathbf{\omega }{t}_d)$$

then $T_{WC}=T_{WB}(t+t_d)T_{BC}$ and the same reprojection as above. The state is SE23 because it already carries $\mathbf{v}$; $\mathbf{\omega }$ is the bias-corrected gyro sample, a measurement, since the graph has no angular rate to estimate.

Jacobian

The SE23 tangent $(\delta \mathbf{\rho },\delta \mathbf{\theta },\delta \mathbf{\nu })$ pushes to the exposure pose’s SE(3) tangent as

$$\delta {\mathbf{\rho }}^{\prime }=R_d^{\top }\left(\delta \mathbf{\rho }+t_d\delta \mathbf{\nu }\right),\delta {\mathbf{\theta }}^{\prime }=R_d^{\top }\delta \mathbf{\theta }$$

$$\frac{\partial T_{WB}}{\partial \text{state}}=\left[\begin{array}{ccc}{R}_d^{\top }& 0& t_d{R}_d^{\top }\\ 0& R_d^{\top }& 0\end{array}\right]\in {\mathbb{R}}^{6\times 9}$$

and the offset column is the exposure pose’s own tangent velocity,

$$\frac{\partial T_{WB}}{\partial t_d}=\left[\begin{array}{c}{R}_c^{\top }\mathbf{v}\\ \mathbf{\omega }\end{array}\right]\in {\mathbb{R}}^{6\times 1},R_c=R{R}_d$$

Both are then chained with $D{\mathrm{Ad}}_{T_{BC}^{-1}}$ exactly as above. Note the offset column vanishes when $\mathbf{v}=0$ and $\mathbf{\omega }=0$: a stationary platform cannot observe $t_d$, which the tests assert directly.

StereoFactor

Rectified stereo, so disparity fixes depth in a single observation.

Blocks $[\text{pose},\mathbf{p}]$ — residual 3, Jacobian $3\times 9$.

Error

$$u_L=c_x+f_x\frac{x}{z},u_R=c_x+f_x\frac{x-b}{z},v=c_y+f_y\frac{y}{z}$$

$$\mathbf{r}=(u_L,u_R,v)-({\stackrel{L,u}{u}}_R,\tilde{v})$$

Jacobian

$$\frac{\partial (u_L,u_R,v)}{\partial {\mathbf{p}}_c}=\frac{1}{z}\left[\begin{array}{ccc}{f}_x& 0& -f_{x}x/z\\ f_x& 0& -f_x(x-b)/z\\ 0& f_y& -f_{y}y/z\end{array}\right]$$

chained with $[R\mid -R[\mathbf{p}{]}_{\times }]$ and $R$. Calibration is fixed at construction — this is the one camera factor not generic over CameraModel.

InverseDepthFactor<CAM>

A landmark parameterized in its anchor camera by pixel $(u,v)$ and inverse depth $d=1/z$, which stays well-conditioned for distant, low-parallax points where a Euclidean 3D point blows up.

Blocks $[T_i,(u,v,d),T_j]$ — residual 2, Jacobian $2\times 15$.

$${\mathbf{X}}_i=\frac{{\pi }^{-1}(u,v)}{d},{\mathbf{p}}_w=T_{wc,i}^{-1}{\mathbf{X}}_i,\mathbf{r}={\pi }_j\left(T_{wc,j}{\mathbf{p}}_w\right)-\tilde{\mathbf{u}}$$

The anchor block’s Jacobian carries $\partial {\mathbf{X}}_i/\partial d=-{\pi }^{-1}(u,v)/d^2$, which is what makes the parameterization behave as $d\to 0$: the inverse depth stays finite and its derivative bounded where $z\to \infty$ would not.

SmartProjectionFactor<CAM>

Structure-less: connects $N$ poses to a landmark that is never a variable. The point is re-triangulated by DLT from the current poses at every linearization, then eliminated exactly by the implicit Schur complement.

Blocks $N$ poses — residual $2N$, Jacobian $2N\times 6N$. Whitens internally; register with NoiseModel::null().

Jacobian

Writing $A_i={\sigma }_i^{-1}\partial {\mathbf{u}}_i/\partial T_i$ (2×6) and $B_i={\sigma }_i^{-1}\partial {\mathbf{u}}_i/\partial \mathbf{p}$ (2×3),

$$J_{ij}={\delta }_{ij}{A}_i-B_i{M}_j,M_j=(\sum _k{B}_k^{\top }{B}_k{)}^{-1}{B}_j^{\top }{A}_j\in {\mathbb{R}}^{3\times 6}$$

The triangulated point satisfies the first-order optimality condition ${\sum }_i{B}_i^{\top }{\mathbf{r}}_i=0$, so this dense $J$ is the total derivative of the re-triangulated residual, and

$$J^{\top }J=\sum _i{A}_i^{\top }{A}_i-\left(\sum _i{A}_i^{\top }{B}_i\right)\left(\sum _k{B}_k^{\top }{B}_k{)}^{-1}\right(\sum _j{B}_j^{\top }{A}_j)$$

is exactly the Schur complement. Degenerate geometry (pure rotation, rank-deficient DLT) returns a bounded penalty; status() reports which.

EssentialMatrixFactor / EssentialMatrixConstraint

2D–2D epipolar geometry over a relative pose, for initialization and loop-closure verification before any landmark exists. Points are normalized camera coordinates.

$$\text{Factor:}{r}_i={\mathbf{p}}_{2,i}^{\top }[\hat{\mathbf{t}}{]}_{\times }R{\mathbf{p}}_{1,i},\hat{\mathbf{t}}=\frac{\mathbf{t}}{\Vert \mathbf{t}\Vert },i=1\dots N$$

$$\text{Constraint:}\mathbf{r}=\left[\begin{array}{c}\mathrm{Log}(R_E^{\top }R)\\ \hat{\mathbf{t}}-{\mathbf{u}}_E\end{array}\right]\in {\mathbb{R}}^6$$

The direction normalization contributes $\partial \hat{\mathbf{t}}/\partial \mathbf{t}=(I-\hat{\mathbf{t}}{\hat{\mathbf{t}}}^{\top })/\Vert \mathbf{t}\Vert$, which is rank 2 — both factors are scale-free by construction. Only the translation direction is observable from epipolar geometry, so a graph containing only these has a one-dimensional null space; the constraint’s six rows are effectively rank 5.

DepthFactor<ONESIDED> / HomogeneousPointFactor

Both act on a homogeneous 4D landmark $\mathbf{h}=[x,y,z,w{]}^{\top }$, the parameterization that keeps points at infinity representable.

DepthFactor — an RGB-D-style depth reading

Blocks $[T_{WS},{\mathbf{h}}_W,T_{SC}]$ — residual 1, Jacobian $1\times 16$.

$${\mathbf{h}}_C=T_{CS}{T}_{SW}{\mathbf{h}}_W,z=\frac{{\mathbf{h}}_C[2]}{{\mathbf{h}}_C[3]},r=W(\tilde{z}-z)$$

With ONESIDED = true (OneSidedDepthFactor) both residual and Jacobian are zeroed when $z>\tilde{z}$, penalizing only points that come too close — a free-space constraint rather than a depth measurement.

HomogeneousPointFactor — a unary prior on the dehomogenized position

Blocks $[\mathbf{h}]$ — residual 3, Jacobian $3\times 4$.

$${\mathbf{p}}_{\text{est}}=\frac{{\mathbf{h}}_{0:3}}{w},\mathbf{r}=W\left(\tilde{\mathbf{p}}-{\mathbf{p}}_{\text{est}}\right),\frac{\partial \mathbf{r}}{\partial \mathbf{h}}=-\frac{W}{w}\left[I_3\right|-{\mathbf{p}}_{\text{est}}]$$

Both whiten internally.

IMU

factors::imu — inertial preintegration. Two factors per group and nothing else, so a choice here is two questions rather than a catalogue.

Preintegration

Integrating raw IMU samples between keyframes would re-integrate the whole interval every time the state changes. Preintegration instead accumulates the measurements into a relative quantity that does not depend on the states — only on the bias linearization point — so a keyframe pair can be re-evaluated in constant time.

ImuPreintegration::new(measurements, params, t0, t1, &bias_ref) accumulates the delta $\Delta$, its $15\times 15$ covariance, and the bias-correction Jacobians $\partial \Delta /\partial \mathbf{b}$. Everything it produces is gravity-free; the factors reinstate gravity when they build the gravity-corrected state.

Two axes

Which group? se23 models a keyframe as $(R,\mathbf{t},\mathbf{v})$; sgal3 adds a time coordinate, $(R,\mathbf{t},\mathbf{v},s)$, making the inter-keyframe interval an estimated quantity — pick it for time-offset or rolling-shutter calibration, se23 otherwise.

Combined or not? This is the split GTSAM draws between ImuFactor and CombinedImuFactor, and it decides the residual dimension.

ImuFactor shares one bias variable across the interval and leaves its evolution to a separate edge; CombinedImuFactor takes a bias per keyframe and embeds the random walk in six extra residual rows. Doing both counts that uncertainty twice.

Weighting follows the same split: the shared-bias form uses a $9\times 9$ information built from measurement noise alone, the combined form the full $15\times 15$ including the random walk.

A keyframe is a single variable on the group, not separate pose and velocity blocks — the optimizer’s update is then a group right-plus, and the pose/velocity coupling inertial integration produces is the group’s job. The practical consequence is that aiding measurements attach to that state: an SE(3)-only factor cannot connect to an SE23 variable, while PriorFactor<SE23> and MarginalPriorFactor are generic and do.

Error

$$\begin{array}{rl}{\text{gc}}_i& ={\mathrm{SE}}_2(3)\left({\mathbf{p}}_i+{\mathbf{v}}_i\Delta t-\frac{1}{2}\mathbf{g}\Delta t^2,{\mathbf{v}}_i-\mathbf{g}\Delta t,R_i\right)\\ \text{predicted}& ={\text{gc}}_i^{-1}\circ {\text{state}}_j\\ \mathbf{r}& =\text{predicted}\boxminus (\Delta ⊞\mathbf{c}(\mathbf{b}))\in {\mathbb{R}}^9\end{array}$$

Folding gravity into the state rather than the residual is what makes the comparison a plain group right-minus. The bias correction $\mathbf{c}(\mathbf{b})$ is the first-order update of the preintegrated delta for a bias away from its linearization point,

$$\mathbf{c}=\left(\frac{\partial \Delta \mathbf{p}}{\partial {\mathbf{b}}_g}\delta {\mathbf{b}}_g-C_{\iint }\delta {\mathbf{b}}_a,-\frac{\partial \Delta \mathbf{\alpha }}{\partial {\mathbf{b}}_g}\delta {\mathbf{b}}_g,\frac{\partial \Delta \mathbf{v}}{\partial {\mathbf{b}}_g}\delta {\mathbf{b}}_g-C_{\int }\delta {\mathbf{b}}_a\right)$$

The combined form appends ${\mathbf{b}}_i-{\mathbf{b}}_j$ (6 rows).

Jacobian

All four factors are thin wrappers over one evaluate, which returns the residual and three tangent-space blocks. Write $J_{\boxminus }^{\text{pred}},J_{\boxminus }^{\Delta }$ for the two Jacobians right_minus reports and $J_{⊞}$ for the tangent Jacobian of right_plus. Then

$$\frac{\partial \mathbf{r}}{\partial {\text{gc}}_i}=J_{\boxminus }^{\text{pred}}\cdot (-{\mathrm{Ad}}_{{\text{predicted}}^{-1}}),\frac{\partial \mathbf{r}}{\partial {\text{state}}_j}=J_{\boxminus }^{\text{pred}},\frac{\partial \mathbf{r}}{\partial \mathbf{b}}=J_{\boxminus }^{\Delta }{J}_{⊞}\frac{\partial \mathbf{c}}{\partial \mathbf{b}}$$

The $-\mathrm{Ad}$ comes from differentiating $X^{-1}Y$ with respect to the left argument: $(X\mathrm{Exp}(\delta ){)}^{-1}Y=\mathrm{Exp}(-{\mathrm{Ad}}_{(X^{-1}Y{)}^{-1}}\delta )X^{-1}Y$.

The bias block expands to

$$\frac{\partial \mathbf{c}}{\partial \mathbf{b}}=\left[\begin{array}{cc}\partial \Delta \mathbf{p}/\partial {\mathbf{b}}_g& -C_{\iint }\\ -\partial \Delta \mathbf{\alpha }/\partial {\mathbf{b}}_g& 0\\ \partial \Delta \mathbf{v}/\partial {\mathbf{b}}_g& -C_{\int }\end{array}\right]\in {\mathbb{R}}^{9\times 6}$$

in the $[\mathbf{\rho },\mathbf{\theta },\mathbf{\nu }]$ tangent order.

The lift from ${\text{state}}_i$ to the gravity-corrected state is a constant matrix, independent of the linearization point, because ${\text{gc}}_i$ shares ${\text{state}}_i$’s rotation and shifts the rest by gravity terms only:

$$\frac{\partial {\text{gc}}_i}{\partial {\text{state}}_i}=\left[\begin{array}{ccc}I& 0& \Delta tI\\ 0& I& 0\\ 0& 0& I\end{array}\right],(\delta \mathbf{\rho },\delta \mathbf{\theta },\delta \mathbf{\nu })\mapsto (\delta \mathbf{\rho }+\Delta t\delta \mathbf{\nu },\delta \mathbf{\theta },\delta \mathbf{\nu })$$

The combined form appends the bias-walk rows, whose Jacobian is constant:

$$\frac{\partial ({\mathbf{b}}_i-{\mathbf{b}}_j)}{\partial {\mathbf{b}}_i}=I_6,\frac{\partial ({\mathbf{b}}_i-{\mathbf{b}}_j)}{\partial {\mathbf{b}}_j}=-I_6$$

Every $J$ above is reported by the group’s own right_plus, right_minus and adjoint; none of it is hand-derived.

Bias evolution

The non-combined factors need a companion edge, which factors::imu::bias builds as a BetweenFactor<Rn> with zero measurement:

$${\mathbf{b}}_j={\mathbf{b}}_i+\mathbf{w},\mathbf{w}\sim \mathcal{N}\left(0,\mathrm{diag}({\sigma }_{gw}^2,{\sigma }_{aw}^2)\Delta t\right)$$

#![allow(unused)]
fn main() {
use apex_solver::factors::imu::{se23, bias_random_walk, bias_random_walk_noise};

problem.add_residual_block(
    &[state_i, state_j, bias],
    Box::new(se23::ImuFactor::new(preintegration)),
    None,
);
problem.add_residual_block_with_noise(
    &[bias, bias_next],
    Box::new(bias_random_walk()),
    None,
    bias_random_walk_noise(&params, dt)?,
);
}

An initial bias prior (an EuclideanPriorFactor on ${\mathbb{R}}^6$) is required for observability in every configuration.

SGal(3) and the time row

SGal(3)’s tangent carries a time coordinate, so the residual gains a row $(t_j-t_i)-\Delta t$, weighted by $1/{\sigma }_t$ (with_time_sigma, default 100 µs). That row is the reason to choose SGal(3): it makes the interval a quantity the optimizer can move.

Known limitation. SGal(3)’s group law is $\mathbf{t}=R_1({\mathbf{t}}_2+s_1{\mathbf{v}}_2)+{\mathbf{t}}_1$ — the left operand’s time couples the right operand’s velocity into translation. The residual composes ${\text{gc}}_i^{-1}\circ {\text{state}}_j$, so it depends on the absolute $s_i$, while the preintegrated delta corresponds to $s_i=0$. A single interval with $s_i=0,s_j=\Delta t$ is correct; chaining these factors across keyframes on a common absolute clock is not. Prefer the se23 factors for multi-keyframe chains until the residual is re-derived in terms of $\Delta s=s_j-s_i$.