Basic Calculus

SLAM back-ends spend most of their time minimizing nonlinear cost functions. Two tools from calculus make that possible: differentiation (Jacobians) and Taylor expansion (linearization).

Differentiation and Jacobians

A scalar function $f: \mathbb{R}^n \to \mathbb{R}$ has a gradient $\nabla f = \left[\frac{\partial f}{\partial x_1}, \ldots, \frac{\partial f}{\partial x_n}\right]^T$ . A vector function $\mathbf{f}: \mathbb{R}^n \to \mathbb{R}^m$ has a Jacobian matrix:

J = \frac{\partial \mathbf{f}}{\partial \mathbf{x}} = \begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \cdots & \frac{\partial f_1}{\partial x_n} \\ \vdots & \ddots & \vdots \\ \frac{\partial f_m}{\partial x_1} & \cdots & \frac{\partial f_m}{\partial x_n} \end{bmatrix} \in \mathbb{R}^{m \times n}

The Jacobian is the key tool in SLAM optimization: given a residual $\mathbf{e}(\mathbf{x})$ (for example, a reprojection error), its Jacobian $J = \frac{\partial \mathbf{e}}{\partial \mathbf{x}}$ tells us how the residual changes with small perturbations to the state — exactly what Gauss-Newton and Levenberg-Marquardt need.

The chain rule in SLAM residuals

SLAM residuals are almost always compositions of simpler functions, so their Jacobians come from the chain rule. The reprojection error of a map point $\mathbf{X}$ observed at pixel $\mathbf{z}$ under pose $T$ is

$\mathbf{e} = \mathbf{z} - \pi\big(T\,\mathbf{X}\big)$

a composition of (1) a rigid transformation, (2) perspective division, and (3) the intrinsic mapping. Its Jacobian factors into a product:

$\frac{\partial \mathbf{e}}{\partial \mathbf{x}} = -\,\frac{\partial \pi}{\partial \mathbf{X}_c}\cdot\frac{\partial \mathbf{X}_c}{\partial \mathbf{x}}$

where $\mathbf{X}_c = T\mathbf{X}$ is the point in the camera frame. Deriving each factor separately and multiplying is far less error-prone than differentiating the whole expression at once — and it is exactly how SLAM libraries structure their analytic Jacobians.

Taylor Expansion

The Taylor series expands a smooth function $f$ around a point $x_0$ :

$f(x) = f(x_0) + f'(x_0)(x - x_0) + \frac{1}{2!}f''(x_0)(x - x_0)^2 + \cdots$

For a multivariate function $f(\mathbf{x})$ around $\mathbf{x}_0$ :

$f(\mathbf{x}) \approx f(\mathbf{x}_0) + J(\mathbf{x}_0)(\mathbf{x} - \mathbf{x}_0) + \frac{1}{2}(\mathbf{x} - \mathbf{x}_0)^T H(\mathbf{x}_0)(\mathbf{x} - \mathbf{x}_0)$

where $J$ is the Jacobian (first order) and $H$ is the Hessian matrix (second order). Truncating at first order gives the linear approximation used in Gauss-Newton; truncating at second order gives the quadratic approximation used in Newton’s method.

From Taylor expansion to Gauss-Newton

Consider minimizing a sum of squared residuals $F(\mathbf{x}) = \frac{1}{2}\|\mathbf{e}(\mathbf{x})\|^2$ . Linearize the residual around the current estimate, $\mathbf{e}(\mathbf{x} + \Delta\mathbf{x}) \approx \mathbf{e} + J\Delta\mathbf{x}$ , and substitute:

$F(\mathbf{x} + \Delta\mathbf{x}) \approx \frac{1}{2}\|\mathbf{e}\|^2 + \mathbf{e}^T J\,\Delta\mathbf{x} + \frac{1}{2}\Delta\mathbf{x}^T J^T J\,\Delta\mathbf{x}$

This is a quadratic in $\Delta\mathbf{x}$ ; setting its derivative to zero gives the normal equations:

$(J^T J)\,\Delta\mathbf{x} = -J^T \mathbf{e}$

Gauss-Newton is therefore “Newton’s method with $H \approx J^T J$ ” — the second-derivative terms of the true Hessian are dropped, which is a good approximation when residuals are small. Levenberg-Marquardt adds a damping term, solving $(J^TJ + \lambda I)\Delta\mathbf{x} = -J^T\mathbf{e}$ , interpolating between Gauss-Newton ( $\lambda \to 0$ ) and gradient descent ( $\lambda$ large).

Checking Jacobians numerically

Analytic Jacobians are notoriously easy to get wrong (a sign, a transposed block). The standard sanity check is central finite differences: perturb one state dimension at a time and compare

$J_{:,k} \approx \frac{\mathbf{e}(\mathbf{x} + h\,\mathbf{1}_k) - \mathbf{e}(\mathbf{x} - h\,\mathbf{1}_k)}{2h}$

with a small step (e.g., $h \sim 10^{-6}$ ). Every serious SLAM codebase has a unit test doing exactly this for each residual type.

Common pitfalls

Sign errors: the residual $\mathbf{z} - \pi(\cdot)$ vs. $\pi(\cdot) - \mathbf{z}$ flips the sign of $J$ ; be consistent.
Differentiating rotations naively: rotation matrices are constrained, so derivatives must be taken with respect to a local perturbation (see Lie groups), not the 9 matrix entries.
Forgetting a chain-rule factor through normalization or distortion functions — the numeric check above catches this immediately.

Why it matters for SLAM

In bundle adjustment, the reprojection error $\mathbf{e}(\mathbf{x} + \Delta\mathbf{x}) \approx \mathbf{e}(\mathbf{x}) + J\Delta\mathbf{x}$ is linearized around the current estimate. This converts the nonlinear least-squares problem into a sequence of linear systems $(J^T J)\Delta\mathbf{x} = -J^T \mathbf{e}$ , solved iteratively. Every optimization-based SLAM system — from pose graph optimization to full bundle adjustment — is built on this linearize-solve-update loop, so being able to derive Jacobians by hand (and check them numerically) is a core skill.