Nonlinear Least Squares

Nonlinear least squares fits a model whose predictions depend nonlinearly on its parameters by minimizing the sum of squared residuals. It is the form of least squares that geometric vision actually needs: projecting a 3D point through a camera involves a division by depth, rotations, and lens distortion, none of which is linear in the unknowns. Camera calibration, triangulation, homography refinement, pose estimation, and bundle adjustment all end in a nonlinear least-squares problem, usually one that minimizes reprojection error, the distance in pixels between where a point was observed and where the model says it should appear.

Without a closed-form solution, the problem is solved iteratively. Each iteration replaces the model by its linearization at the current estimate and solves a linear least-squares problem for the update. The two standard methods are Gauss–Newton and its damped variant, the Levenberg–Marquardt method, proposed by Kenneth Levenberg in 1944 and later by Donald Marquardt in 1963.

Definition

Given $m$ residuals $r_i(\boldsymbol{\theta})$, each a differentiable function of $n \le m$ parameters $\boldsymbol{\theta}$, a nonlinear least-squares problem asks for the parameters that minimize $\sum_i r_i(\boldsymbol{\theta})^2$, where at least one residual is nonlinear in $\boldsymbol{\theta}$. The methods for it find a local minimizer: a point whose cost is no larger than that of any nearby point.

Intuition

In the linear case the cost is a quadratic bowl, and one solve of the normal equations lands at its bottom. A nonlinear cost can be curved, tilted, and have several valleys. Gauss–Newton replaces it near the current estimate by the bowl that a linearized model would produce, jumps to that bowl’s bottom, and repeats. Near the solution the bowl is accurate and the jumps converge quickly; far from it a jump can overshoot wildly. Damping prevents this by shortening the step and turning it downhill whenever the linear model’s prediction proves wrong.

Formal Definition

Collect the residuals in a vector $\mathbf{r}(\boldsymbol{\theta}) \in \mathbb{R}^m$ and minimize

$$F(\boldsymbol{\theta}) = \tfrac{1}{2} \sum_{i=1}^{m} r_i(\boldsymbol{\theta})^2 = \tfrac{1}{2} \lVert \mathbf{r}(\boldsymbol{\theta}) \rVert^2,$$

where the factor $\tfrac{1}{2}$ only simplifies the derivatives. With the Jacobian $J(\boldsymbol{\theta}) \in \mathbb{R}^{m \times n}$, $J_{ij} = \partial r_i / \partial \theta_j$, the gradient and Hessian of $F$ are

$$\nabla F = J^\top \mathbf{r}, \qquad \nabla^2 F = J^\top J + \sum_{i=1}^{m} r_i \, \nabla^2 r_i .$$

A minimizer satisfies $J(\boldsymbol{\theta})^\top \mathbf{r}(\boldsymbol{\theta}) = \mathbf{0}$, a system of $n$ nonlinear equations that in general has no closed-form solution, and may have several solutions. In the linear case $\mathbf{r} = A\boldsymbol{\theta} - \mathbf{b}$, the Jacobian is the constant matrix $A$, the second term of the Hessian vanishes, and the condition reduces to the normal equations.

Properties

The Gauss–Newton step

Linearizing the residuals around the current estimate, $\mathbf{r}(\boldsymbol{\theta} + \boldsymbol{\delta}) \approx \mathbf{r} + J\boldsymbol{\delta}$, turns the problem for the step into linear least squares, $\min_{\boldsymbol{\delta}} \lVert J\boldsymbol{\delta} + \mathbf{r} \rVert^2$, whose normal equations are

$$J^\top J \, \boldsymbol{\delta}_{\text{gn}} = -J^\top \mathbf{r}, \qquad \boldsymbol{\theta} \leftarrow \boldsymbol{\theta} + \boldsymbol{\delta}_{\text{gn}} .$$

This is Newton’s method with the Hessian approximated by $J^\top J$, dropping the term $\sum_i r_i \nabla^2 r_i$. Only first derivatives are needed; second derivatives of a projection with distortion are tedious to derive. When $J$ has full column rank, $J^\top J$ is positive definite and the step points downhill. Each step is a linear least-squares solve, so the numerical advice for linear least squares applies inside the loop.

The dropped term is small when the residuals at the solution are small or the model is nearly linear there. Madsen, Nielsen and Tingleff show that near the solution Gauss–Newton converges quadratically if the residuals vanish there, can converge superlinearly if the residuals or their curvatures are small, and in general converges only linearly. Triggs and colleagues observe that bundle adjustment with a plain squared cost behaves as a nearly linear, small-residual problem near its minimum: feature positions are uncertain by a few pixels at most, while perspective and lens distortion change little over such distances. Far from the minimum, with large residuals, the approximation can be poor and undamped steps can diverge.

Damping, trust regions, and Levenberg–Marquardt

The Levenberg–Marquardt (LM) method adds a damping term to the Gauss–Newton system:

$$\left(J^\top J + \lambda I\right) \boldsymbol{\delta}_{\text{lm}} = -J^\top \mathbf{r}, \qquad \lambda > 0 .$$

For small $\lambda$ the step approaches the Gauss–Newton step. For large $\lambda$ it approaches $-J^\top \mathbf{r}/\lambda$, a short step along the negative gradient, as in gradient descent. The matrix is positive definite for any $\lambda > 0$, so the step is always a descent direction, even when $J$ is rank deficient. The damping is adapted from the gain ratio, the actual decrease of $F$ divided by the decrease the linear model predicted. A good prediction lets $\lambda$ shrink so steps become more Gauss–Newton-like; a poor one enlarges $\lambda$, and a step that increases the cost is rejected. A common variant replaces $I$ by the diagonal of $J^\top J$, so that the damping does not depend on the units of the parameters.

A trust-region method instead minimizes the linear model’s cost within a ball of radius $\Delta$ and adapts $\Delta$ from the same gain ratio. Madsen and colleagues show that the damped step for a given $\lambda$ is the trust-region step for radius $\lVert \boldsymbol{\delta}_{\text{lm}} \rVert$, so the two families are closely related; Powell’s dog-leg method is a trust-region alternative to LM. Nocedal and Wright (2006, ch. 10) treat these methods in full.

Convergence, local minima, and initialization

Damping makes the iteration robust to a poor linearization, not to a poor starting point. Every method above descends to a local minimizer, or toward a degenerate limit, near where it starts. The practical answer is a good initialization, which in vision usually comes from a linear method: the direct linear transform for homographies and camera matrices, the eight-point algorithm for the fundamental matrix, a closed-form estimate of the intrinsics in calibration, or linear triangulation. The nonlinear stage then refines a nearly correct answer, where Gauss–Newton is also most accurate.

Parameters that do not form a vector space need care. A rotation updated by adding a small vector to its matrix entries is no longer a rotation, so implementations parametrize the step locally, for example by a small rotation vector $\boldsymbol{\omega}$ with $R \leftarrow R \exp([\boldsymbol{\omega}]_\times)$, or renormalize a quaternion after each update.

Robust losses and reweighting

Squared residuals give outliers a large influence. A robust cost replaces each squared residual by a function $\rho$ that grows more slowly, an M-estimator:

$$F_\rho(\boldsymbol{\theta}) = \sum_{i=1}^{m} \rho\big(r_i(\boldsymbol{\theta})\big).$$

The Huber loss is $\tfrac{1}{2}r^2$ for $|r| \le k$ and $k|r| - \tfrac{1}{2}k^2$ beyond; the Cauchy loss $\tfrac{c^2}{2}\log\!\left(1 + r^2/c^2\right)$ grows only logarithmically. Since $\nabla F_\rho = J^\top W \mathbf{r}$ with weights $w_i = \rho'(r_i)/r_i$, the problem can be solved by iteratively reweighted least squares (IRLS): compute the weights from the current residuals, take a weighted Gauss–Newton or LM step with $J^\top W J$, and repeat. For the Huber loss $w_i = \min(1, k/|r_i|)$; for the Cauchy loss $w_i = 1/(1 + r_i^2/c^2)$. Triggs and colleagues point out that reweighting reaches the true minimum of the robust cost but can converge more slowly than a step that also models how the weights change with the residual. Losses that flatten out, such as the Cauchy loss, are not convex, so a robust cost does not remove the need for a good start; gross mismatches are usually removed first with RANSAC.

Sparsity

In vision problems each residual usually depends on only a few parameters. In bundle adjustment one reprojection residual involves the parameters of one camera and one 3D point, so the Jacobian has two nonzero blocks per row and $J^\top J$ has a characteristic structure: the point–point block is block diagonal with $3 \times 3$ blocks, since points couple only through cameras. Triggs and colleagues describe how eliminating the point parameters with the Schur complement leaves a reduced camera system, much smaller than the full one, and trace this reduction to Brown’s 1958 method. Calibration has similar structure: each detected corner depends on the shared intrinsics and distortion coefficients and on the pose of a single view. Dense factorization cost grows with the cube of the number of parameters, so exploiting this structure is what makes large reconstructions practical.

Examples

Fitting a circle

The code fits a circle to 30 noisy points on a quarter arc by minimizing the geometric residuals $r_i = \lVert \mathbf{p}_i - \mathbf{c} \rVert - R$, the distances of the points from the circle. It starts Gauss–Newton from an algebraic fit, which is linear least squares, then runs undamped Gauss–Newton and Levenberg–Marquardt from 1,000 random starting points.

import numpy as np

rng = np.random.default_rng(1)

# 30 noisy points on a 90-degree arc of the circle with centre (2, -1) and radius 5.
theta = rng.uniform(0, np.pi / 2, 30)
pts = np.column_stack([2 + 5 * np.cos(theta), -1 + 5 * np.sin(theta)])
pts += rng.normal(0, 0.05, pts.shape)

def residuals(p):
    """Geometric residuals r_i = |p_i - c| - R for parameters p = (cx, cy, R)."""
    return np.linalg.norm(pts - p[:2], axis=1) - p[2]

def jacobian(p):
    diff = p[:2] - pts
    d = np.linalg.norm(diff, axis=1)
    return np.column_stack([diff / d[:, None], -np.ones(len(pts))])

def cost(p):
    return 0.5 * np.sum(residuals(p) ** 2)

def gauss_newton(p, iters=50):
    for _ in range(iters):
        r, J = residuals(p), jacobian(p)
        p = p + np.linalg.solve(J.T @ J, -J.T @ r)   # full, undamped step
    return p

def levenberg_marquardt(p, iters=200, lam=1e-3):
    for _ in range(iters):
        r, J = residuals(p), jacobian(p)
        A, g = J.T @ J, J.T @ r
        while lam < 1e12:
            h = np.linalg.solve(A + lam * np.eye(3), -g)
            if cost(p + h) < cost(p):
                p, lam = p + h, lam / 3      # accepted: move toward Gauss-Newton
                break
            lam *= 2                         # rejected: shorter, more gradient-like step
    return p

# Linear initialization: x^2 + y^2 + D x + E y + F = 0 is linear in (D, E, F).
A = np.column_stack([pts, np.ones(len(pts))])
D, E, F = np.linalg.lstsq(A, -(pts ** 2).sum(axis=1), rcond=None)[0]
c0 = np.array([-D / 2, -E / 2])
p = np.array([*c0, np.sqrt(c0 @ c0 - F)])

print("Gauss-Newton from the algebraic fit")
for k in range(4):
    print(f"  iter {k}: (cx, cy, R) = {np.round(p, 4)}, cost = {cost(p):.10f}")
    p = gauss_newton(p, iters=1)
p_star = p

# 1000 random starting points in a box around the data.
rs = np.random.default_rng(2)
starts = np.column_stack([rs.uniform(-5, 10, (1000, 2)), rs.uniform(0.5, 10, 1000)])
with np.errstate(all="ignore"):
    for method in (gauss_newton, levenberg_marquardt):
        found, other = 0, []
        for p0 in starts:
            try:
                p = method(p0)
            except np.linalg.LinAlgError:
                continue
            if np.allclose(p, p_star, atol=1e-4):
                found += 1
            elif np.isfinite(p).all():
                other.append(p)
        print(f"{method.__name__:20s} reaches the minimum from {found} of 1000 starts")
other = np.array(other)
print(f"LM failures: smallest |R| = {np.abs(other[:, 2]).min():.3g}, "
      f"median cost = {np.median([cost(p) for p in other]):.4f}")

# Cost of the best straight line (perpendicular distances), the limit R -> infinity.
s = np.linalg.svd(pts - pts.mean(axis=0), compute_uv=False)
print(f"best straight-line cost = {0.5 * s[-1] ** 2:.4f}")

Output:

Gauss-Newton from the algebraic fit
  iter 0: (cx, cy, R) = [ 2.0096 -0.9784  4.9785], cost = 0.0213382286
  iter 1: (cx, cy, R) = [ 1.9867 -1.0047  5.0098], cost = 0.0212016445
  iter 2: (cx, cy, R) = [ 1.9865 -1.0049  5.0101], cost = 0.0212016292
  iter 3: (cx, cy, R) = [ 1.9865 -1.0049  5.0101], cost = 0.0212016292
gauss_newton         reaches the minimum from 333 of 1000 starts
levenberg_marquardt  reaches the minimum from 597 of 1000 starts
LM failures: smallest |R| = 1.21e+06, median cost = 2.7352
best straight-line cost = 2.7352

From the algebraic fit, Gauss–Newton settles in two iterations: the start is close and the residuals are small. From random starts, undamped Gauss–Newton reaches the minimum a third of the time and LM from most starts, but not all. Every LM failure ends with a radius above a million and the cost of the best straight line: the circle has flattened into a line, a limit at infinity that a descent method cannot leave. Damping widens the region of successful starts; it does not replace a good one.

Common Misconceptions

• “Levenberg–Marquardt finds a better solution than Gauss–Newton.” Both minimize the same cost and, when both converge, reach the same minimum, as the example shows. Damping changes the path and the set of starting points that work, not the estimator.
• “Convergence means the global minimum.” The methods stop at a stationary point, a local minimum at best. Comparing the final residuals with the expected noise level is a cheap check.
• “Damping fixes an underdetermined problem.” It makes every linear system solvable, but parameters the data do not constrain stay undetermined. In bundle adjustment the overall position, orientation, and scale of a calibrated reconstruction are such a freedom: Triggs and colleagues note that the full Hessian is singular until this gauge is fixed.

Where It Is Used

• Camera calibration: Zhang’s method refines a closed-form estimate of intrinsics, distortion, and poses by Levenberg–Marquardt minimization of reprojection error.
• Triangulation: a linear estimate of each 3D point is refined by Gauss–Newton or LM on the reprojection error, solving a $3 \times 3$ system per iteration.
• Homography and fundamental matrix estimation: a normalized linear estimate, often from the inliers of RANSAC, initializes the minimization of a geometric error.
• Pose estimation and bundle adjustment: camera pose from known 3D points (the perspective-n-point problem) is refined the same way, and bundle adjustment refines all cameras and points of a reconstruction jointly, as the core of structure from motion and of many visual SLAM systems.
• Image alignment: the iterative Lucas–Kanade method is a Gauss–Newton method on the brightness difference between a template and a warped image.