Math 4 · Nonlinear optimization

Why this chapter exists. Chapter 3 turned estimation into \(\min_X\sum_k\tfrac12\lVert r_k(X)\rVert^2\), a nonlinear least-squares problem over a product of manifolds. No closed form exists, so we iterate: build a local model, solve it, step, repeat. This chapter derives the three methods GTSAM ships (Gauss–Newton, Levenberg–Marquardt, Dogleg), shows how they work on manifolds, and connects every parameter in LevenbergMarquardtParams, DoglegParams, and GncParams to the math.

1. The problem and its derivatives

Stack all whitened residuals into one vector \(r(x)\in\R^m\). The objective is

With \(J = \partial r/\partial x\in\R^{m\times n}\) (the stacked Jacobian):

Derivation: \(\partial F/\partial x_j = \sum_ir_i\,\partial r_i/\partial x_j\), which is \((J^\top r)_j\). Differentiate again with the product rule to get the two terms. The first term, \(J^\top J\), needs only first derivatives. The second needs second derivatives of every residual, and it is small when the residuals \(r_i\) are small (a good fit) or the residuals are nearly linear (\(\nabla^2r_i\approx0\)).

Optimality conditions. At a local minimum \(x^\star\): \(\nabla F(x^\star)=0\) (first order) and \(\nabla^2F(x^\star)\succeq0\) (second order). These are local guarantees only. Rotations make \(F\) non-convex, so many local minima can exist (§11).

2. Gradient descent: simple and slow

Step along the negative gradient, \(x\leftarrow x-\alpha\nabla F\). It always decreases \(F\) for small enough \(\alpha\), but on an elongated valley (Hessian condition number \(\kappa\)) it zig-zags. Its linear convergence rate is about \(\big(\tfrac{\kappa-1}{\kappa+1}\big)\) per step. SLAM problems routinely have \(\kappa\sim10^6\) and more, so plain gradient descent is unusable. It does reappear as one ingredient of LM and Dogleg.

3. Newton's method

Approximate \(F\) by its second-order Taylor model at \(x\) and jump to the model's minimum:

It converges quadratically near a minimum (the number of correct digits roughly doubles per iteration). However, it needs second derivatives of every residual, and \(\nabla^2F\) may be indefinite far from the minimum, in which case the step can go uphill.

4. Gauss–Newton: linearize the residual, not the cost

Instead of a quadratic model of \(F\), use a linear model of \(r\): \(r(x+\delta)\approx r + J\delta\). Then

a linear least-squares problem in \(\delta\) (chapter 1):

This is Newton's method with \(\nabla^2F\) replaced by \(J^\top J\), i.e. with the second-order residual term dropped (§1). Properties:

• Only first derivatives are needed: exactly the Jacobians every GTSAM factor provides.
• \(J^\top J\succeq0\) always. If \(J\) has full column rank the step is a descent direction: \(\nabla F^\top\delta_{gn} = -\delta_{gn}^\top J^\top J\delta_{gn}<0\).
• Convergence is quadratic for zero-residual problems and linear otherwise, fast when the fit is good.
• Failure mode: the linear model is only trustworthy nearby. A full GN step from a poor estimate can increase \(F\) (overshoot), and GN has no mechanism to detect or prevent that.

In GTSAM, GaussNewtonOptimizer::iterate() is literally: graph.linearize(values) (a GaussianFactorGraph whose factors are the whitened \((J_k, -r_k)\) blocks), then optimize (sparse elimination solving \(J^\top J\delta=-J^\top r\), chapter 5), then values.retract(delta).

5. Optimizing on manifolds: one change

Variables live on manifolds (chapter 2), so "\(x+\delta\)" becomes "\(x\oplus\delta\)":

1. Linearize each factor in the tangent space at the current estimate: \(r_k(x\oplus\delta)\approx r_k(x) + J_k\delta\), where \(J_k\) are the manifold Jacobians of chapter 2 §13 (what evaluateError returns via OptionalJacobian).
2. Solve the linear least-squares problem for the stacked tangent step \(\delta = (\delta_1,\dots,\delta_n)\), a VectorValues with one tangent vector per variable.
3. Retract each variable separately: \(x_j\leftarrow x_j\oplus\delta_j\) (Values::retract calls each variable's traits<T>::Retract).

Nothing else changes. The linear algebra never sees a rotation matrix, only tangent vectors and Jacobians. The next linearization happens at the new point, in the new tangent spaces, which is why errors from the first-order approximation don't accumulate. This "lift → solve → retract" loop is the whole optimization layer of GTSAM.

6. Trust regions and the gain ratio

To fix GN's overshooting, trust the model only within a region:

with \(g = J^\top r\), \(B = J^\top J\). After computing a step, compare actual and predicted decrease:

• \(\rho\approx1\): the model is accurate, so trust it more.
• \(\rho\) small or negative: the model lied, so trust it less.

The predicted decrease has a neat form: \(m(0)-m(\delta) = \tfrac12\lVert r\rVert^2 - \tfrac12\lVert r+J\delta\rVert^2\), the drop in the linearized error. That is what GTSAM computes as linearizedCostChange, the error of the (undamped) linear graph at \(0\) minus at \(\delta\). GTSAM calls \(\rho\) modelFidelity in LM.

7. Levenberg–Marquardt

7.1 The equations

7.2 Three ways to understand it

1. An interpolation. \(\lambda\to0\) gives the Gauss–Newton step. \(\lambda\to\infty\) gives \(\delta\approx-\tfrac1\lambda J^\top r\), a short gradient-descent step. One knob slides between fast-but-reckless and slow-but-safe.
2. A trust region in disguise. The KKT conditions of the trust-region problem in §6 are \((B+\lambda I)\delta = -g\), \(\lambda\ge0\), \(\lambda(\lVert\delta\rVert-\Delta)=0\). Every \(\lambda\) corresponds to some radius \(\Delta\), and larger \(\lambda\) means a smaller region.
3. A prior on the step. \((J^\top J+\lambda I)\delta = -J^\top r\) is the normal equation of

i.e. the linearized problem plus a zero-mean Gaussian prior on each variable's step with \(\sigma = 1/\sqrt\lambda\).

GTSAM implements view 3 literally. LevenbergMarquardtState::buildDampedSystem copies the linear factor graph and appends one JacobianFactor per variable, with \(A=I\), \(b=0\), and an isotropic noise model of sigma \(1/\sqrt\lambda\). With diagonalDamping, \(A=\diag\big(\sqrt{(J^\top J)_{jj}}\big)\), clamped to [minDiagonal, maxDiagonal]. Then it eliminates the augmented graph with the ordinary sparse solver. The damping needs no special linear algebra; it's just more factors.

Why Marquardt's \(D=\diag(J^\top J)\)? It makes LM invariant to rescaling the variables. With \(D=I\), a variable measured in millimetres is damped very differently from the same variable in kilometres. Ceres uses diagonal damping by default; GTSAM's legacy default does not, while SetCeresDefaults turns it on.

7.3 The λ schedule, exactly as GTSAM runs it

LevenbergMarquardtOptimizer::iterate() linearizes once, then loops in tryLambda until a step is accepted:

build damped system for current λ, solve for δ
predicted = linear_error(0) − linear_error(δ)          # undamped linear model
if predicted ≥ 0:
    actual = F(x) − F(x ⊕ δ)
    ρ = actual / predicted                                # "modelFidelity"
    accept = ρ > minModelFidelity                         # default 1e-3
if accept:  x ← x ⊕ δ;  decrease λ
else:       increase λ;  if λ ≥ lambdaUpperBound: stop

Two λ policies (gtsam/nonlinear/internal/LevenbergMarquardtPolicy.h):

Nielsen's rule decreases λ a lot when \(\rho\approx1\) and barely when \(\rho\approx\tfrac12\), and on repeated failures it increases λ by growing factors. Default parameter sets:

Cost per inner iteration. Each new λ means a new numerical factorization. The ordering and symbolic structure are reused, and GTSAM 4.3's NonlinearMultifrontalSolver/MultifrontalSolver caches them explicitly. A port that reproduces this loop exactly will match GTSAM's iteration counts; one that perturbs it (a different ρ test, a different λ update) will still converge, but its iterates won't be comparable during debugging.

8. Powell's Dogleg

Dogleg solves the trust-region problem approximately using two special points, without refactorizing for each radius.

The Cauchy point: minimize the model along the steepest descent direction. With \(\delta = -\alpha g\), \(m(-\alpha g) = F - \alpha g^\top g + \tfrac12\alpha^2g^\top Bg\), minimized at

The Gauss–Newton point \(\delta_{gn}\) from §4.

The dogleg path runs from \(0\) to \(\delta_{sd}\) to \(\delta_{gn}\). Choose the step:

1. If \(\lVert\delta_{gn}\rVert\le\Delta\): take \(\delta_{gn}\).
2. Else if \(\lVert\delta_{sd}\rVert\ge\Delta\): take \(\frac{\Delta}{\lVert g\rVert}(-g)\), steepest descent clipped to the boundary.
3. Else take \(\delta_{sd}+\tau(\delta_{gn}-\delta_{sd})\) with \(\tau\in[0,1]\) solving \(\lVert\delta_{sd}+\tau(\delta_{gn}-\delta_{sd})\rVert = \Delta\), a scalar quadratic.

Radius update, from DoglegOptimizerImpl.h: if \(\rho\ge0.75\), \(\Delta\leftarrow\max(\Delta, 3\lVert\delta\rVert)\); if \(0.25\le\rho<0.75\), keep \(\Delta\); if \(0\le\rho<0.25\), \(\Delta\leftarrow\Delta/2\) (step still accepted); if \(\rho<0\), reject, halve \(\Delta\) and retry, stopping at \(\Delta\approx10^{-5}\). DoglegParams::deltaInitial = 1.0.

Why iSAM2 uses Dogleg and not LM. LM needs a new factorization for every trial λ. Dogleg needs only \(\delta_{gn}\) (back-substitution on the existing Bayes tree) and \(g\) (computable from the Bayes tree via gradientAtZero). Changing \(\Delta\) costs nothing. In an incremental setting where the factorization is updated rather than recomputed, that is decisive. ISAM2Params therefore offers ISAM2GaussNewtonParams (default) or ISAM2DoglegParams.

9. Robustness: IRLS and Graduated Non-Convexity

IRLS inside GN/LM. With robust kernels (chapter 3 §12), each linearization recomputes the weights \(w_k = w(\lVert r_k\rVert)\) at the current estimate and scales the factor's rows by \(\sqrt{w_k}\). The outer GN/LM iterations are therefore also the IRLS iterations; no extra loop is needed.

The non-convexity problem. Redescending kernels (Geman–McClure, TLS, Tukey) reject outliers fully but create many local minima. Start far away and the optimizer may decide the inliers are the outliers.

Graduated Non-Convexity (GNC), Yang et al. 2020, GncOptimizer, introduces a family of surrogate costs \(\rho_\mu\) controlled by a parameter \(\mu\), such that

• at one end of \(\mu\), \(\rho_\mu\) is convex (essentially least squares, so there is a unique solution),
• at the other end, \(\rho_\mu\) is the true robust cost.

Solve the convex problem first, then gradually change \(\mu\), warm-starting each solve from the last. The global structure is found while the problem is easy, and outliers are progressively down-weighted. Thanks to the Black–Rangarajan duality, each stage is a weighted least-squares problem with closed-form weights:

• GM: the shape parameter \(\lambda\) starts large and is divided by lambdaStep = 1.4 until it reaches 1 (the true GM).
• TLS (truncated least squares, \(\rho(r)=\min(r^2,\bar c^2)\)): \(\lambda\) starts near 0 and is multiplied by 1.4. The weights become exactly 0 or 1 at the end, giving a hard inlier/outlier split.

The inlier threshold \(\bar c\) is set from the χ² inverse CDF at a chosen confidence (chapter 3 §12). GNC wraps an inner GaussNewtonOptimizer or LevenbergMarquardtOptimizer (the template parameter of GncOptimizer<GncParameters<...>>), and can be told which factors are known inliers (priors, odometry).

10. Stopping criteria

NonlinearOptimizerParams (checkConvergence in NonlinearOptimizer.cpp) stops when any of these holds:

• relative decrease: \((F_{old}-F_{new})/F_{old} <\) relativeErrorTol (1e-5),
• absolute decrease: \(F_{old}-F_{new} <\) absoluteErrorTol (1e-5),
• small error: \(F_{new} <\) errorTol (0),
• iteration cap: maxIterations (100).

The error here is \(F = \tfrac12\sum\lVert r\rVert^2\) in whitened units. Scale matters: a problem with \(10^6\) factors has a large \(F\) even at the optimum, so the absolute tolerance is effectively never the binding one. A port must use the same error definition, including the ½, or its tolerances will mean something different.

11. Initialization: the part the optimizer can't do for you

GN/LM/Dogleg are local methods: they converge to the minimum of the basin they start in. Rotations make SLAM non-convex. For example, a pose graph initialized with 180° heading errors converges to a twisted, wrong map. Practical initialization strategies, several of them in GTSAM:

Typical robust pipeline: initialize rotations globally (chordal or Shonan), then translations, then run LM or GNC on the full problem.

12. Cost of one iteration

1. Linearize: \(O(\#\text{factors})\) evaluations of evaluateError with Jacobians. This is parallel across factors, and dominant for factors with expensive models (projection, IMU).
2. Eliminate: sparse factorization, whose cost depends on the fill of \(R\), set by the ordering (chapter 5). For a 2D grid-like SLAM graph with \(n\) variables and nested dissection, it is \(O(n^{1.5})\). For a chain, \(O(n)\).
3. Back-substitute and retract: \(O(\text{nnz}(R))\) plus \(O(n)\).

LM pays step 2 once per inner iteration. GN and Dogleg pay it once per outer iteration. iSAM2 pays it only for the part of the Bayes tree touched by new information (chapter 5 §11).

13. Check yourself

1. Derive \(\nabla F\) and \(\nabla^2F\) for \(F=\tfrac12\lVert r\rVert^2\), and explain when dropping \(\sum r_i\nabla^2r_i\) is safe.
2. Show that the LM step is the solution of the linearized problem plus the prior \(\tfrac12\lambda\lVert\delta\rVert^2\), and identify the prior's σ.
3. Derive the Cauchy step length \(\alpha^\star = \lVert g\rVert^2/\lVert Jg\rVert^2\).
4. Why can Dogleg change its trust region without refactorizing, while LM cannot change λ without refactorizing?
5. A port's LM converges in 12 iterations where GTSAM takes 9. List three implementation details that could explain the difference.
6. Why does GNC start from the convex surrogate, and what does each stage's warm start buy you?