SE(3)-LIO: Smooth IMU Propagation With Jointly Distributed Poses on SE(3) Manifold for Accurate and Robust LiDAR-Inertial Odometry
Authors: Gunhee Shin, Seungjae Lee, Jei Kong, Youngwoo Seo, Hyun Myung
Organizations: School of Electrical Engineering, KAIST (Korea Advanced Institute of Science and Technology), Daejeon, 34141, South Korea · Hanwha Aerospace, 6, Pangyo-ro 319 beon-gil, Bundang-gu, Seongnam-si, Gyeonggi-do, South Korea.
In estimating odometry accurately, an inertial measurement unit (IMU) is widely used owing to its high-rate measurements, which can be utilized to obtain motion information through IMU propagation. In this paper, we address the limitations of existing IMU propagation methods in terms of motion prediction and motion compensation. In motion prediction, the existing methods typically represent a 6-DoF pose by separating rotation and translation and propagate them on their respective manifold, so that the rotational variation is not effectively incorporated into translation propagation. During motion compensation, the relative transformation between predicted poses is used to compensate motion-induced distortion in other measurements, while inherent errors in the predicted poses introduce uncertainty in the relative transformation. To tackle these challenges, we represent and propagate the pose on SE(3) manifold, where propagated translation properly accounts for rotational variation. Furthermore, we precisely characterize the relative transformation uncertainty by considering the correlation between predicted poses, and incorporate this uncertainty into the measurement noise during motion compensation. To this end, we propose a LiDAR-inertial odometry (LIO), referred to as SE(3)-LIO, that integrates the proposed IMU propagation and uncertainty-aware motion compensation (UAMC). We validate the effectiveness of SE(3)-LIO on diverse datasets. Our source code and additional material are available at: https://se3-lio.github.io/.
Figures & tables
Figure 1: Overview of the proposed IMU propagation and LiDAR–inertial odometry (LIO), referred to as SE(3) -LIO. The IMU propagation is performed on SE(3) manifold to yield a smooth trajectory and accurate prior. The joint distribution of predicted poses is derived to characterize the relative transformation uncertainty by considering their correlation, which is incorporated into the measurement noise during motion compensation to construct a probabilistic undistorted point cloud. Finally, SE(3) -LIO optimizes both the prior and the measurement distribution, improving the performance of odometry estimation.
Figure 2: Illustration of the difference between the conventional [ 6 ] and the proposed IMU propagation method, which are denoted as SO(3)×R3 and SE(3) , respectively. The twist, ζ(t)≐[v(t)ω(t)] , is defined as a variable to simulate aggressive motion [ 4 ] and generate the ground-truth trajectory with a sufficiently small time step, Δtgt=10−4 (black dashed line). Both methods are performed with 10 IMU inputs, each computed from adjacent twist values, and an interval of Δt=10−2 . The outputs of the proposed method (green dots) yield a smoother trajectory and follow the ground truth (black dashed line) more closely compared with the outputs of the conventional method (red crosses). The highlighted box shows that the proposed method properly addresses rotational changes while propagating translation, whereas the conventional method does not.
Figure 3: Uncertainty characterization of predicted poses from IMU propagation and relative transformations for motion compensation. (a) IMU propagation is performed using 100 IMU inputs computed same as Fig. 2 , with a process noise covariance Q . Initial pose is set to origin with covariance Σ0 . (b) Relative transformation uncertainty is computed with respect to the last predicted pose Tk , either by neglecting cross terms and assuming independence [ 9 ] (red ellipses) or by considering the correlation between poses as the proposed method (purple ellipses). The uncertainties are illustrated as 95% confidence regions and validated using Monte Carlo simulation with 100 samples (black dots). The proposed method provides a more accurate characterization, while the independence assumption leads to overestimation. Results are shown every 20 IMU inputs for clarity.
Figure 4: Resultant noise of the undistorted point cloud after applying uncertainty-aware motion compensation (UAMC). (a) Points are acquired clockwise over time. (b) The noise of raw measurements increases with distance from the center. (c) The point cloud is undistorted through UAMC. As highlighted, points acquired at the beginning exhibit larger noise than those acquired later because earlier points are transformed using relative transformations with higher uncertainty.
Figure 5: Pipeline of SE(3) -LIO that integrates the proposed IMU propagation and uncertainty-aware motion compensation (UAMC). Assume the pose at tk−1 has been estimated, and a new LiDAR point cloud is acquired at tk . (a) IMU propagation is performed on SE(3) manifold using IMU measurements acquired over [tk−1,tk] , providing a prior distribution for the state update. (b) Relative transformations, along with their uncertainties, are computed from the reference pose at tk to poses at each point acquisition time. (c) These relative transformations are used to compensate for motion distortion in the raw point cloud, while their uncertainties are incorporated into the noise of each point, resulting in a probabilistic undistorted point cloud. (d) The undistorted point cloud is transformed into the world frame and used to construct residuals with the local map, for which we adopt VoxelMap [ 28 ] to obtain a measurement distribution. (e) Finally, the prior and measurement distributions are optimized through the update step, yielding an updated pose estimate at tk .
Figure 6: Description of our dataset, including CW (blue), CCW (gray), and mid (orange) sequences. (a) A ground vehicle equipped with a 128-channel LiDAR (Ouster OS2-128), an IMU (Xsens MTi-300), and a RTK-GNSS receiver (Emlid Reach RS). (b)–(c) Data were collected in large-scale rough terrain, including dense forest and wide open areas.
Dataset
NTU-VIRAL [ 18 ]
Newer-College [ 22 ]
Our dataset
Sequence
eee
nya
rtp
sbs
tnp
spms
quad-D
quad-M
quad-H
math-H
CW
CCW
mid
FAST-LIO2 [ 25 ]
0.250
0.256
0.276
0.27
0.244
3.12
0.356
0.064
0.084
0.084
14.07
17.08
2.50
PV-LIO
0.579
0.404
1.71
0.753
0.74
-
0.135
0.093
0.107
0.082
13.47
21.62
2.00
DLIO [ 3 ]
0.254
0.269
0.231
0.286
0.258
0.423
0.150
0.076
2.66
0.116
12.34
43.04
3.77
LIO-EKF [ 24 ]
0.772
0.479
1.04
0.308
0.365
2.45
2.48
0.314
2.95
1.21
-
40.35
-
MA-LIO [ 9 ]
0.252
0.256
2.32
0.268
0.248
0.464
0.156
0.060
0.307
0.102
76.82
27.77
3.09
Table 1: The root mean square error (RMSE) of absolute trajectory error (ATE) (m) on the NTU, Newer-College, and our datasets. Bold value denotes the best performance, underline value denotes the second-best performance and ‘-’ denotes that the odometry has diverged.
Figure 7: Qualitative comparison in spms sequence of the NTU-VIRAL [ 18 ] dataset (upper) and CW sequence of our dataset (lower). Baselines including (A) Fast-LIO2 [ 25 ] , (B) DLIO [ 3 ] , and (C) MA-LIO [ 9 ] are compared with (D) SE(3) -LIO (ours). Point clouds are colored by time, where brown and gray indicate the beginning and the end of the sequence, respectively. Highlighted boxes indicate the top-down view of the corresponding areas, showing the drift in estimated trajectory. Resultant point clouds are captured by identical viewpoints for fair comparison.
Sequences
eee
nya
rtp
sbs
tnp
spms
SO(3)×R3 (baseline)
0.548
0.469
2.64
0.750
0.693
0.638
SE(3) w/o UAMC
0.324
0.259
0.268
0.258
0.216
0.241
SE(3) w/ UAMC
0.321
0.246
0.269
0.253
0.204
0.238
Table 2: Ablation study on NTU-VIRAL [ 18 ] dataset. RMSE of ATE (m) is reported. Bold denotes the best performance.
Sequences
eee
nya
sbs
rtp
tnp
spms
avg
FAST-LIO2 [ 25 ]
16.02
14.15
18.44
14.24
14.80
17.74
15.90
PV-LIO [ 22 ]
51.84
38.92
46.49
49.79
36.48
44.87
44.73
DLIO [ 3 ]
99.08
98.60
100.21
98.89
99.78
100.44
99.50
MA-LIO [ 9 ]
23.97
23.77
28.52
23.33
24.22
26.38
25.03
SE(3) -LIO (ours)
26.66
13.95
25.03
19.15
11.32
26.66
20.46
Table 3: Comparison of average computation time (ms) of baselines and ours in NTU-VIRAL [ 18 ] dataset. Bold denotes the lowest computation time, and underline denotes the second-lowest computation time.
Figure 8: Time usage for each module of the proposed LIO framework in NTU-VIRAL [ 18 ] dataset.
LiDAR-inertial odometry (LIO) is a key component of autonomous navigation, but high-dynamic driving exposes two coupled challenges: intra-scan motion distortion and vibration-contaminated inertial measurements. Most real-time LiDAR-inertial pipelines propagate the system state by integrating raw IMU measurements and then use the propagated trajectory for point cloud de-distortion, thereby propagating inertial noise into both the corrected scan and the subsequent scan-to-map registration. This paper presents WNOJ-LIO, a LiDAR-IMU fusion framework based on a White-Noise-on-Jerk (WNOJ) Extended Kalman Filter (EKF). WNOJ-LIO employs a decoupled WNOJ prior on R3×\SO(3) for state prediction and treats the IMU as a high-frequency measurement source rather than the driver of state propagation. The resulting posterior state history is then used for LiDAR scan de-distortion and subsequent point-to-plane LiDAR updates. The decoupled process model enables closed-form covariance propagation, thereby bridging the gap between batch WNOJ Gaussian process (GP) trajectory priors and recursive filtering. Simulation results demonstrate improvements in acceleration and angular-velocity denoising, scan de-distortion, and localization accuracy over a FAST-LIO-style baseline. Real-world experiments were conducted using an autonomous racing car on four driving segments with maximum speeds ranging from 53 to 208~km/h, covering a wide range of vehicle vibration levels. The experiments further validate the proposed method and provide a comprehensive evaluation of its performance in estimating acceleration, angular velocity, body-frame linear velocity, attitude, and position under highly dynamic driving. The source code of WNOJ-LIO is publicly available at https://github.com/LvJohny/wnoj-ekf-lio.git.
Junning Lyu, Qizhi Guo, Xia Ning +2
School of Aerospace Engineering, Beijing Institute of Technology, Beijing 100081, China
LiDAR Inertial Odometry (LIO) is a critical component for many mobile robots that need to navigate without relying on external positioning (e.g., GPS). Platforms that operate autonomously in different environments and with heterogeneous LiDAR sensors require a LIO approach that can adapt to these different scenarios without human intervention. Existing LIO approaches can typically provide reliable and accurate odometry in scenarios with similar environments and sensors when suitably tuned. However, many approaches struggle to retain robust odometry across heterogeneous environments and sensors while using a consistent configuration. This paper presents EllipseLIO, a real-time LIO approach that generalises between scenarios by using methods for LiDAR scan filtering and registration that adapt to the sensor capabilities and environment without requiring scenario-specific tuning. Experiments with EllipseLIO and state-of-the-art LIO approaches on five datasets with diverse and challenging scenarios demonstrate that EllipseLIO is the best-performing approach overall. It achieves a 38% lower odometry error on average than the second-best approach and is the only approach that does not diverge in any experiment. An open-source version of EllipseLIO will be available at github.com/v4rl-ucy/ellipselio.
Rowan Border, Margarita Chli
Vision for Robotics Lab (V4RL), University of Cyprus, Cyprus · ETH Zurich, Switzerland
Multiple LiDAR-inertial odometry methods have been widely applied in robotic applications owing to their enhanced accuracy and reliability. However, adopting multiple LiDAR systems can be challenging due to the discrepancies in measurement noise across different LiDARs. Existing methods have typically overlooked the noise discrepancies, which can significantly affect accuracy. In this paper, we propose Noise-aware Multiple LiDAR-Inertial Odometry (NM-LIO) that addresses the noise discrepancies. We integrate a noise model to quantify the measurement noise of each LiDAR. Additionally, we estimate the uncertainty of the residuals based on the measurement noise, allowing the measurement model to capture the noise discrepancies. Our proposed method is evaluated on a public multiple LiDAR dataset and compared with state-of-the-art methods. The experimental results demonstrate that the proposed method can accurately estimate the odometry in various environments by accounting for the noise discrepancies.
Gunhee Shin, Seungjae Lee, Minho Oh +4
School of Electrical Engineering, KAIST (Korea Advanced Institute of Science and Technology), Daejeon 34141, Republic of Korea · Hanwha Aerospace, 6, Pangyo-ro 319 beon-gil, Bundang-gu, Seongnam-si, Gyeonggi-do, Republic of Korea