SE(3)-LIO: Smooth IMU Propagation With Jointly Distributed Poses on SE(3) Manifold for Accurate and Robust LiDAR-Inertial Odometry

Abstract

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 (8)

• Figure 1: Overview of the proposed IMU propagation and LiDAR–inertial odometry (LIO), referred to as $\mathrm{SE}(3)$-LIO. The IMU propagation is performed on $\mathrm{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, $\mathrm{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 conventionalforster2017tro and the proposed IMU propagation method, which are denoted as $\mathrm{SO}(3) \times \mathbb{R}^3$ and $\mathrm{SE}(3)$, respectively. The twist, $\boldsymbol{\zeta}(t)\doteq[\mathbf{v}(t) \; \boldsymbol{\omega}(t)]$, is defined as a variable to simulate aggressive motionfoehn2021sr and generate the ground-truth trajectory with a sufficiently small time step, $\Delta t_{gt}=10^{-4}$ (black dashed line). Both methods are performed with 10 IMU inputs, each computed from adjacent twist values, and an interval of $\Delta 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. \ref{['fig:PSE3']}, with a process noise covariance $\mathbf{Q}$. Initial pose is set to origin with covariance $\mathbf{\Sigma}_0$. (b) Relative transformation uncertainty is computed with respect to the last predicted pose $\mathbf{T}_k$, either by neglecting cross terms and assuming independence jung2023ral (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 $\mathrm{SE}(3)$-LIO that integrates the proposed IMU propagation and uncertainty-aware motion compensation (UAMC). Assume the pose at $t_{k-1}$ has been estimated, and a new LiDAR point cloud is acquired at $t_k$. (a) IMU propagation is performed on $\mathrm{SE}(3)$ manifold using IMU measurements acquired over $[t_{k-1}, t_k]$, providing a prior distribution for the state update. (b) Relative transformations, along with their uncertainties, are computed from the reference pose at $t_k$ 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 Yuan2022ral to obtain a measurement distribution. (e) Finally, the prior and measurement distributions are optimized through the update step, yielding an updated pose estimate at $t_k$.