Skip to the lesson
FixTheRobot Explore Robotics / Localization & SLAM / M5 LiDAR SLAM / M5-T8

Localization & SLAM · Theory · M5

M5-T8 — LIO-SAM

Instead of a filter that forgets, keep a graph: every sensor becomes a factor and the optimiser reconciles them all.

  • Lesson 8 of 11
  • Module 5: LiDAR SLAM
  • Theory
  • About 10 minutes to read

LOAM-style systems estimate motion from the lidar alone and treat the IMU as an afterthought. LIO-SAM (Shan et al., 2020) turns the design around. It builds a factor graph in which IMU preintegration, lidar odometry, GPS and loop closures are all constraints on keyframe poses, and it solves the graph incrementally with iSAM2. This lesson covers the problem, the factors, how the system stays real-time, what it assumes, and where it fails.

Learning objectives

After this lesson, you should be able to:

  • List the four factor types in LIO-SAM and what each constrains.
  • Explain IMU preintegration and why it makes smoothing with high-rate IMU data affordable.
  • Describe the role of keyframes, the sliding-window local map and iSAM2 in keeping the system real-time.
  • State the assumptions of LIO-SAM and diagnose typical failures: IMU bias, a poor extrinsic, degenerate scenes and bad GPS.

The problem and the intuition

Imagine navigating in fog with three unreliable helpers. One says “you moved 4.1 m forward” based on matching what they see (lidar). One feels your acceleration and turning at 200 Hz and integrates it, but the sum drifts (IMU). One occasionally shouts your position with a few metres of error (GPS). A good navigator does not trust any one of them; they hold all the statements at once, and when a new one arrives they reconsider the whole story. That is smoothing, and a factor graph is the notebook in which the statements are written down.

A filter (M3-T4) collapses the past into a mean and covariance and cannot revisit it. A graph keeps the constraints, so a late loop closure can correct the early poses, and a re-linearisation can undo an early bad estimate. The price is more computation, which LIO-SAM manages by keeping only keyframes as nodes and by using incremental solving.

LIO-SAM replaces “integrate and correct” by “write every measurement as a factor and solve for all poses together”, and uses keyframes and iSAM2 to keep that affordable.

LIO-SAM factor graphA chain of keyframe poses connected by lidar odometry factors, with IMU preintegration factors between consecutive keyframes, a GPS factor on one pose and a loop closure factor linking a late pose to an early one. x0x1x2x3x4x5 loop closure factor GPS grey line: lidar odometry factordashed green: IMU preintegration factor Only keyframes become nodes; iSAM2 updates the graph incrementally.
A LIO-SAM style factor graph: keyframe poses with odometry, IMU, GPS and loop factors.

The algorithm: factors, preintegration, keyframes, iSAM2

Problem. Estimate the trajectory and a lidar map from a spinning lidar, a 6-axis or 9-axis IMU (typically 200–500 Hz) and optionally GPS, online, with bounded drift and the ability to correct the past on loop closure.

Assumptions. The IMU–lidar extrinsic (rotation and translation) is known; timestamps are synchronised; the IMU noise and bias random walk are approximately Gaussian; the lidar sees enough structure for scan-to-map matching; GPS errors are bounded when it is used; the gravity direction is observable at initialisation.

  1. IMU preintegration. Between two keyframe times i and j, integrate the raw gyroscope and accelerometer readings, expressed in the frame of keyframe i, into three relative quantities ΔRij, Δvij and Δpij. They do not depend on the pose or velocity at i, so when the optimiser changes the state at i they are not recomputed; only a first-order bias correction is applied (Forster et al., 2017).
  2. High-rate pose from the IMU. A second small graph (the “IMU odometry”) combines the latest lidar-optimised keyframe with the IMU after it, giving a pose at IMU rate. This pose is used to deskew each lidar scan (M5-T2) and to initialise scan matching.
  3. Keyframe selection. A scan becomes a keyframe only if the robot has moved or rotated beyond a threshold (for example 1 m or 0.2 rad). This keeps the graph small.
  4. Lidar odometry factor. Extract edge and planar features (M5-T3) from the deskewed scan, register them to a sub-map built from the last n keyframes (a sliding window; M5-T4) and add the resulting relative pose between keyframes as a factor.
  5. GPS and loop factors. A GPS factor constrains a keyframe position; it is added only when the estimated position covariance exceeds the GPS covariance, so it corrects drift without wasting effort when the pose is already well determined. A loop factor is added when a candidate keyframe (found by Euclidean-distance search) passes ICP verification.
  6. Incremental smoothing. iSAM2 (Kaess et al., 2012) updates the factorisation of the Bayes tree locally, touching only the variables affected by the new factors. After optimisation, the IMU bias and velocity are re-estimated and fed back into the IMU odometry.
LIO-SAM factor graph cost
x∗=arg min⁡{xk, vk, bk}[∑∥rIMU(xi,xj,bi)∥ΣIMU2+∑∥rlidar(xi,xj)∥ΣL2+∑∥rGPS(xk)∥ΣG2+∑∥rloop(xa,xb)∥ΣC2]\mathbf x^{*} = \operatorname*{arg\,min}_{\{x_k,\,v_k,\,b_k\}}\Big[\sum\lVert r_{\text{IMU}}(x_i,x_j,b_i)\rVert^{2}_{\Sigma_{\text{IMU}}} + \sum\lVert r_{\text{lidar}}(x_i,x_j)\rVert^{2}_{\Sigma_{L}} + \sum\lVert r_{\text{GPS}}(x_k)\rVert^{2}_{\Sigma_{G}} + \sum\lVert r_{\text{loop}}(x_a,x_b)\rVert^{2}_{\Sigma_{C}}\Big]
The four terms are the preintegrated IMU factor, the scan-to-map relative pose, the GNSS position prior and the loop closure.
xk
keyframe state: pose (R, p), velocity v.
bk
IMU bias (gyroscope and accelerometer), modelled as a random walk and estimated.
ΔRij, Δvij, Δpij
preintegrated rotation, velocity and position increments between keyframes i and j.
Σ
the covariance of each factor; it decides how strongly each source is trusted.

Strengths, limitations, failure modes

StrengthLimitation / failure mode
Tight IMU coupling gives a good prior for matching and robust deskewing in fast motionA wrong IMU–lidar extrinsic or time offset biases every factor; the system looks fine and drifts
Factor graph lets loop closures and GPS correct the whole trajectoryBad loop or GPS factors corrupt the map and need robust kernels or verification
Keyframes and iSAM2 keep the cost boundedKeyframe thresholds that are too large lose accuracy in aggressive manoeuvres; too small slows the optimiser
Handles degenerate scans better than lidar-only systems when the IMU is goodIn long corridors the IMU only bridges for a few seconds; bias drift then dominates

A worked example: why preintegration is needed and what GPS does

  1. Cost without preintegration. The keyframes are 1 s apart and the IMU runs at 200 Hz, giving 200 samples per interval. If each sample were its own state, the graph would gain 200 nodes per second: 12 000 per minute. With preintegration the 200 samples are folded into one factor, so one node per keyframe: about 60 per minute at one keyframe a second.
  2. Why the preintegrated terms do not depend on the start state. ΔRij is the product of the small rotations (Exp((ω̃k − bg)Δt)) over the samples; it depends only on the measured gyroscope values and the bias. If the optimiser rotates keyframe i by a few degrees, ΔRij is unchanged. Only a change in the bias bg requires a correction, which is linear: ΔRij(bg + δ) ≈ ΔRij(bg) · Exp(Jg δ).
  3. A bias example. A gyroscope bias of 0.005 rad/s (about 0.29°/s, a mid-grade MEMS value) integrated for 60 s gives 0.3 rad ≈ 17° of heading error if uncorrected. In the graph the bias is a variable linked by a random-walk factor, and the lidar factors reveal it: a persistent yaw discrepancy between IMU and lidar over many keyframes pulls the bias estimate to 0.005 rad/s.
  4. GPS gating. Suppose the position covariance has σ = 0.3 m after matching and the GPS σ is 2.0 m. Adding GPS would add almost no information. When the lidar drifts so that σ grows to 3 m (open field, no features), the gate opens and GPS pulls the position to within its 2 m accuracy. In a feature-rich scene the GPS factor is skipped; in an empty field it saves the run.
  5. A failure to recognise. If the IMU–lidar extrinsic rotation is wrong by 5°, then after any turn the IMU and lidar disagree by an angle proportional to the turn, and the optimiser explains it with a wrong bias. The signature is a bias estimate that changes sign with the manoeuvre, not a constant offset. The lab asks you to distinguish this from true bias and from missing loop closure.

Engineering implications and limitations

Engineering implications

  • Calibrate first. The extrinsic transform and the time offset between lidar and IMU matter more than any parameter in the optimiser (M6-T8 covers the equivalent for cameras).
  • Check IMU specifications. Noise density and bias random walk set the factor covariances. Copying numbers from a different IMU gives a filter that is over- or under-confident.
  • Choose the IMU rate and axes. A 9-axis IMU supplies an absolute yaw reference through the magnetometer, but indoors magnetic distortion makes it harmful; many deployments use the 6-axis output.
  • Verify loops and use robust kernels. A single false loop distorts the whole map (M4-T4, M4-T5); LIO-SAM’s radius-search and ICP-fitness gate is minimal.
  • Monitor consistency. Log the bias estimates, the size of the lidar correction applied to the IMU prediction, the optimisation time per keyframe and the number of matched features.
  • Know the compute limit. The sub-map size and downsample resolution determine the CPU load; on small embedded boards you may need to trade accuracy for rate.

Limits

LIO-SAM is tightly coupled in the graph sense: the IMU enters the same optimisation as the lidar. The lidar odometry factor itself, however, comes from a separate scan-to-map registration, so the lidar and IMU do not share one residual per point (FAST-LIO, M5-T9, does). The system also depends on keyframes: between keyframes the pose is only as good as IMU integration. And, like every graph system, it assumes the covariances you give it are roughly honest.

Common misconceptions

“Tightly coupled means the IMU makes lidar matching unnecessary.”

The IMU drifts quickly; the lidar factors are what bound the bias and the pose error.

“Every IMU sample is a node in the graph.”

Preintegration folds the samples between keyframes into one factor; only keyframes are nodes.

“GPS makes the map globally accurate everywhere.”

GPS bounds the position in open areas; indoors or under canopy it is absent or wrong, and bad GPS factors can hurt.

“If the trajectory looks smooth, the system is healthy.”

A wrong extrinsic or bias gives a smooth but biased trajectory. Check the bias estimates and residuals.

Engineering takeaways

  1. LIO-SAM writes IMU, lidar, GPS and loop measurements as factors on keyframe poses and solves them with iSAM2.
  2. IMU preintegration folds many samples into one factor that is independent of the start state, apart from a bias correction.
  3. Keyframes, a sliding sub-map and incremental solving keep the cost bounded and real-time.
  4. GPS is gated by the current uncertainty; loops are verified by ICP. Both can still go wrong.
  5. Extrinsic, time offset, IMU noise parameters and bias are the usual causes of silent drift.

References and further reading

Primary papers and authoritative documentation.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

Question 1

Why is IMU preintegration used instead of adding every IMU sample as a graph node?

Explanation

Folding the 100s of samples between two keyframes into one factor keeps the graph small, and because the preintegrated increments are expressed relative to the first keyframe they need not be recomputed when the optimiser changes that keyframe’s pose; only a linear bias correction is applied.

Question 2

In an open field the lidar sees almost nothing. Why does LIO-SAM add the GPS factor then, and not in a city street?

Explanation

The GPS factor is gated by the estimated position covariance. In a feature-rich street the lidar already localises to well under the GPS error, so a GPS factor adds nothing; in an open field the covariance grows beyond the GPS error and the factor is added to bound drift.

Question 3

The estimated gyroscope bias flips sign each time the robot turns. What is the most likely cause?

Explanation

A bias is slowly varying and should not depend on the manoeuvre. A sign that follows the turning direction suggests that the IMU–lidar extrinsic rotation is wrong (or the time offset is), so the optimiser explains the inconsistency with a spurious bias.

What’s next

LIO-SAM couples the IMU in a graph. The next lesson shows the filter alternative: an iterated error-state Kalman filter that puts every lidar point into the update and maintains the map with an incremental tree.

Practice this lesson

Diagnose LiDAR SLAM Drift

Drift accumulates in a long run: attribute it to registration, IMU bias or missing loop closure and fix it.

Next lesson

M5-T9 — FAST-LIO and FAST-LIO2

Describe the iterated error-state Kalman filter and the ikd-tree map of FAST-LIO and FAST-LIO2.

Nothing below is required to finish this lesson.

  • Before
    M5-T7: LeGO-LOAM

    Explain ground-optimised segmentation, two-step optimisation and loop closure for ground vehicles.

  • Lesson
    M4-T8: Factor Graphs and Incremental Smoothing

    Draw a SLAM problem as a factor graph, predict the fill-in of elimination and marginalization, and explain what iSAM2 recomputes when a measurement arrives.

  • Lab
    M5-L4: Diagnose LiDAR SLAM Drift

    Drift accumulates in a long run: attribute it to registration, IMU bias or missing loop closure and fix it.