Levenberg-Marquardt

Levenberg-Marquardt (LM) is the workhorse solver for nonlinear least-squares problems in SLAM. It is a damped version of Gauss-Newton that interpolates between Gauss-Newton (fast near the minimum) and gradient descent (safe far from it), making it much more robust to poor initialization.

From Gauss-Newton to LM

For a cost $F(\mathbf{x}) = \tfrac{1}{2}\|\mathbf{e}(\mathbf{x})\|^2$, Gauss-Newton linearizes the residual $\mathbf{e}(\mathbf{x}_k + \Delta\mathbf{x}) \approx \mathbf{e}(\mathbf{x}_k) + J_k \Delta\mathbf{x}$ and solves the normal equations

This can diverge when the linearization is a bad approximation, or when $J_k^T J_k$ is (nearly) singular. LM adds a damping term $\lambda I$:

where:

• $J_k$ is the Jacobian of the residual vector at the current estimate $\mathbf{x}_k$,
• $\lambda > 0$ is the damping parameter,
• $\Delta\mathbf{x}$ is the update, applied as $\mathbf{x}_{k+1} = \mathbf{x}_k + \Delta\mathbf{x}$ (or via the exponential map for poses on a manifold).

The two limits explain the behavior:

• $\lambda \to 0$: the equation reduces to Gauss-Newton — large, aggressive steps with near-quadratic convergence close to the minimum.
• $\lambda \to \infty$: the equation approaches $\lambda\,\Delta\mathbf{x} = -J_k^T\mathbf{e}$, i.e. a short step along the negative gradient — slow but reliable descent.

Marquardt’s refinement scales the damping by the curvature of each coordinate, replacing $\lambda I$ with $\lambda\,\mathrm{diag}(J_k^T J_k)$, so that weakly constrained directions get damped more and the method becomes invariant to per-parameter rescaling.

Adapting the damping

$\lambda$ is adjusted every iteration based on whether the step actually helped:

1. Solve for $\Delta\mathbf{x}$ with the current $\lambda$.
2. Evaluate the true cost at $\mathbf{x}_k + \Delta\mathbf{x}$.
3. If the cost decreased: accept the step and decrease $\lambda$ (trust the linearization more).
4. If the cost increased: reject the step and increase $\lambda$ (take a smaller, more gradient-like step), then re-solve.

A common refinement compares the actual cost reduction against the reduction predicted by the linearized model (the gain ratio): a ratio near 1 means the local quadratic model is trustworthy and $\lambda$ can shrink aggressively; a small or negative ratio means the model is poor and $\lambda$ must grow. This is exactly the logic of a trust-region method — LM can be read as implicitly maintaining a region around $\mathbf{x}_k$ within which the linearization is trusted, with $\lambda$ inversely related to the region’s radius.

Practical notes for SLAM problems

• The damped system matrix $J^T J + \lambda I$ is always positive definite for $\lambda > 0$, so Cholesky factorization always succeeds — important for gauge-underconstrained problems such as monocular bundle adjustment.
• The sparsity of the SLAM Hessian is preserved: damping only touches the diagonal, so the Schur complement trick for bundle adjustment works unchanged.
• Robust kernels (M-estimators) fold into LM as iteratively reweighted least squares — weights modify $J$ and $\mathbf{e}$, the LM loop is otherwise identical.
• LM is the default optimizer in Ceres Solver, g2o, and GTSAM, and hence the algorithm actually running inside most SLAM back-ends.

Why it matters for SLAM

Nearly every optimization in a modern SLAM pipeline — bundle adjustment, pose graph optimization, PnP refinement, camera calibration — is solved with LM. SLAM problems are highly nonlinear (projection, rotation manifolds) and initializations are often mediocre (motion-model predictions, noisy triangulations), so the pure Gauss-Newton step frequently overshoots. LM’s automatic damping is what makes these solvers converge reliably frame after frame without hand-tuning, which is why it has been the default in SLAM back-ends for decades.

Related

• Gauss-Newton
• Non-linear optimization
• Bundle Adjustment
• Reprojection error
• Math libraries (Eigen, Ceres, GTSAM, g2o)