Non-linear Optimization

SLAM estimation ultimately reduces to minimizing a cost function

F(\mathbf{x}) = \frac{1}{2}\sum_i \|\mathbf{e}_i(\mathbf{x})\|^2_{\Sigma_i^{-1}}

over the state $\mathbf{x}$ (poses, landmarks, biases), where each $\mathbf{e}_i$ is a residual — a reprojection error, an odometry error, an IMU error — weighted by its inverse covariance. The residuals are non-linear in the state: camera projection divides by depth, and rotations live on a curved manifold. So there is no closed-form solution; we must iterate.

The iterative descent template

All practical solvers share one loop: starting from an initial guess $\mathbf{x}_0$ , repeatedly find an update $\Delta\mathbf{x}$ that decreases the cost, apply it, and stop when the update or the gradient becomes tiny. Methods differ in how they pick $\Delta\mathbf{x}$ :

Gradient descent: $\Delta\mathbf{x} = -\alpha\, \nabla F$ . Only needs first derivatives, but zig-zags and converges slowly (linearly) — rarely used directly in SLAM.
Newton’s method: solve $H\,\Delta\mathbf{x} = -\nabla F$ with the true Hessian $H$ . Quadratic convergence, but second derivatives of residuals are expensive and $H$ may not be positive definite far from the optimum.
Gauss-Newton: exploit the least-squares structure. Linearize each residual, $\mathbf{e}(\mathbf{x} + \Delta\mathbf{x}) \approx \mathbf{e}(\mathbf{x}) + J\Delta\mathbf{x}$ , and solve the normal equations $(J^T J)\,\Delta\mathbf{x} = -J^T\mathbf{e}$ . The matrix $J^T J$ approximates the Hessian using only first derivatives — the key trick that makes large-scale SLAM tractable.
Levenberg-Marquardt: Gauss-Newton with adaptive damping, $(J^T J + \lambda I)\,\Delta\mathbf{x} = -J^T\mathbf{e}$ , blending toward gradient descent when the linearization is untrustworthy. The de-facto default.
Dogleg / trust-region: explicitly maintain a region radius and combine the Gauss-Newton and gradient steps inside it; comparable in spirit to LM, often faster on bundle adjustment.

Optimization on manifolds

Camera poses are elements of $\mathrm{SE}(3)$ , not vectors — naively adding an update to a rotation matrix destroys orthogonality. The standard fix is to optimize a local perturbation $\boldsymbol{\xi} \in \mathbb{R}^6$ in the Lie algebra and apply it through the exponential map:

T \leftarrow T \cdot \exp(\hat{\boldsymbol{\xi}})

The solver only ever sees the small vector $\boldsymbol{\xi}$ ; the retraction keeps the state on the manifold. Every SLAM library implements this (local parameterizations in Ceres, vertex oplus in g2o, retractions in GTSAM).

What makes SLAM problems solvable

Sparsity. Each residual touches very few variables (one pose + one landmark, or two poses), so $J^T J$ is extremely sparse. Sparse Cholesky and the Schur complement turn otherwise-impossible problems (tens of thousands of variables) into real-time ones.
Good initialization. These methods only find the local minimum of a non-convex cost. SLAM survives because it always has a decent initial guess — the previous frame’s pose, a motion model, a minimal-solver result from RANSAC. Cold-start problems (relocalization, map merging) are hard precisely because this guess is missing.
Robustness. Squared costs assume Gaussian noise; real matchers produce outliers. Robust kernels (M-estimators) reshape the cost so gross errors stop dominating, at the price of more non-convexity.

Reading a solver’s output

Iterations are stopped by practical criteria: the cost decrease per iteration falls below a tolerance, the update norm $\|\Delta\mathbf{x}\|$ becomes negligible, the gradient norm approaches zero, or an iteration/time budget is exhausted (real-time systems often cap bundle adjustment at a handful of iterations per keyframe). When a solve goes wrong, the symptoms map to causes: cost exploding usually means a bad initial guess or a Jacobian bug; cost stalling high with many small steps suggests convergence to a poor local minimum or unmodeled outliers; a rank-deficient normal-equation matrix points to unobservable directions (gauge freedom, unconstrained landmarks) that need priors or fixing.

Why it matters for SLAM

The back-end of every modern SLAM system — bundle adjustment, pose graph optimization, VIO sliding windows, direct photometric alignment — is an instance of sparse non-linear least squares solved with Gauss-Newton/LM iterations on a manifold. The probabilistic view (MLE & MAP) says what to minimize; non-linear optimization is how. Fluency here pays off everywhere: reading a paper’s “we minimize the following energy” section, diagnosing divergence (bad initialization? wrong parameterization? outliers?), and using Ceres/g2o/GTSAM effectively all rest on this material.

Hands-on

Ceres-solver hands-on