The tight solve, end to end

This capstone assembles the measurement models, factors, covariances, and geometry from lessons 1–8 into one LiDAR–inertial update. Identify what each term constrains and how it is weighted, then follow one implementation from raw scan to updated map.

0 · Warm-up, from memory

Lessons 3 and 4 in one line: the IMU residual this solve stacks.

1 · The one equation

A tightly coupled estimator combines motion and geometric evidence in one objective. A prior anchors what the current measurements cannot determine; IMU and LiDAR factors contribute residuals scaled by their uncertainties. A robust loss limits the influence of incorrect point-to-plane correspondences:

The exact state, prior, and number of factors depend on the estimator. This pattern is the LiDAR counterpart of the visual–inertial MAP objective in Forster et al. On-Manifold Preintegration…, §IV, eq. (26); the IMU factor with bias updates is §VI-D, eq. (45).

Case study: a current-state solve

glass-lio instantiates this objective with one IMU interval and point-to-plane LiDAR residuals. It solves for the new state and gravity, an 18-dimensional increment \([\delta\boldsymbol{\phi},\ \delta\mathbf{p},\ \delta\mathbf{v},\ \delta\mathbf{b}^{g},\ \delta\mathbf{b}^{a},\ \delta\mathbf{g}]\):nav_state.hpp, lines 46–79: the state layout, and boxplus, which composes rotation on the right. Gravity is appended in tight_registration.hpp.

Four blocks: every LiDAR point (lesson 6, Huber \(\rho\)), the IMU factor (lessons 3–5), a bias prior, and a gravity prior. All four fold into the same \(\mathbf{H}\), "That sum IS the sensor fusion."tight_registration.cpp, lines 59–170, and glass-lio's docs/implementation/5-registration.md §3.11, "One tight solve, four residual blocks".

eq.addScalar(r * inv_sigma, Jl * inv_sigma, huber_whitened);
eq.addBlock<9>(imuResidual(xi, x, pre, g), Jimu, imu_information);
eq.addBlock<6>(biasResidual(xi, x), Jb, bias_information);
eq.addBlock<3>(g - gravity_prior, Jg, gravity_information);

2 · One scan, in order

Across implementations, the reusable sequence is: choose a common time and frame for the measurements; predict motion from the IMU; associate LiDAR points with map geometry; combine residuals and priors; solve and check the result; then update the map only when the pose is trusted. Each decision needs its own uncertainty model.

Case study: glass-lio's scan update

The following steps show one implementation. Its warm-up length, acceptance threshold, covariance approximation, and gyro-bias feedback gate are design choices, not requirements of tight coupling.

1. Deskew the points with the gyro, integrated on SO(3) (lesson 2).
2. Loose for the first few scans, to measure the velocity that a static init can't observe (lesson 1).
3. Preintegrate the IMU over (last scan's end, this scan's end]: the deltas and their covariance (lessons 3, 5).
4. Grow the bias covariance by its random walk over that interval.
5. Predict the new state with predictState, putting gravity back in the world (lesson 2).
6. Iterate the solve: re-associate every point to a plane (lesson 7), stack the four blocks with bias-corrected deltas (lesson 4), solve 18×18, retract.
7. Accept or coast: too few correspondences, a non-finite solve, or RMSE above a configured limit, commits the prediction and keeps the scan out of the map.
8. Carry the posteriors: the bias and navigation blocks of \(H_{\rm total}^{-1}\), the latter becoming the next \(\mathbf{P}_i\) (lesson 8).
9. Resync deskew's gyro bias, but only if the LiDAR alone constrains rotation (lesson 7's eigen-ratio, on the rotation block).
10. Insert the deskewed scan into the map, only if the pose was trusted.

This flow spans three functions.processScan() (deskew, downsample, register, insert), registerScan() (the warm-up switch), and registerScanTight(), lines 284–407. Now close your eyes on the list and rebuild it:

The loose path's translation eigen-ratio is not a rejection gate in this tight loop. The rotation-block ratio controls only deskew bias feedback. Carrying a posterior means extracting blocks of the full inverse information, as derived in lesson 8; the total information already includes the priors.

3 · Justify every weight

For any estimator, a residual scale states how much error that measurement model expects. Compare weights only after accounting for units, correlations, and robust losses. Calibrate them from the sensor and the full residual, then test their effect on held-out motion and geometry.

Case study: measured weight failures

These historical experiments motivated the weights below. The outcomes depend on the dataset and estimator revision; changing a noise setting does not universally produce the same failure:glass-lio docs/implementation/5-registration.md §3.11–3.12; the case study is in docs/implementation/testing.md §12.

One structural fact ties them together: a LiDAR row has zeros in the velocity, bias and gravity columns (lesson 6). The IMU couples velocity, biases, and gravity to the pose; the bias and gravity priors also constrain their own variables. LiDAR influences velocity indirectly through the joint IMU constraints, including their orientation and bias couplings. Its weight affects the estimate, but does not alone determine velocity error.

x = boxplus(x, dx.head<kNavDim>());
g += dx.segment<3>(kIdxGrav);

That's the retraction: \(\mathrm{Exp}\) on the right for rotation, plain addition for everything else, gravity included.tight_registration.cpp, lines 148–169: solve, retract, and stop once both the translation and rotation increments are small.

4 · Who holds what: the information budget of one scan

Section 3 justified each weight on its own. Put them side by side and you can see which block actually determines each part of the state over one scan:

Read the right-hand column as a list of vulnerabilities. Velocity has no direct measurement at all: the LiDAR row has zeros there, so velocity is only as good as the IMU's integration, checked through the position it produces. That is why an under-trusted LiDAR let velocity run away. The accelerometer bias starts weakly known and gains little per scan (lesson 4: only turns reveal it); its random walk adds only a tiny variance per scan compared with its initial uncertainty, so the prior dominates for a long time. That is the design working as intended: a weakly observable state should move slowly. Gravity is the one state where the prior is meant to win, because over one scan nothing else can tell a small gravity tilt from a small velocity or bias error. An anchor that followed its own estimate would have given it no restoring force, which is exactly the runaway failure.

5 · Debugging symptoms and state fingerprints

In the glass-lio case study, the hardest bugs were not found by staring at the trajectory. They were found by logging the state: velocity, bias, gravity, rejections, scan by scan. Each failure has a fingerprint in those numbers that points at one term of this solve.docs/implementation/testing.md §12.4, “Log the state, not the pose”, and the case study around it. Here is every failure this course has met, arranged as you would meet them:

Notice what the second column has in common: every fingerprint is a state quantity, and almost none is visible in the pose alone. A pose that runs away looks the same whatever the cause; the velocity, the bias and the gravity vector each break in their own way. When you build an estimator of your own, log every state variable from the first day, not only the trajectory.

6 · What this solve still leaves out

An honest capstone ends with the edges. glass-lio's tight solve is a careful, measured design, and it is also a deliberately partial one. Each gap below is a real next step, and each connects to a lesson you now have:

• No window. Only the newest state is solved; the previous one is marginalized out of a single factor (lesson 8). A fixed-lag window would let later scans correct earlier states, and it is where preintegration's savings grow largest (lesson 3).
• Partial covariance. The carried 15×15 navigation block includes position–bias correlations, but gravity cross-correlations and the complete joint history are not carried forward (lesson 8). A full-state filter would propagate the complete current-state covariance.
• A fixed gravity anchor. Gravity can move only within its prior around the initial value. That is right for odometry; a system that ran for hours might re-level gravity against a longer history.
• No loop closure. The map forgets geometry beyond a configured range, so revisiting a place corrects nothing. This is odometry, not SLAM, by choice.
• Consistency is untested. The covariance has never been checked against ground truth with a statistic like NEES (lesson 8). It is plausible; it is not yet proven honest.

7 · Practice

From memory, no scrolling. The last question comes from lesson 1, where this started.