This capstone brings the course together: walk an interviewer through glass-lio's tight coupling and justify every term. Nothing here is new. It's lessons 1–8 put in the order the code runs them. By the end you should be able to narrate one scan from raw points to an updated map, and say for each weight what glass-lio measured when it was set wrong.
0 · Warm-up, from memory
Lessons 3 and 4 in one line: the IMU residual this solve stacks.
From memory: write the velocity row of the IMU residual, with the bias-corrected delta.
- State side: \(\mathbf{R}_i^{\top}(\cdots)\) with gravity, from lesson 3
- Minus the delta, corrected to the current bias, from lesson 4's eq. (44)
1 · The one equation
Forster writes visual-inertial estimation as a single least-squares problem: a prior, the IMU factors, and the sensor factors, each weighted by its own covariance:Forster et al., On-Manifold Preintegration…, §IV, eq. (26); the IMU factor with bias updates is §VI-D, eq. (45).
glass-lio's tight solve is that shape with one IMU interval and LiDAR in place of the camera. 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 doc/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
- Deskew the points with the gyro, integrated on SO(3) (lesson 2).
- Loose for the first 10 scans, to measure the velocity that a static init can't observe (lesson 1).
- Preintegrate the IMU over (last scan's end, this scan's end]: the deltas and their covariance (lessons 3, 5).
- Grow the bias covariance by its random walk over that interval.
- Predict the new state with
predictState, putting gravity back in the world (lesson 2). - 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.
- Accept or coast: too few correspondences, a non-finite solve, or RMSE above the configured 0.5 m, commits the prediction and keeps the scan out of the map.
- Carry the posteriors: the bias and navigation blocks of \(H_{\rm total}^{-1}\), the latter becoming the next \(\mathbf{P}_i\) (lesson 8).
- Resync deskew's gyro bias, but only if the LiDAR alone constrains rotation (lesson 7's eigen-ratio, on the rotation block).
- 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:
- Deskew the scan with the gyro
- Preintegrate the IMU since the last scan
- Predict the new state with the deltas
- Stack LiDAR, IMU, bias and gravity rows
- Solve the 18×18 system and retract
- Accept, or coast on the prediction
- Carry the posterior covariances forward
- Insert the scan into the map
Steps 4–5 repeat until the increment is small, re-associating points each time, the ICP loop from lesson 6 over a bigger state. Coasting (step 6) skips steps 7 and 8, and the deskew resync sits between them.
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
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
doc/5-registration.md §3.11–3.12; the case study is in
doc/testing.md §12.
| Block | Weight | Set wrong, glass-lio measured… |
|---|---|---|
| LiDAR | \(1/\sigma^2\), \(\sigma = 0.02\) m (a Livox's real noise) | at 0.05, the IMU over-integrated velocity to ~40 m/s; drift fell from 1,339 m to 429 m once calibrated |
| IMU | \(\Sigma_{\text{eff}}^{-1}\), \(\Sigma_{\text{pre}} + \mathbf{J}_i\mathbf{P}_i\mathbf{J}_i^{\top}\) | with \(\mathbf{x}_i\) treated as exact, the IMU's information overruled the LiDAR and pinned the pose ("the FREEZE") |
| Bias prior | \(P_b^{-1}\), the inverse carried bias covariance | a random-walk constant alone can't say how wrong the bias was to begin with, so the accel bias would stay frozen |
| Gravity prior | \(\mathbf{I}/\sigma_g^2\), \(\sigma_g = 0.05\), anchored to \(\mathbf{g}_{\text{init}}\) | anchored to its own estimate, \(|\mathbf{g}|\) wandered to ~25 in 200 scans and threw the pose ~500 km |
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.
Say it at a whiteboard: the whole scan
"Each scan, glass-lio deskews the points with the gyro, then preintegrates the IMU from the previous scan's end to this one's: \(\Delta\mathbf{R}\), \(\Delta\mathbf{v}\), \(\Delta\mathbf{p}\) and their covariance, after growing the bias uncertainty by its random walk. It predicts the new state from those deltas, putting gravity back in the world frame. Then it iterates one 18-DoF Gauss-Newton solve over rotation, position, velocity, both biases and gravity, with four blocks in the same \(\mathbf{H}\): every LiDAR point's point-to-plane residual, whitened by its 2 cm noise and Huber-weighted; the 9-row IMU residual with bias-corrected deltas, weighted by the preintegration covariance inflated by the previous state's uncertainty; a prior from the carried bias covariance; and a gravity prior anchored to its initial value. Rotation is retracted on the right. If the solve has too few correspondences, a non-finite update, or an excessive residual, it coasts on the prediction and keeps the scan out of the map. Otherwise it carries the posterior covariances forward, resyncs deskew's gyro bias if the LiDAR constrains rotation, and inserts the scan into the map."
4 · Practice
From memory, no scrolling. The last question comes from lesson 1, where this started.
Why does glass-lio run its first 10 scans loose before switching to the tight solve?
A static window can't tell rest from constant velocity, and the test bag starts already moving at ~1.5 m/s. Seed \(\mathbf{v} = 0\) and the IMU factor fights the LiDAR. Loose ICP measures the velocity first; the tight solver starts from it.
What does \(\Sigma_{\text{eff}} = \Sigma_{\text{pre}} + \mathbf{J}_i\mathbf{P}_i\mathbf{J}_i^{\top}\) prevent?
With \(\mathbf{x}_i\) held exact (\(\mathbf{P}_i = 0\)), the IMU's information over 0.1 s is enormous and pins the pose. Inflating by \(\mathbf{x}_i\)'s uncertainty is "the closed-form marginalization of a Gaussian x_i out of this one factor": lesson 8, applied.
Why is the gravity prior anchored to the initial value, not to the previous scan's estimate?
Gravity is nearly unobservable over one 0.1 s scan. An anchor that chases the estimate has no restoring force, so every small pose error is explained by tilting \(\mathbf{g}\): it reached \(|\mathbf{g}|\approx 25\) in 200 scans and threw the pose ~500 km.
In the historical experiment described above, what happened when lidar_sigma was 0.05 instead of 0.02?
A bigger \(\sigma\) trusts the LiDAR less. In that experiment, reducing LiDAR's influence on the joint IMU constraints allowed velocity to drift: ~40 m/s on a bland stretch. A global scale only cancels in the LiDAR-only loose path, never once a second sensor shares \(\mathbf{H}\).
After an accepted tight solve, when does glass-lio skip resyncing deskew's gyro bias?
Below a rotation eigen-ratio of 0.05, the refined bias is mostly IMU dead-reckoning the LiDAR can't verify. On Corridor02, resyncing anyway corrupted every later scan's geometry and diverged.
From lesson 1. What does the accelerometer in this whole pipeline actually measure?
Specific force, \(\mathbf{R}^{\top}(\mathbf{a} - \mathbf{g})\) plus bias and noise. Every line of this lesson rests on that one equation.
From memory: name the four blocks of the tight solve, what each one's weight is, and one thing that went wrong when glass-lio set it badly. Then distinguish direct LiDAR constraints from the IMU couplings and the bias/gravity priors.
- LiDAR: \(1/\sigma^2\) per point, \(\sigma = 0.02\) m, Huber; at 0.05 velocity ran away
- IMU: \(\Sigma_{\text{eff}}^{-1} = (\Sigma_{\text{pre}} + \mathbf{J}_i\mathbf{P}_i\mathbf{J}_i^{\top})^{-1}\); without the inflation, the freeze
- Bias prior: inverse of the carried bias marginal covariance; update that covariance from the bias block of \(H_{\rm total}^{-1}\) after the solve
- Gravity prior: anchored to \(\mathbf{g}_{\text{init}}\), \(\sigma_g = 0.05\); anchored to itself, \(|\mathbf{g}|\to 25\)
- Velocity, both biases and gravity: the LiDAR's columns there are zero
That's the whole solve in five lines. Try this box again after a night's sleep; it's the best single check that the course has stuck.
Apply it: complete the practical worksheet exercise before moving on. Derive an answer and test it on the supplied inputs.