The mathematical toolkit

This course starts from vectors and basic derivatives. This toolkit bridges those ideas to rotations, Jacobians, covariance, and least squares. Allow 45–60 minutes with pencil and paper; revisit the relevant section as each lesson needs it.

You should be able to multiply a matrix by a vector, differentiate a scalar function, and use velocity and acceleration. If one of those is unfamiliar, review it before continuing.

How to use this page. Each section has two layers. First the idea, stated once and exactly. Then the mistake the idea exists to prevent, because every tool here is in the course because getting it slightly wrong produces an estimator that still runs and is still wrong. Do the “Try it” boxes before opening them. The quizzes at the end are for a second sitting, a day later.

1 · Vectors, frames, and transposes

A vector's coordinates depend on its frame. In this course \(R_{WB}\) takes body coordinates into world coordinates: \(u_W=R_{WB}u_B\). A rotation preserves lengths and angles, so \(R^\top R=I\), \(R^{-1}=R^\top\), and \(\det R=1\). The set of these 3×3 matrices is called SO(3). A pose \((R,t)\) is an element of SE(3). It acts on a point by \(q=Rp+t\).

A rotation's transpose undoes it. Matrix multiplication does not generally commute: turning about x and then z is different from turning about z and then x.

Reading a subscript

The two letters on \(R_{WB}\) are not decoration: read them right to left, “from \(B\) to \(W\)”. The payoff is that chains check themselves. Adjacent inner letters must match, and they cancel:

A LiDAR-frame vector \(u_L\) goes to the IMU frame, then to the world. Write \(R_{IL}\,R_{WI}\) instead and the inner letters (\(L\) next to \(W\)) do not match; the expression type-checks as matrix algebra but means nothing. This exact product appears in glass-lio's initialisation: gravity is measured in the IMU frame, but the pose tracks the LiDAR, so the initial attitude is \(R_{WL}=R_{WI}R_{IL}\). On the Mid-360 \(R_{IL}\) is the identity, which is precisely why forgetting it would go unnoticed until someone mounts an external IMU.The project's own write-up calls this “a bug that only appears on someone else's hardware is the worst kind”: docs/implementation/1-imu-init.md, “Applying the extrinsic”.

What orthogonality buys

\(R^\top R = I\) is one equation with three consequences you will use constantly.

• Lengths survive. \(\|Ru\|^2 = u^\top R^\top R u = \|u\|^2\). A LiDAR range is unchanged when you rotate the point into the world.
• The inverse is free. \(R^{-1} = R^\top\): no matrix inversion, no numerical error, and \(R_{BW} = R_{WB}^\top\) simply swaps the subscripts.
• The determinant is ±1, and the sign matters. An orthogonal matrix with \(\det = -1\) is a reflection. It preserves lengths too, which is why it can pass casual checks, but no physical motion produces it. Rotations are the orthogonal matrices with \(\det = +1\), and that is the whole of SO(3).

A frame change that matters: the tilted sensor

A sensor is rarely mounted perfectly level. At rest the accelerometer reads specific force pointing up (lesson 1 explains why up), and in a tilted body frame “up” is the world \(+z\) rotated back through the tilt: part of gravity lands on the body's horizontal axes. A pipeline that forgot the frame change would read that component as a forward acceleration and integrate it into large position errors within seconds. Initialisation exists to find this rotation and remove it.

Poses: composing and inverting

A pose \(T=(R,t)\) maps points by \(q=Rp+t\). Two rules follow directly from that one equation, and both are worth deriving rather than memorising:

For the inverse, solve \(q=Rp+t\) for \(p\): \(p = R^\top(q-t) = R^\top q - R^\top t\). The inverse translation is not \(-t\); it is \(-t\) expressed in the other frame.

Software usually packs \((R,t)\) into a 4×4 homogeneous matrix so composition becomes one matrix product. glass-lio does this when it moves a scan into the world before inserting it into the map (pcl::transformPointCloud(*deskewed, *scan_world, pose_.matrix())). The algebra is the same; the 4×4 form is bookkeeping.

2 · Cross products, Exp, and local coordinates

The hat operator turns a vector into a matrix that computes a cross product:

A rotation vector \(\phi=\theta a\) encodes an axis \(a\) of unit length and an angle \(\theta\) in radians. The exponential map makes it a rotation matrix. For \(\theta=\|\phi\|\ne0\), Rodrigues' formula is

At zero, use the limits. To first order, \(\operatorname{Exp}(\delta\phi)\approx I+\delta\phi^\wedge\). The approximation explains derivatives; the exact Exp keeps updates on SO(3). Log maps a rotation back to local coordinates; a unique smooth inverse requires a suitable branch away from the angle-π ambiguity.

A tangent increment is a local coordinate for a small change. Retraction applies it to the state: \(R\leftarrow R\operatorname{Exp}(\delta\phi_B)\) for a body-frame increment, or \(R\leftarrow\operatorname{Exp}(\delta\phi_W)R\) for a world-frame increment. They describe the same physical update if \(\delta\phi_W=R\delta\phi_B\). Addition \(R+\delta R\) does not generally preserve rotation constraints.

Four properties of the hat you will lean on

Every rotation Jacobian in the later lessons is two or three applications of these, and nothing else:

• Skew symmetry is why \(u^\top a^\wedge u = 0\): a cross product is always perpendicular to its inputs.
• \(a^\wedge a=0\) says rotating a vector about itself does nothing. Lesson 7 meets its consequence: a point lying on a rotation axis gives no information about rotation about that axis.
• The swap \(a^\wedge b = -b^\wedge a\) is how you move the unknown to the right of a product so it can be factored out. It is the single most used move in differentiating rotated points (§3).
• The conjugation rule says that re-expressing a cross product in a rotated frame is the cross product of the rotated vector. It is where the frame-change rule \(\delta\phi_W = R\,\delta\phi_B\) above comes from.

How good is “first order”?

Rotating \(+x\) about \(z\) by ε, the first-order result \([1,\epsilon,0]\) misses the exact \([\cos\epsilon,\sin\epsilon,0]\) by about \(\epsilon^2/2\): negligible for the small increments an estimator takes, and growing quickly for large ones.

The lesson is not “first order is bad”. It is the opposite: over the increments an estimator actually takes, first order is excellent for derivatives, and that is the only job it is given. The state itself is always updated with the exact Exp, so the small error in the linearisation is corrected at the next iteration rather than accumulated.

Composing, not adding

Why not keep three angles and add increments to them? Because rotations about different axes do not commute. Rotate a quarter-turn about \(x\) then about \(z\), and in the other order, and the object ends up in different places. Adding angle increments silently assumes that order does not matter. It is only true when every increment shares one axis, which is exactly the special case most hand examples use, so the mistake survives the examples.

Composition gets it right by construction. glass-lio's gyro integrator does exactly this for every IMU sample during a scan: \(R_k = R_{k-1}\operatorname{Exp}(\Delta\theta_k)\), with the increment on the right because a gyro measures rotation in its own, body, frame. Lesson 2 builds this step by step.

Left, right, and the adjoint

The two retractions above are both correct; they differ in which frame the increment is written in. Conjugation converts one into the other:

glass-lio uses both, deliberately. Its SE(3) Gauss-Newton solver writes corrections in the world frame and composes on the left; its 18-dimensional inertial state composes on the right, because the IMU equations are naturally body-frame. Neither is a mistake. Mixing them is: hand a right-perturbation Jacobian to a left-perturbation solver and the estimator still converges, to the wrong answer.The project records the convention per component in a table: docs/implementation/gauss-newton.md, §6 “The perturbation convention”.

Log, and the half-turn

\(\operatorname{Log}\) returns the rotation vector of a rotation. It is well defined for angles below π, and that is all an estimator ever asks of it: the rotations it takes a Log of are differences between nearby estimates, or single IMU steps, a few degrees at most. At exactly π the answer stops being unique, since a half-turn about \(a\) and about \(-a\) are the same rotation. Keep that in mind when you meet \(J_r^{-1}\) in §4: its formula divides by \(\sin\theta\) for precisely this reason.

3 · A Jacobian is a local prediction

For a vector residual \(r(x)\), the Jacobian gives \(r(x+\delta x)\approx r(x)+J\delta x\). It has one row per residual and one column per state coordinate. The prediction is exact for a linear residual; for a nonlinear one, the omitted terms are quadratic in \(\delta x\).

On rotations, define the perturbation before taking derivatives. To test column \(j\) numerically, perturb using the same retraction as the solver:

Try several small ε values: too large gives truncation error; too small gives round-off error. Lesson 6 applies this to an actual point-to-plane residual.

Units: every column has its own

A Jacobian entry has units of residual per state coordinate. For a point-to-plane residual in metres and a state of translation (m) and rotation (rad), the translation columns are dimensionless and the rotation columns are metres per radian: a lever arm. That is not pedantry. It is why a distant point pulls on rotation harder than a near one, in proportion to its range, and why the far points in a LiDAR scan are the ones that pin down heading.

The chain rule, which is most of the work

Real residuals are compositions: transform a point, then project it, then compare. The chain rule multiplies the pieces' Jacobians, innermost last:

The shapes have to chain: a 1×3 times a 3×6 gives the 1×6 row a point-to-plane residual contributes. If the shapes do not chain, the derivation is wrong before any number is checked, which makes shape-checking the cheapest test there is.

Differentiating a rotated point

Take \(q = Rp\) and perturb \(R\) on the right, \(R\operatorname{Exp}(\delta\phi)\). Using \(\operatorname{Exp}(\delta\phi)\approx I+\delta\phi^\wedge\) and then the swap from §2:

Perturb on the left instead, \(\operatorname{Exp}(\delta\phi)R\), and the same steps give \(-(Rp)^\wedge\). The two are genuinely different matrices. Under the right perturbation, a column is rotation about a body axis, so a point lying on that body axis does not move and the column is zero. Under the left perturbation, the same column is rotation about a world axis, and the same point generally swings. Same point, same “axis”, different physics. A central-difference check through the solver's own retraction settles which one you have, and it is the check you should always run.

Finite differences, done properly

The numerical Jacobian is your oracle, so it pays to know how accurate it is. A central difference has two error sources pulling in opposite directions: truncation error, which shrinks as the step ε shrinks, and round-off error, which grows as ε shrinks because you subtract two nearly equal numbers. The error therefore has a floor, and pushing ε smaller past it makes the result worse.

Two practical rules follow. Compare analytic and numerical Jacobians with a tolerance near that floor, not with exact equality. And perturb through the solver's own retraction: an analytic Jacobian derived for a right perturbation will disagree with a numerical one taken by adding to matrix entries, and the disagreement is a bug in the test, not the code.

4 · Why a right Jacobian appears

Adding to a rotation vector and composing rotations are different operations. The right Jacobian converts between them locally:

Near zero, \(J_r(\phi)\approx I-\tfrac12\phi^\wedge+\tfrac16(\phi^\wedge)^2\), and \(J_r(0)=I\). In lesson 5 the noisy gyro changes the vector inside Exp, so \(J_r\) maps that noise to a right rotation error. You need the meaning and the zero limit before memorizing the formula. The inverse map appears when differentiating Log: \(\operatorname{Log}(\operatorname{Exp}(\phi)\operatorname{Exp}(\delta))\approx\phi+J_r^{-1}(\phi)\delta\).

What it corrects for

If rotations commuted, \(\operatorname{Exp}(\phi+\delta\phi)\) would simply equal \(\operatorname{Exp}(\phi)\operatorname{Exp}(\delta\phi)\) and \(J_r\) would be the identity. They do not, and \(J_r\) is exactly the first-order bill for that: it tells you which small composed rotation has the same effect as a small added change to the rotation vector. The size of the correction grows with \(\theta\). At zero there is nothing to correct, which is the \(J_r(0)=I\) above.

A case you can check by hand

Perturb along the rotation's own axis, \(\delta\phi \parallel \phi\). Then \(\phi^\wedge\delta\phi = 0\), both correction terms vanish, and \(J_r(\phi)\,\delta\phi = \delta\phi\). That is right: two rotations about the same axis do commute, and their angles simply add. Only the components of \(\delta\phi\) perpendicular to the axis are bent by the manifold's curvature. When you implement \(J_r\), this is the first check to write, because it needs no reference values at all.

The inverse is not the sign-flipped Jacobian

The inverse has its own closed form, and its small-angle limit is a common trap:

The second-order coefficient is \(\tfrac1{12}\), not \(\tfrac16\) with a flipped sign. The denominator \(\sin\theta\) is the half-turn singularity from §2 showing up in algebra: at \(\theta=\pi\) the Log is not unique, so its derivative cannot exist.

The numerical cliff: when the correct formula is the wrong code

The closed form above is exact, and in floating point it is dangerous at small angles, which are the most common angles of all: a gyro at rest over one IMU step. The culprit is \(1-\cos\theta\), a subtraction of two nearly equal numbers. As \(\theta\) shrinks, the result loses digits, and for small enough angles \(\cos\theta\) rounds to exactly 1 and the closed form returns zero instead of one half.

No division by zero ever happens here; the formula simply goes quietly wrong long before \(\theta=0\). glass-core's implementation therefore switches to the Taylor series below a small threshold where the closed form is still accurate, so that both branches agree in the overlap.so3_jacobian.hpp, lines 41–59 explains the threshold; lines 64–81 implement \(J_r\), and lines 97–111 its inverse, with the \(\tfrac1{12}\). The general lesson outlives this formula: a branch that looks like it guards against 0/0 is usually guarding against cancellation, and its threshold should be chosen where the exact formula is still good, not as small as possible.

5 · Covariance, eigenvalues, and information

For an error \(e\) with zero mean, \(P=E[ee^\top]\). Diagonal entries are variances (squared units); off-diagonal entries encode correlations. In direction \(u\) of unit length, variance is \(u^\top Pu\). An eigenvector of a symmetric covariance gives a principal direction; its eigenvalue gives the variance in that direction.

The inverse \(P^{-1}\) is information: a direction with large variance is weakly informed, so an equal residual in that direction is penalised less.

If \(e'=Ae+n\) and \(n\) is independent of \(e\), then

Expand \((Ae+n)(Ae+n)^\top\); the cross terms vanish by independence and zero means. The transform acts on both sides of \(P\), and forgetting one side is the classic slip. With correlated noise the cross terms must be included.

Correlation: why the diagonal is not enough

Off-diagonal entries tilt the uncertainty ellipse. When two errors tend to share a sign, the largest uncertainty lies along a diagonal direction, and it can exceed every diagonal entry. If you looked only at the diagonal, you would draw an axis-aligned ellipse and miss both the direction and the size of the worst error. The eigen-decomposition finds both. Lesson 7's degeneracy test is exactly this computation, applied to a matrix of plane normals instead of a covariance.

Information adds

Two independent measurements \(z_1, z_2\) of the same quantity, with variances \(P_1, P_2\), fuse by adding information:

The fused estimate sits closer to the better measurement, and it is more certain than either alone. This is the scalar version of the whole course. glass-lio's tight solve adds LiDAR information, IMU information and prior information into one matrix, and the sum decides, direction by direction, which sensor wins.

Through a nonlinear map

Most maps are nonlinear, so an estimator linearises them and propagates with the Jacobian: \(P' \approx J P J^\top\). A heading error moves a point ahead of the sensor sideways by an amount proportional to its range, so the Jacobian is the range itself and heading uncertainty becomes lateral position uncertainty that grows with distance. That is §3's lever arm again, now seen through covariance.

From a noise density to a variance

IMU datasheets give noise as a density, in units per \(\sqrt{\mathrm{Hz}}\). Two conversions cover almost every use:

• the standard deviation of one sample at rate \(1/\Delta t\) is \(\sigma_c/\sqrt{\Delta t}\);
• integrated white noise grows as a random walk, with variance \(\sigma_c^2\,t\) after time \(t\).

A single IMU sample is noisy, yet the integrated angle or velocity is far more precise than that single-sample noise suggests, because independent errors partly cancel: the standard deviation of integrated noise grows only with \(\sqrt t\). Averaging is why integration is so much better than any single sample suggests. That holds for the noise part only; bias, which does not average out, grows linearly. Lesson 5 does this properly, with the full 9×9 covariance.

6 · From residuals to a linear solve

For Gaussian measurement errors, a weighted least-squares cost is \(\tfrac12\sum r^\top\Sigma^{-1}r\). This course writes \(\|r\|_\Sigma^2=r^\top\Sigma^{-1}r\). Replace \(r\) by \(r+J\delta x\) and set the derivative to zero:

The minus sign is already in \(b\). Other books define \(b\) as the positive gradient and write \(H\delta x=-b\); read the definition before copying a formula. Whiten a scalar residual and its Jacobian by dividing both by σ. For correlated residuals, use a matrix square root of the information.

A singular H means at least one local direction is unconstrained. Adding a small diagonal can make a linear system solvable, but it does not turn missing measurements into real information. Lesson 7 explores this distinction.

Weights are claims

When no model passes through every measurement, least squares spreads the misfit, and the weights decide how. Declare one measurement more precise and the solution moves towards it, while the others absorb more of the residual. Weights are not a tuning knob; they are the claim you make about each measurement's precision, and the answer follows the claim.

Nonlinear problems: iterate the same step

When \(r\) is nonlinear, Gauss-Newton linearises at the current estimate, solves the linear problem, updates, and repeats. Near a solution with small residuals the number of correct digits roughly doubles every step: quadratic convergence. Far away there is no such promise, which is why glass-lio always hands its solver a good starting guess from the IMU and refuses a result instead of thrashing when the guess was not good enough.

Robust weights: when one residual lies

A squared cost lets one oversized residual pull quadratically hard. The Huber weight keeps small residuals quadratic and makes large ones linear, so an outlier's influence saturates instead of growing. In glass-lio every LiDAR residual is Huber-weighted: a moving car or a mis-associated wall can shift the fit a little but cannot take it over. The IMU factor is not Huber-weighted, because a preintegrated IMU delta is not an outlier candidate the way a single laser return is.gauss_newton.hpp, lines 31–38: the Huber weight.

A singular H, and what damping does not fix

Fit a line to a single point and \(H\) is singular: raise the intercept and lower the slope together and the line still passes through the point. The data genuinely cannot choose between those lines, and that direction is \(H\)'s null space.

Adding \(\lambda I\) to \(H\) (Levenberg–Marquardt damping) makes the system solvable, and it returns one particular line. It chose that line because of λ, not because of any measurement. In a long corridor the same thing happens to translation along the corridor, and the correct remedy is not a bigger λ but another sensor whose information fills the null space. Lesson 7 builds the test that detects the situation; lesson 9 shows the IMU filling it.

7 · Practice

Answer from memory, ideally a day after reading. Having to work to recall it is what makes it stick.