Mathematical Background

Four pieces of mathematics recur in almost every chapter. This appendix states each in the symbols of the Notation table and names the chapters that use it. They are vectors and distances, rigid transformations, the covariance matrix with its eigendecomposition, and least-squares estimation together with the uncertainty it propagates.

A.1 Vectors, Distances, and Products

We write a point $\boldsymbol{p}$ in 3D space as a column vector:

$$\boldsymbol{p} = \begin{pmatrix} x \\ y \\ z \end{pmatrix} \in \mathbb{R}^3 \tag{A.1}$$

We measure the separation of two points $\boldsymbol{p}$ and $\boldsymbol{q}$ by the Euclidean distance:

$$d(\boldsymbol{p}, \boldsymbol{q}) = \left\lVert \boldsymbol{p} - \boldsymbol{q} \right\rVert_2 = \sqrt{(x_p - x_q)^2 + (y_p - y_q)^2 + (z_p - z_q)^2} \tag{A.2}$$

The dot product $\left\langle \boldsymbol{p}, \boldsymbol{q} \right\rangle = \boldsymbol{p}^\top \boldsymbol{q} = x_p x_q + y_p y_q + z_p z_q$ measures alignment, equalling $\left\lVert \boldsymbol{p} \right\rVert \left\lVert \boldsymbol{q} \right\rVert \cos\theta$ for the angle $\theta$ between the two vectors. The cross product $\boldsymbol{p} \times \boldsymbol{q}$ is a vector perpendicular to both, of magnitude equal to the area of the parallelogram they span.

Coordinate reference systems and the transformations between them are treated in section 3.2.

A.2 Rigid Transformations

A rigid transformation, or proper isometry, moves points as one rigid body. So every distance between them is preserved. We build one from a rotation $\mathbf{R} \in SO(3)$ and a translation $\boldsymbol{t} \in \mathbb{R}^3$, and apply it to a point:

$$\boldsymbol{q} = \mathbf{R}\,\boldsymbol{p} + \boldsymbol{t} \tag{A.3}$$

where $\mathbf{R}$ is a $3 \times 3$ orthogonal matrix satisfying $\mathbf{R}^\top \mathbf{R} = \mathbf{I}$ and $\det(\mathbf{R}) = 1$. These two constraints keep $\mathbf{R}$ from stretching or reflecting the geometry and define the group $SO(3)$.

Homogeneous coordinates carry the rotation and the translation in a single matrix:

$$\begin{pmatrix} \boldsymbol{q} \\ 1 \end{pmatrix} = \underbrace{\begin{pmatrix} \mathbf{R} & \boldsymbol{t} \\ \boldsymbol{0}^\top & 1 \end{pmatrix}}_{\mathbf{T} \in SE(3)} \begin{pmatrix} \boldsymbol{p} \\ 1 \end{pmatrix} \tag{A.4}$$

This $4 \times 4$ matrix $\mathbf{T}$ belongs to the special Euclidean group $SE(3)$, and composing two transformations becomes a matrix multiplication, $\mathbf{T}_{12} = \mathbf{T}_1 \mathbf{T}_2$. Registration (chapter 8) estimates such a transformation.

A rotation has several equivalent parameterisations, each suited to a different computation. The nine entries of $\mathbf{R}$ are convenient for applying a rotation and awkward for estimating one, because the six orthogonality constraints in $\mathbf{R}^\top\mathbf{R} = \mathbf{I}$ must be enforced during optimisation. Three Euler angles are minimal and easy to read but lose one rotational degree of freedom at specific configurations (gimbal lock). The axis-angle form carries a unit axis $\boldsymbol{u}$ and a turn angle $\theta$ in the single vector $\boldsymbol{\omega} = \theta\boldsymbol{u}$. And Rodrigues' formula turns it into a matrix:

$$\mathbf{R} = \exp([\boldsymbol{\omega}]_\times) = \mathbf{I} + \sin\theta\,[\boldsymbol{u}]_\times + (1 - \cos\theta)\,[\boldsymbol{u}]_\times^2 \tag{A.5}$$

where $[\boldsymbol{u}]_\times$ is the skew-symmetric matrix with $[\boldsymbol{u}]_\times\boldsymbol{v} = \boldsymbol{u} \times \boldsymbol{v}$ for every $\boldsymbol{v} \in \mathbb{R}^3$. Keeping only the first-order term leaves $\mathbf{R} \approx \mathbf{I} + [\boldsymbol{\omega}]_\times$, the small-angle linearisation that point-to-plane ICP applies at every iteration (section 8.3.2). The neglected terms are of order $\theta^2$, which is why that iteration needs a starting alignment close to the answer.

The unit quaternion $\boldsymbol{q}_R$, four numbers of unit norm, composes rotations with little computation, has no singular configuration, and represents every rotation twice, since $\boldsymbol{q}_R$ and $-\boldsymbol{q}_R$ give the same $\mathbf{R}$. Estimation code, including the registration of chapter 8, carries the rotation in whichever form suits the solver and applies each increment in the three unconstrained parameters of $\boldsymbol{\omega}$.

A.3 Covariance Matrix and PCA

Principal Component Analysis (PCA) summarises the local shape of a point neighbourhood. Given $k$ points $\{\boldsymbol{p}_1, \ldots, \boldsymbol{p}_k\}$ with centroid $\bar{\boldsymbol{p}} = \frac{1}{k}\sum_{i=1}^{k} \boldsymbol{p}_i$, we shift them onto that centroid. We collect their spread into the covariance matrix:

$$\mathbf{C} = \frac{1}{k} \sum_{i=1}^{k} (\boldsymbol{p}_i - \bar{\boldsymbol{p}})(\boldsymbol{p}_i - \bar{\boldsymbol{p}})^\top \in \mathbb{R}^{3\times 3} \tag{A.6}$$

Dividing by $k$ rather than by the $k-1$ of the usual sample covariance multiplies all three eigenvalues by the same factor and leaves the eigenvectors and the eigenvalue ratios of chapter 10 unchanged.

$\mathbf{C}$ is symmetric and positive semi-definite. So it has three real, non-negative eigenvalues $\lambda_1 \geq \lambda_2 \geq \lambda_3 \geq 0$ with corresponding orthonormal eigenvectors $\boldsymbol{e}_1, \boldsymbol{e}_2, \boldsymbol{e}_3$. The eigenvectors give a local frame aligned with the principal directions of variation and the eigenvalues the spread along each. Each eigenvector is fixed only up to sign, so PCA returns an unoriented direction that a separate orientation step has to resolve. When two eigenvalues are equal, the eigenvectors inside their common subspace are not determined, although the subspace is. Full isotropy requires $\lambda_1 = \lambda_2 = \lambda_3$.

The eigenvector $\boldsymbol{e}_3$ belonging to the smallest eigenvalue approximates the surface normal, because points on a locally flat surface vary least in the direction perpendicular to it. That approximation is exact for a planar neighbourhood. On a curved surface, sampling symmetric about the query point removes the covariance between the normal and the tangent components. So one eigenvector lies along the normal and the other two span the tangent plane. Whether the normal carries the smallest variance is a second condition that can fail. On the paraboloid $z = (\kappa/2)(x^2 + y^2)$ sampled uniformly over a disc of radius $a$ in projection, the tangent variances are $a^2/4$ and the normal variance $\kappa^2 a^4/48$. So $\boldsymbol{e}_3$ leaves the normal once $\kappa a > \sqrt{12}$, above about three and a half radii of curvature. A sphere sampled evenly around the query point never crosses that threshold. The fitted plane there becomes a secant, offset into the surface by about $\kappa r^2/4$ and parallel to the tangent plane. So PCA returns the exact radial direction for every support smaller than the sphere's full diameter. At the full diameter the covariance is isotropic and no normal direction is defined. The direction degrades where the sampling is asymmetric, at a coverage edge, under a density gradient, or where an occlusion truncates the neighbourhood. The angular error then grows with the support radius, the high-bias failure of the support curve of figure 7.4. Section 7.3.1 bounds that error and gives the radius that minimises it.

The ratios among the three eigenvalues encode the local shape, which chapter 10 develops into the linearity, planarity, and sphericity features of figure 10.1. Chapter 7 treats normal estimation, chapter 11 seeds plane fitting on flat neighbourhoods, and example A.1 runs the computation on three points small enough to check by hand.

A.4 Least Squares Fitting

Many algorithms fit a model by minimising a sum of squared residuals. The ordinary least squares estimate is the parameter vector that makes that sum smallest:

$$\hat{\boldsymbol{\theta}} = \operatorname*{arg\,min}_{\boldsymbol{\theta}} \sum_{i=1}^{n} \left(y_i - f(\boldsymbol{x}_i; \boldsymbol{\theta}) \right)^2 \tag{A.7}$$

For a model linear in its parameters, each model value is $f(\boldsymbol{x}_i; \boldsymbol{\theta}) = \boldsymbol{a}_i^\top \boldsymbol{\theta}$ for a row vector $\boldsymbol{a}_i^\top$ built from $\boldsymbol{x}_i$. We stack those rows into a design matrix $\mathbf{A}$ and the observations into a vector $\boldsymbol{y}$. And the normal equations give the minimum in closed form, with no search. If $\mathbf{A}$ has full column rank, $\mathbf{A}^\top\mathbf{A}$ is invertible and the minimiser is unique:

$$\hat{\boldsymbol{\theta}} = (\mathbf{A}^\top \mathbf{A})^{-1} \mathbf{A}^\top \boldsymbol{y} \tag{A.8}$$

Without full column rank, $\mathbf{A}^\top\mathbf{A}$ is singular, equation A.8 does not apply, and the minimum-norm solution is $\hat{\boldsymbol{\theta}} = \mathbf{A}^{+}\boldsymbol{y}$ with the Moore–Penrose pseudoinverse $\mathbf{A}^{+}$ that the SVD supplies directly. Geometrically, $\mathbf{A}\hat{\boldsymbol{\theta}}$ is the orthogonal projection of $\boldsymbol{y}$ onto the column space of $\mathbf{A}$, the closest the linear model comes to the observations.

Equation A.8 applies where the residual is measured along one axis, as in the moving least squares fitting of chapter 6, whose polynomial step fits a height above a reference plane. Two problems that recur in point cloud processing are not of that form. Fitting a plane to a point neighbourhood minimises the perpendicular distance from each point to the plane, orthogonal distance regression (Shakarji, 1998). Its solution is the eigenvector of the smallest eigenvalue of equation A.6, the computation of section A.3 rather than equation A.8. The distinction matters, because the form $z = ax + by + c$ cannot represent a vertical plane, and vertical planes are the façades of a terrestrial scan. Point-to-point ICP is the second, since it estimates a rotation under an orthogonality constraint that section 8.2.1 solves in closed form through the SVD of the cross-covariance matrix.

When the observations differ in reliability, each residual receives a weight $w_i$ and the solution minimises $\sum_i w_i \left(y_i - \boldsymbol{a}_i^\top \boldsymbol{\theta} \right)^2$. This gives the weighted least squares estimate $\hat{\boldsymbol{\theta}} = (\mathbf{A}^\top \mathbf{W} \mathbf{A})^{-1} \mathbf{A}^\top \mathbf{W} \boldsymbol{y}$ with $\mathbf{W} = \mathrm{diag}(w_1, \ldots, w_n)$, valid when $\mathbf{A}^\top \mathbf{W} \mathbf{A}$ is nonsingular. Weights come from one of two places. Where the measurement variance is known, $w_i = 1/\sigma_i^2$ makes the weighted estimate the maximum-likelihood one for independent Gaussian errors. Where the concern is contamination rather than known precision, the weights are recomputed from the current residuals at each pass of an iteratively reweighted solution. This is used in the robust plane fitting of section 7.5 and the robust correspondence kernels of section 8.6.2.

Every estimate above rests on measurements that carry errors, and one first-order calculation propagates them into what follows. Write the derived quantity as $f(\boldsymbol{x})$ in uncertain inputs $\boldsymbol{x}$. Near their estimated values $\delta f \approx \mathbf{J}\,\delta\boldsymbol{x}$ with the Jacobian $\mathbf{J} = \partial f / \partial \boldsymbol{x}$, so the covariances satisfy $\mathbf{C}_{f} \approx \mathbf{J}\,\mathbf{C}_{\boldsymbol{x}}\,\mathbf{J}^\top$. With $\mathbf{W}$ set to the inverse covariance of the observations, the weighted estimate above has covariance $(\mathbf{A}^\top\mathbf{W}\mathbf{A})^{-1}$. The off-diagonal entries are the reason for the matrix form, because two inputs that share an observation, a trajectory or a calibration are correlated. And a difference of two such estimates then has variance $\sigma_a^2 + \sigma_b^2 - 2\sigma_{ab}$ rather than the sum. So a shared error partly cancels. State which errors cancel before reading a difference. Section 2.10 carries a range budget into a coordinate this way, chapter 14 two epoch precisions into a level of detection.

A model nonlinear in its parameters usually has no closed-form minimiser, although special structure sometimes supplies one, as the point-to-point ICP rotation above does. Where none is available the estimate is reached by iteration. Gauss–Newton linearises the residuals about the current estimate, solves the resulting linear problem for a correction, and repeats. Levenberg–Marquardt solves each correction from the damped normal matrix $\mathbf{J}^\top\mathbf{J} + \lambda\mathbf{D}^\top\mathbf{D}$, where $\mathbf{J}$ is the Jacobian of the residuals. A small $\lambda$ returns the Gauss–Newton step and a large one a short gradient step scaled by $\mathbf{D}$, so the iteration still makes progress from a poor starting estimate. The damping rises after a step that fails to reduce the residual and falls after one that succeeds. In the reference algorithms of the National Institute of Standards and Technology (NIST), $\mathbf{D}^\top\mathbf{D} = \mathbf{I} + \operatorname{diag}(\mathbf{J}^\top\mathbf{J})$ and $\lambda$ starts at $10^{-4}$. Each increase multiplies it by 10 and each decrease by 0.04 (Shakarji, 1998). Bundle adjustment (chapter 3) and pose-graph optimisation (section 8.8.1) are solved this way, and each pass of point-to-plane ICP (section 8.3.2) is one Gauss–Newton step on a re-linearised pose.

References

• Shakarji, C. M. (1998). Least-squares fitting algorithms of the NIST Algorithm Testing System. Journal of Research of the National Institute of Standards and Technology, 103(6), 633–641. doi:10.6028/jres.103.043.