Source-linked AI summary
LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping
Tixiao Shan, Brendan Englot, Drew Meyers, Wei Wang, Carlo Ratti, Daniela Rus
TL;DR
LIO-SAM addresses limitations in lidar-inertial SLAM involving loop closure, absolute measurements, dense-map efficiency, and large-scale drift. It combines tightly coupled factor-graph estimation with IMU-assisted lidar processing and local sliding-window scan matching, achieving efficient real-time operation and processing up to 13× real-time playback.
Problem
Existing LOAM-based lidar-inertial estimation has difficulty incorporating loop closures and absolute measurements, becomes less efficient with dense global voxel maps, and can drift in large-scale tests.
Method
LIO-SAM tightly couples lidar and IMU measurements in a factor graph, using IMU motion for point-cloud de-skewing and initialization while registering selected keyframes against fixed-size local sub-keyframes.
Results
LIO-SAM uses significantly less runtime than competing methods and processes data up to 13× faster than real time without failure while maintaining similar performance.
Takeaways & Limitations
The factor-graph formulation supports multi-sensor fusion, while absolute measurements can reduce accumulated drift during long-duration operation or in feature-poor environments.
Abstract
from arXiv · showhide
We propose a framework for tightly-coupled lidar inertial odometry via smoothing and mapping, LIO-SAM, that achieves highly accurate, real-time mobile robot trajectory estimation and map-building. LIO-SAM formulates lidar-inertial odometry atop a factor graph, allowing a multitude of relative and absolute measurements, including loop closures, to be incorporated from different sources as factors into the system. The estimated motion from inertial measurement unit (IMU) pre-integration de-skews point clouds and produces an initial guess for lidar odometry optimization. The obtained lidar odometry solution is used to estimate the bias of the IMU. To ensure high performance in real-time, we marginalize old lidar scans for pose optimization, rather than matching lidar scans to a global map. Scan-matching at a local scale instead of a global scale significantly improves the real-time performance of the system, as does the selective introduction of keyframes, and an efficient sliding window approach that registers a new keyframe to a fixed-size set of prior ``sub-keyframes.'' The proposed method is extensively evaluated on datasets gathered from three platforms over various scales and environments.
I. INTRODUCTION
LIO-SAM addresses limitations of lidar-based odometry by tightly coupling lidar and inertial measurements in a factor graph, while using local sliding-window scan matching for real-time operation.
- LOAM can struggle with loop closures, absolute measurements, dense global maps, and drift in large-scale tests.
- LIO-SAM jointly optimizes lidar, IMU, and optional absolute or place-recognition measurements within a global factor graph.The framework introduces factors for IMU preintegration, lidar odometry, GPS, and loop closure.
- IMU-derived motion de-skews point clouds, initializes lidar odometry optimization, and supports estimation of IMU bias.
- Selective keyframes and local sliding-window scan matching register new keyframes against a fixed-size set of prior sub-keyframes for real-time performance.
- The framework is validated across various scales, vehicles, and environments.
III. LIDAR INERTIAL ODOMETRY VIA SMOOTHING AND MAPPING
LIO-SAM formulates robot state estimation as factor-graph MAP inference and incrementally optimizes states using multiple sensor-derived factors.
- A. System Overview: The robot state includes rotation, position, velocity, and IMU bias, with the body-to-world transformation represented by T = [R | p].
- A. System Overview: LIO-SAM models lidar, IMU, and optional GPS observations as a MAP estimation problem represented by a factor graph.Under Gaussian noise, inference becomes nonlinear least-squares optimization.
- A. System Overview: The graph contains IMU preintegration, lidar odometry, GPS, and loop closure factors, with new state nodes added when pose changes exceed a threshold.
- A. System Overview: The factor graph is incrementally optimized with smoothing and mapping using the Bayes tree implementation iSAM2.
B. IMU Preintegration Factor
IMU preintegration converts raw inertial measurements into relative motion constraints that support lidar point-cloud correction and joint factor-graph optimization.
- B. IMU Preintegration Factor: Raw IMU angular-velocity and acceleration measurements are affected by slowly varying bias and white noise.
- B. IMU Preintegration Factor: The IMU measurements are integrated to compute the robot’s velocity, position, and rotation at a later time.
- B. IMU Preintegration Factor: The integration assumes angular velocity and acceleration remain constant during the integration interval.
- B. IMU Preintegration Factor: Preintegration produces relative body-motion measurements Δv_ij, Δp_ij, and ΔR_ij between two timesteps.
- B. IMU Preintegration Factor: These preintegrated measurements form factor-graph constraints, while IMU bias is jointly optimized with lidar odometry factors.
C. Lidar Odometry Factor
LIO-SAM extracts edge and planar features, selects keyframes, and builds a fixed-size voxel map from recent sub-keyframes for lidar odometry.
- Each incoming lidar scan is reduced to edge and planar features using local-region roughness.Large roughness values identify edge features, while small values identify planar features.
- A lidar frame contains the extracted edge and planar features, represented in the body frame.
- Keyframes are selected when the robot pose change exceeds a user-defined threshold, while intervening lidar frames are discarded.The selected keyframe is associated with a new robot state node in the factor graph.
- 1) Sub-keyframes for voxel map: The sliding window merges the n most recent keyframes into a voxel map containing separate edge and planar feature maps.The sub-keyframes are transformed into frame W before being merged.
- 1) Sub-keyframes for voxel map: The implementation uses 25 sub-keyframes and downsamples the edge and planar maps at 0.2 m and 0.4 m resolutions, respectively.
2) Scan-matching:
For scan-matching, LIO-SAM registers each new lidar frame against the local voxel map using an IMU-predicted initial transformation and feature correspondences.
- The new lidar frame is matched to the local voxel map using a scan-matching method selected for computational efficiency and robustness.The paper uses the method proposed in LOAM for this step.
- IMU-predicted robot motion provides the initial transformation that moves the new frame from B to W.
- The transformed edge and planar features are associated with corresponding edge or planar structures in the voxel maps.The detailed correspondence procedures are omitted in the supplied passage.
3) Relative transformation:
LIO-SAM estimates relative motion by minimizing feature-to-correspondence distances, then uses the optimized transformation as a lidar odometry factor between successive poses.
- Feature-to-correspondence distances are computed for edge lines and planar patches in the voxel maps.The correspondences use indexed edge and planar feature points.
- Gauss-Newton optimization minimizes the correspondence-based objective to solve for the optimal transformation.
- The resulting relative transformation ∆T_i,i+1 links consecutive robot poses as a lidar odometry factor.
- An alternative computes the relative transformation directly by representing the voxel map in the frame of the earlier pose, but the paper chooses the more computationally efficient approach.
D. GPS Factor
GPS measurements are incorporated as absolute factors when they provide a tighter position uncertainty than the current lidar-inertial estimate, helping address long-duration drift.
- Lidar-inertial odometry can suffer drift during long-duration navigation, motivating absolute measurements such as GPS.
- GPS measurements are transformed into a local Cartesian frame and associated with new factor-graph nodes.Unsynchronized GPS and lidar timestamps are handled by linear interpolation.
- The datasets span a handheld device, the Clearpath Jackal unmanned ground vehicle, and the Duffy 21 electric boat.
- A GPS factor is added only when the estimated position covariance exceeds the received GPS position covariance.This avoids constantly adding GPS factors when reception is available.
E. Loop Closure Factor
LIO-SAM uses factor-graph loop closures to incorporate trajectory constraints and correct drift, particularly when absolute positioning is unreliable.
- Loop-closure mechanism: Loop closures are incorporated as factors in LIO-SAM’s factor graph and can use Euclidean-distance detection or point-cloud descriptors.The system searches nearby prior states, scan-matches their sub-keyframes, and adds the resulting relative transformation as a loop-closure factor.
- Practical benefit: Loop-closure factors are especially useful for correcting altitude drift when GPS is the only absolute sensor.The paper attributes this benefit to inaccurate GPS elevation measurements.
- Evaluation setting: The evaluation spans five datasets collected across handheld, ground-vehicle, and boat platforms.The datasets cover varied scales and environments, including campus, park, and Amsterdam canal settings.
A. Rotation Dataset
The Rotation test evaluates robustness to aggressive rotational motion while the sensor remains stationary. LIO-SAM produces a meaningful map, whereas LIOM fails in this test.
- Test setup: The Rotation dataset applies aggressive rotational maneuvers while the user stands still, reaching a maximum rotational speed of 133.7 °/s.The test environment contains surrounding structures for mapping evaluation.
- Mapping outcome: LIOM fails to produce meaningful results in the Rotation test.The figure compares mapping results from LOAM and LIO-SAM while documenting LIOM’s failure.
- Method comparison: The Rotation test presents mapping outputs for LOAM and LIO-SAM under aggressive rotational motion.The corresponding maps are shown for direct qualitative comparison.
D. Park Dataset
On the Park dataset, intermittent GPS and unreliable elevation measurements expose drift and loop-closing weaknesses in several methods, while LIO-SAM uses loop closures to eliminate altitude drift. The evaluation also reports real-time processing and faster-than-real-time playback across challenging environments.
- Experimental setup: The Park test uses a UGV on a forested trail with GPS restricted to widely open areas, simulating repeated GPS-denied regions.The vehicle returns to its initial position after 40 minutes across asphalt, grass, and dirt surfaces.
- Trajectory results: LOAM, LIOM, and LIO-odom suffer significant drift without absolute correction data, while LIOM runs at 0.67× real-time.LIO-GPS and LIO-SAM coincide horizontally, but their altitude behavior differs because reliable absolute elevation measurements are unavailable.
- Trajectory results: LIO-GPS cannot close the loop on return because of altitude drift, whereas LIO-SAM eliminates this drift using loop closure factors.The paper notes that GPS elevation errors approached 100m in tests without loop closures.
- Quantitative evaluation: LIO-GPS and LIO-SAM achieve similar RMSE translation error relative to GPS ground truth, although full GPS access could reduce both errors by at least an order of magnitude.The reported RMSE excludes error along the z axis and reflects the restricted-GPS evaluation setting.
- Runtime: Across five datasets, LIO-SAM uses significantly less runtime than the competing methods and can process data up to 13× faster than real-time without failure.Runtime is more influenced by feature-map density than by the number of factor-graph nodes and factors.
V. CONCLUSIONS AND DISCUSSION
LIO-SAM combines tightly coupled lidar-inertial estimation with factor-graph optimization for real-time mapping in complex environments. Its factorized sensor fusion and local sliding-window scan matching support absolute corrections while maintaining computational efficiency.
- Conclusions and discussion: LIO-SAM formulates lidar-inertial odometry as a factor graph, allowing GPS, compass, altimeter, and place-recognition measurements to be incorporated.Absolute measurements can eliminate accumulated drift during long-duration operation or in feature-poor environments.
- Conclusions and discussion: A sliding window marginalizes old lidar frames, while selective keyframes and fixed-size sub-keyframes enable local scan matching in real time.The framework registers new keyframes only to a fixed-size set of sub-keyframes when lidar odometry and loop-closure factors are generated.
- Conclusions and discussion: The proposed framework performs real-time state estimation and mapping in complex environments and is evaluated across datasets from three platforms.The evaluation spans varied scales and environments.