Math 1 · Linear algebra & least squares

Why this chapter exists. Every iteration of every GTSAM optimizer ends in the same place: solve a large, sparse, linear least-squares problem. Everything clever in GTSAM (elimination, Bayes trees, orderings, iSAM2) is a way to do that one thing faster or incrementally. OMPL needs much less of it, only distances, projections, and a little SVD, but the language is the same.

1. Vectors and matrices as maps

A vector \(x\in\R^n\) is a list of \(n\) numbers. A matrix \(A\in\R^{m\times n}\) is a linear map from \(\R^n\) to \(\R^m\). Read the product \(Ax\) in two ways:

• Column picture: \(Ax = x_1 a_1 + x_2 a_2 + \dots + x_n a_n\), a weighted combination of the columns \(a_j\) of \(A\). Solving \(Ax=b\) asks: which combination of columns produces \(b\)?
• Row picture: the \(i\)-th entry of \(Ax\) is \(\langle \text{row}_i, x\rangle\). Each row is one equation, which in GTSAM means one scalar measurement.

In estimation, columns correspond to unknowns (variables) and rows to measurements. A pose-graph Jacobian with 1,000 3-DoF poses and 1,500 odometry/loop measurements is a \(4500\times 3000\) matrix, with each row block touching only 2 column blocks.

Matrix multiplication is composition of maps, \((AB)x = A(Bx)\). It is associative, so you can bracket a chain however is cheapest, but not commutative: \(AB\ne BA\) in general. Rotations will make this concrete in chapter 2.

2. The four fundamental subspaces, rank, and why gauge freedom is a null space

For \(A\in\R^{m\times n}\):

The rank–nullity theorem says \(\dim\mathcal{R}(A) + \dim\mathcal{N}(A) = n\). The row space and null space are orthogonal complements in \(\R^n\).

Why you care. If \(\mathcal{N}(A)\ne\{0\}\), then \(x\) and \(x + n\) (any \(n\) in the null space) fit the data equally well, so the solution is not unique. In SLAM this happens whenever all measurements are relative: odometry and loop closures only constrain poses relative to each other, so translating and rotating the whole map changes nothing. That null space is called the gauge freedom. It is 3-dimensional for 2D pose graphs, 6 for 3D, and 7 for monocular SfM (scale too). The fix is to add a prior factor on one pose, which adds rows that make \(A\) full column rank. If you forget, GTSAM's Cholesky hits a zero pivot and throws IndeterminateSystemException (gtsam/linear/linearExceptions.h).

3. Inner products, norms, orthogonality

The inner product is \(\langle x,y\rangle = x^\top y\), and the norm is \(\lVert x\rVert = \sqrt{x^\top x}\). Two vectors are orthogonal if \(x^\top y=0\). The Cauchy–Schwarz inequality \(\lvert x^\top y\rvert\le\lVert x\rVert\lVert y\rVert\) gives angles: \(\cos\theta = x^\top y/(\lVert x\rVert\lVert y\rVert)\).

A square matrix \(Q\) is orthogonal if \(Q^\top Q = I\), i.e. its columns are orthonormal. Then:

• \(Q^{-1}=Q^\top\) (inverting costs nothing),
• \(\lVert Qx\rVert = \lVert x\rVert\) (it preserves lengths, since \(x^\top Q^\top Qx = x^\top x\)), and therefore angles too,
• \(\det Q = \pm1\).

Rotation matrices are exactly the orthogonal matrices with \(\det=+1\) (chapter 2). Length preservation is also why QR factorization is numerically safe (§8).

A weighted inner product \(\langle x,y\rangle_W = x^\top W y\) with \(W\) symmetric positive definite gives the norm \(\lVert x\rVert_W^2 = x^\top Wx\). With \(W=\Sigma^{-1}\) this is the Mahalanobis norm that measures residuals in "number of standard deviations" (chapter 3).

4. Projection: the geometry of least squares

Problem. \(Ax=b\) with \(m>n\) (more measurements than unknowns) usually has no exact solution, because \(b\) isn't in the column space. Find the \(x\) that makes \(Ax\) closest to \(b\):

Geometric derivation. The closest point to \(b\) in the plane \(\mathcal{R}(A)\) is the orthogonal projection \(p = A\hat x\). The residual \(r = b - A\hat x\) must be perpendicular to every column of \(A\), otherwise you could move along that column and get closer:

Calculus derivation. \(f(x) = \tfrac12(Ax-b)^\top(Ax-b) = \tfrac12 x^\top A^\top Ax - b^\top Ax + \tfrac12 b^\top b\). Its gradient is \(\nabla f = A^\top Ax - A^\top b\), and setting it to zero gives the same equations. The Hessian is \(A^\top A\), which is positive semidefinite, so any stationary point is a minimum. It is unique iff \(A\) has full column rank (§2).

The matrix \(P = A(A^\top A)^{-1}A^\top\) is the projector onto \(\mathcal{R}(A)\). It satisfies \(P^2 = P\) and \(P^\top=P\).

5. Weighted least squares and whitening

Measurements have different precisions. A gyro is better than a wheel encoder, and a loop closure may be worse than odometry. With noise covariance \(\Sigma\), the right objective is

Factor the information matrix as \(\Sigma^{-1} = W^\top W\) (e.g. Cholesky, §9). Then

which is an ordinary least-squares problem in the whitened quantities \(WA\), \(Wb\). For diagonal \(\Sigma = \diag(\sigma_i^2)\), whitening divides row \(i\) by \(\sigma_i\): precise measurements get large rows and dominate the fit.

In GTSAM every factor is whitened at linearization time, so all downstream linear algebra sees unit covariance. noiseModel::Gaussian stores exactly this \(W\) (its thisR(), with \(W^\top W = \Sigma^{-1}\)), and NoiseModelFactor::linearize calls noiseModel_->WhitenSystem(A, b) (gtsam/nonlinear/NonlinearFactor.cpp). Diagonal and isotropic models just scale rows.

6. Conditioning: when small errors become big ones

The condition number \(\kappa(A) = \sigma_{\max}/\sigma_{\min}\) (ratio of largest to smallest singular value, §11) bounds how much relative input error can be amplified into relative output error. In double precision (about 16 digits), solving a system with \(\kappa\approx 10^k\) costs about \(k\) digits of accuracy.

The crucial fact:

Forming the normal equations squares the condition number. A Jacobian with \(\kappa=10^8\), which is not unusual when mixing millimetre and kilometre scales or near-degenerate geometry, becomes a \(10^{16}\) problem, and you lose every digit.

That is why GTSAM offers both QR-based (EliminateQR, works on \(A\)) and Cholesky-based (EliminateCholesky, works on \(A^\top A\)) elimination. Cholesky is faster and is the default (EliminatePreferCholesky); QR is the robust fallback.

7. Triangular systems and back-substitution

If \(R\) is upper triangular with nonzero diagonal, \(Rx = d\) is solved from the bottom up:

This costs \(O(n^2)\) for dense \(R\), and much less when \(R\) is sparse. All of GTSAM's direct solving ends with this: GaussianConditional::solve is one block row of back-substitution, and GaussianBayesTree::optimize runs it clique by clique from the root down.

8. QR factorization (and how GTSAM's EliminateQR works)

Goal. Factor \(A = QR\) with \(Q\) orthogonal (\(m\times m\)) and \(R\) upper triangular (\(m\times n\), zero below row \(n\)).

Why it solves least squares. Because \(Q^\top\) preserves norms:

The second term doesn't depend on \(x\), so the minimizer solves the square triangular system \(R_1\hat x = d\), and the leftover residual is \(\lVert e\rVert^2\). You never form \(A^\top A\), so there is no condition-number squaring.

How to compute it: Householder reflections. A reflection across the hyperplane orthogonal to \(v\) is \(H = I - 2\frac{vv^\top}{v^\top v}\). It is symmetric and orthogonal, \(H^2=I\). Choose \(v = a - \alpha e_1\) with \(\alpha = -\operatorname{sign}(a_1)\lVert a\rVert\) (sign chosen to avoid cancellation). Then \(Ha = \alpha e_1\): the whole column collapses onto its first entry. Apply \(H_1\) to zero column 1 below the diagonal, then \(H_2\) to the trailing submatrix to zero column 2, and so on:

The cost is about \(2mn^2 - \tfrac23 n^3\) flops. You never need \(Q\) explicitly: append \(b\) as an extra column and the same reflections produce \(Q^\top b = (d, e)\) for free.

This is literally EliminateQR (gtsam/linear/JacobianFactor.cpp). It stacks all factors touching the variables being eliminated into one matrix \([A_{\text{frontal}} \mid A_{\text{separator}} \mid b]\) (a VerticalBlockMatrix), runs in-place Householder QR on it, and splits the result:

Because QR is applied block by block, eliminating one group of variables at a time is the same as doing sparse QR on the whole system. Chapter 5 builds on this.

9. Cholesky factorization (and why it is "repeated Schur complements")

Positive definite. A symmetric \(H\) is positive definite (PD) if \(x^\top Hx>0\) for all \(x\ne0\). Equivalently, all its eigenvalues are positive. \(H=A^\top A\) is always positive semidefinite, since \(x^\top A^\top Ax = \lVert Ax\rVert^2\ge0\), and PD iff \(A\) has full column rank.

Theorem. Every PD matrix factors uniquely as \(H = R^\top R\) with \(R\) upper triangular and positive diagonal.

Derivation by peeling off the first variable. Write \(H\) in blocks with scalar \(a\):

Multiply it out to check. The middle block \(K - ww^\top/a\) is the Schur complement of \(a\) (§10). It is again PD, so recurse on it. Each step produces one row of \(R\), \([\sqrt a,\; w^\top/\sqrt a]\), and a smaller matrix. The algorithm (right-looking Cholesky):

for k = 1..n:
    r_kk = sqrt(H_kk)                       # fails if H_kk ≤ 0  → matrix not PD (rank-deficient)
    r_k,j = H_kj / r_kk           for j > k
    H_ij -= r_k,i * r_k,j         for i,j > k   # Schur complement update: this is where fill-in appears

The cost is \(\tfrac13 n^3\) flops, about half of LU and less than QR. The key conceptual point: one Cholesky step is the same thing as eliminating one variable. It expresses that variable in terms of the rest (a row of \(R\), i.e. a conditional) and updates the remaining system (the Schur complement, i.e. the marginal). Chapters 3 and 5 return to this repeatedly.

In GTSAM: HessianFactor stores \(\begin{bmatrix}H & -g\\ -g^\top & f\end{bmatrix}\) as a SymmetricBlockMatrix, and EliminateCholesky runs a partial Cholesky on the frontal block (gtsam/base/cholesky.h, choleskyPartial), leaving the Schur complement as the new factor on the separator.

Variant. \(LDL^\top\) (unit lower triangular \(L\), diagonal \(D\)) avoids square roots and handles some indefinite matrices.

10. The Schur complement: the most important identity in this whole site

Take a block linear system:

Solve the first row for \(x\), giving \(x = A^{-1}(a - By)\), and substitute into the second:

\(S = C - B^\top A^{-1}B\) is the Schur complement of \(A\). You have eliminated \(x\) and got a smaller system in \(y\) alone. Once \(y\) is known, recover \(x = A^{-1}(a-By)\) by back-substitution.

It shows up everywhere in both libraries:

Block inverse and the matrix inversion lemma. Applying the same elimination to the inverse gives

Eliminating in the other order and equating the top-left blocks gives the Woodbury identity, \((A + UCV)^{-1} = A^{-1} - A^{-1}U(C^{-1} + VA^{-1}U)^{-1}VA^{-1}\). This is exactly the algebra that turns the Kalman filter's covariance-form update into the information-form update (chapter 3). Note also: the bottom-right block of the inverse is \(S^{-1}\). In probability terms, the marginal covariance of \(y\) is the inverse of the Schur complement of the information matrix. Keep that in mind for chapter 3.

11. Eigenvalues, the SVD, and their uses

Symmetric eigendecomposition. A real symmetric \(H\) has an orthonormal basis of eigenvectors: \(H = V\Lambda V^\top\) with \(V\) orthogonal and \(\Lambda\) diagonal and real. \(H\) is PD iff all \(\lambda_i>0\). The quadratic form \(x^\top Hx\) has ellipsoidal level sets, with axes along the eigenvectors and semi-axis lengths \(\propto 1/\sqrt{\lambda_i}\). This is how covariance ellipses are drawn (chapter 3).

Singular value decomposition. Every matrix factors as \(A = U\Sigma V^\top\) with \(U, V\) orthogonal and \(\Sigma\) diagonal with \(\sigma_1\ge\dots\ge\sigma_r>0\). Geometrically, every linear map is rotate → scale along axes → rotate. Uses:

• \(\rank A\) is the number of nonzero \(\sigma_i\). Numerically, count those above a tolerance. This is how you detect degenerate configurations (e.g. triangulation with parallel rays).
• The null space is spanned by the columns of \(V\) whose \(\sigma_i=0\). GTSAM's "indeterminate system" notebook (gtsam/linear/doc/IndeterminateSystemException.ipynb) uses this to find missing gauge priors.
• \(\kappa(A) = \sigma_1/\sigma_r\) (§6).
• Pseudo-inverse \(A^+ = V\Sigma^{+}U^\top\), the minimum-norm least-squares solution. It is used for constrained-manifold projection in OMPL (chapter 6).
• Nearest rotation (orthogonal Procrustes). Given an arbitrary \(3\times3\) matrix \(M\), the rotation minimizing \(\lVert R - M\rVert_F\) is \(R = U\diag(1,1,\det(UV^\top))V^\top\) from \(M=U\Sigma V^\top\). The determinant fix prevents returning a reflection. This is used in Rot3::ClosestTo, in chordal rotation initialization (InitializePose3), and in point-set alignment (Pose3::Align, Kabsch/Umeyama).
• Smallest eigenvalue / power method. Shonan averaging certifies global optimality by checking the minimum eigenvalue of a certificate matrix (gtsam/linear/PowerMethod.h, AcceleratedPowerMethod.h, Spectra).

12. Skew-symmetric matrices and the cross product

For \(\omega\in\R^3\) define

The properties you will use constantly in chapter 2 (prove each by expanding):

1. \(\skew{\omega}^\top = -\skew{\omega}\) (skew-symmetric), so \(v^\top\skew{\omega}v = 0\).
2. \(\skew{\omega}v = -\skew{v}\omega\) (anticommutativity of \(\times\)). This lets you move the variable you're differentiating with respect to into the vector slot.
3. \(\skew{\omega}^2 = \omega\omega^\top - \lVert\omega\rVert^2 I\).
4. \(\skew{\omega}^3 = -\lVert\omega\rVert^2\skew{\omega}\). This makes the series for the matrix exponential collapse.
5. \(\skew{R\omega} = R\skew{\omega}R^\top\) for any rotation \(R\). This gives the adjoint of SO(3).

GTSAM calls it skewSymmetric(w) (gtsam/base/Matrix.h). Eigen has no built-in, so every port needs this helper.

13. Sparse matrices and fill-in

Structure of SLAM matrices. Order the columns of \(A\) by variable and its rows by factor. A factor touching variables \(i\) and \(j\) contributes a row block with nonzeros only in column blocks \(i\) and \(j\). Consequently:

• \(A\) has \(O(\#\text{factors})\) nonzero blocks,
• \(H = A^\top A\) has a nonzero block \((i,j)\) iff some factor touches both \(i\) and \(j\). Its sparsity pattern is the adjacency matrix of the variable graph (the "Markov random field", chapter 5).

For an odometry chain \(x_1 - x_2 - \dots - x_n\), \(H\) is block-tridiagonal. A loop closure between \(x_1\) and \(x_n\) adds blocks \((1,n)\) and \((n,1)\).

Fill-in. Cholesky's update \(H_{ij} \mathrel{-}= r_{ki}r_{kj}\) (§9) creates a nonzero at \((i,j)\) whenever \(i\) and \(j\) were both coupled to the eliminated variable \(k\), even if \(H_{ij}\) was zero. In graph terms, eliminating a variable connects all of its neighbours to each other. The order of elimination decides how much fill appears.

Finding the fill-minimizing order is NP-hard (Yannakakis, 1981), so solvers use heuristics: minimum degree (always eliminate the variable with the fewest current neighbours), its fast approximation AMD/COLAMD, and nested dissection (METIS). Chapter 5 covers these properly. In GTSAM: Ordering::Colamd (default), Ordering::Metis, and the vendored gtsam/3rdparty/CCOLAMD.

Storage. General sparse matrices are usually stored column-compressed (CSC: values + row indices + column pointers). GTSAM mostly avoids a global sparse matrix. It keeps the factor graph itself as the sparse structure, with small dense blocks per factor (VerticalBlockMatrix, SymmetricBlockMatrix). It only builds a global sparse matrix for external solvers (SparseEigen.h, CHOLMOD, cuDSS).

14. Cost cheat-sheet

For sparse problems the costs depend on the fill of \(R\), not on \(n\) directly. That is the subject of chapter 5.

15. Check yourself

1. Why is the least-squares residual orthogonal to the column space? Derive the normal equations without calculus.
2. A 3D pose graph with only BetweenFactors has how many zero singular values in its Jacobian? What fixes it?
3. Derive one step of Cholesky and identify the Schur complement in it.
4. Show \(\kappa(A^\top A)=\kappa(A)^2\) using the SVD.
5. Eliminate the arrow matrix's leaves one by one and confirm no fill appears.
6. Prove \(\skew{R\omega} = R\skew{\omega}R^\top\). (Hint: \(R(a\times b) = Ra\times Rb\) for rotations.)