The tight solve · Reference card

For the local Gaussian approximation, $P\approx H_{\rm total}^{-1}$; take marginal covariance blocks after solving the full system. Priors are already included in $H_{\rm total}$. See the derivation and implementation limits.

Key equations

Warm-up, from memory

$$ \mathbf{r}_{\Delta v} = \mathbf{R}_i^{\top}(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\,\Delta t_{ij}) - \Delta\hat{\mathbf{v}}_{ij}(\mathbf{b}) $$

The one equation

$$ \mathcal{X}^{\star} = \arg\min\; \|\mathbf{r}_0\|^2_{\Sigma_0} + \sum \|\mathbf{r}_{\mathcal{I}_{ij}}\|^2_{\Sigma_{ij}} + \sum\sum \|\mathbf{r}_{\mathcal{C}_{il}}\|^2_{\Sigma_{\mathcal{C}}} \tag{26} $$

$$ \begin{aligned} \arg\min_{\mathbf{x}_j,\,\mathbf{g}}\;\; &\sum_{l} \rho\big(r_l/\sigma\big) \;+\; \|\mathbf{r}_{\mathcal{I}}\|^{2}_{\Sigma_{\text{eff}}}\\ +\;&\|\mathbf{b}_j - \mathbf{b}_i\|^{2}_{\mathbf{P}_b} \;+\; \|\mathbf{g} - \mathbf{g}_{\text{init}}\|^{2}_{\sigma_g^{2}\mathbf{I}} \end{aligned} $$

Say it at a whiteboard

"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."