💻 computer science

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

This paper presents SE(3)-LIO, a LiDAR-inertial odometry system that improves accuracy and robustness by jointly propagating poses on the SE(3) manifold to better integrate rotational and translational motion, while incorporating correlated pose uncertainties into motion compensation to mitigate distortion errors.

Original authors: Gunhee Shin, Seungjae Lee, Jei Kong, Youngwoo Seo, Hyun Myung

Published 2026-03-18

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Gunhee Shin, Seungjae Lee, Jei Kong, Youngwoo Seo, Hyun Myung

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are trying to navigate a dark, foggy forest using only two tools: a high-speed stopwatch (your IMU sensor) and a slow, blurry camera (your LiDAR sensor).

The stopwatch ticks 200 times a second, telling you exactly how much you turned or accelerated in that split second. But it doesn't know where you are relative to the trees; it just knows how you moved.
The camera takes a picture of the trees once every second. It tells you exactly where you are, but because you are moving fast, the picture is often a blurry mess (motion distortion).

The goal of this paper is to build a navigation system that combines these two tools perfectly to keep a robot from getting lost. The authors call their new system SE(3)-LIO.

Here is how they solved the two biggest problems with previous systems, using some simple analogies.

Problem 1: The "Separate Tracks" Mistake (Motion Prediction)

The Old Way:
Imagine you are driving a car. To predict where you will be in one second, old systems treated your turning (rotation) and your driving forward (translation) as two completely separate things.

They calculated: "I turned 10 degrees."
Then they calculated: "I drove 5 meters."
The Flaw: In the real world, if you turn your car while driving, your path curves. If you calculate the turn and the drive separately, you end up predicting a path that looks like a square corner instead of a smooth curve. This error gets worse the faster you move.

The New Way (SE(3)-LIO):
The authors say, "Stop treating turning and driving as separate tracks!"
Instead, they put both on a single, smooth highway called the SE(3) Manifold.

The Analogy: Think of the old method as trying to draw a circle by drawing a square first and then rounding the corners. It's clunky. The new method draws the circle in one smooth, continuous motion.
The Result: When the robot spins and moves at the same time (like a drone doing a barrel roll), this new method predicts the path much more accurately because it understands that turning changes how you move forward.

Problem 2: The "Blurry Photo" Problem (Motion Compensation)

The Old Way:
When the LiDAR camera takes a picture, it scans the world point by point. If the robot is moving, the first point it scans is from a different location than the last point. This makes the picture look like a "jelly" or a smear.
To fix this, the system tries to "un-smear" the photo by mathematically shifting every point back to where it should have been.

The Flaw: The system uses its best guess (from the stopwatch) to do this shifting. But the stopwatch isn't perfect; it has tiny errors.
The Old Mistake: Previous systems assumed these errors were random and unrelated. They thought, "The error at point A has nothing to do with the error at point B." This led them to over-correct or under-correct, making the "un-smearing" process shaky, especially for points far away.

The New Way (Uncertainty-Aware Motion Compensation):
The authors realized that the errors in the stopwatch are connected.

The Analogy: Imagine you are walking down a hallway while holding a long stick. If you stumble at the start of the hallway, your whole body (and the end of the stick) is off. The error at the start causes the error at the end. They are linked.
The Solution: The new system calculates these "linked errors" (correlations). It says, "I know my guess for point A is slightly off, and because of that, I know my guess for point B is also off in a specific way."
The Result: When it "un-smears" the photo, it doesn't just shift the points; it adds a "fuzziness" (uncertainty) to the points that were shifted using the most error-prone guesses. This tells the robot: "This part of the map is a bit shaky, so trust it less." This makes the final map much more reliable.

The Big Picture: SE(3)-LIO

The paper combines these two ideas into a single robot brain:

Smooth Propagation: It uses the "single highway" method to predict where the robot is going, even during crazy, fast movements.
Smart Un-Blurring: It uses the "linked errors" method to clean up the blurry LiDAR photos, knowing exactly how much to trust each part of the cleaned-up image.

Why does this matter?
The authors tested this on drones flying aggressively and cars driving over bumpy, rough terrain.

Old systems would get confused, drift off course, or get lost when the robot moved fast or the ground was uneven.
SE(3)-LIO stayed on track, producing a smooth, accurate map even in the most chaotic environments.

In short: They stopped treating movement as separate math problems and started treating it as one smooth, connected reality. They also stopped guessing blindly about errors and started understanding how those errors are connected. The result is a robot that knows exactly where it is, even when it's moving like a race car.

1. Problem Statement

The paper addresses two critical limitations in existing LiDAR-Inertial Odometry (LIO) systems regarding IMU propagation and motion compensation:

Inaccurate Motion Prediction: Conventional methods typically represent the 6-DoF pose by separating rotation ($SO(3)$) and translation ( $\mathbb{R}^3$ ) and propagating them on their respective manifolds. This decoupling fails to effectively incorporate rotational variations into translation propagation. Consequently, under aggressive motion (high angular velocity), the predicted trajectory becomes inaccurate because the rotation-induced translation is neglected.
Uncertainty in Motion Compensation: To correct motion-induced distortions in LiDAR scans (e.g., "rolling shutter" effects), systems transform samples to a reference pose using relative transformations derived from IMU propagation. However, these predicted poses inherently contain errors due to sensor noise and numerical integration. Existing methods often treat these poses as independent, leading to an overestimation of uncertainty in the relative transformation. This results in suboptimal weighting of measurements, particularly for distant LiDAR points where small rotational errors cause large spatial deviations.

2. Methodology

The authors propose SE(3)-LIO, a framework that integrates two core innovations: Smooth IMU Propagation on the SE(3) Manifold and Uncertainty-Aware Motion Compensation (UAMC).

A. Smooth IMU Propagation on SE(3) Manifold

Instead of separating rotation and translation, the authors define the state vector on the SE(3) Lie group, representing the pose as a single transformation matrix $T$ .

Kinematic Model: The system uses a continuous-time kinematic model where the pose evolves based on the "twist" $\zeta = [v, \omega]^T$ in the body frame.
Propagation: The discrete propagation step utilizes the exponential map on SE(3). Crucially, the translation update term includes the Left Jacobian of the exponential map ( $J_l$ $J_{l}$ ), which mathematically couples rotational changes with translational updates.
- Result: The propagated translation properly accounts for rotational variation, yielding a smoother and more accurate trajectory, especially during dynamic maneuvers.
Error State: The system employs an Error-State Kalman Filter (ESKF), defining the error state $\tilde{x}$ on the tangent space of SE(3) to maintain consistency.

B. Uncertainty-Aware Motion Compensation (UAMC)

To address the uncertainty in motion compensation, the authors derive the joint distribution of predicted poses rather than assuming independence.

Joint Distribution Derivation: Since IMU propagation is recursive (the current state depends on the previous state and IMU noise), the error states of predicted poses are correlated. The authors derive the joint covariance matrix of these poses, explicitly calculating the cross-terms between different time steps.
Relative Transformation Uncertainty: Using the Baker-Campbell-Hausdorff (BCH) formula, the uncertainty of the relative transformation between a reference pose and a sample pose is computed. This calculation includes the cross-terms derived from the joint distribution.
Probabilistic Undistortion: During motion compensation, the uncertainty of the relative transformation is incorporated into the measurement noise of each LiDAR point. This creates a probabilistic undistorted point cloud, where points with higher uncertainty (e.g., those transformed from distant past poses) are weighted less during the optimization step.

C. Integration Framework

The full pipeline operates as follows:

Prediction: IMU data is propagated on the SE(3) manifold to generate a prior state and its covariance.
Compensation: Raw LiDAR points are undistorted to the reference time using relative transformations. The uncertainty of these transformations is calculated via UAMC and added to the point noise.
Update: An ESKF update step optimizes the state by minimizing the residual between the probabilistic undistorted point cloud and a local voxel map (using VoxelMap for feature extraction).

3. Key Contributions

SE(3) Manifold Propagation: A novel IMU propagation method that evolves poses on the SE(3) manifold, ensuring rotational variations are correctly integrated into translation, significantly improving motion prediction accuracy.
Uncertainty-Aware Motion Compensation (UAMC): A method to precisely characterize relative transformation uncertainty by considering the correlation (cross-terms) between jointly distributed predicted poses, preventing uncertainty overestimation.
SE(3)-LIO Framework: An integrated LIO system combining the above methods, validated to be more accurate and robust than state-of-the-art methods.
Comprehensive Validation: Extensive experiments on diverse datasets (aggressive drone motion, handheld shaking, and rough terrain ground vehicles) demonstrating superior performance.

4. Experimental Results

The authors evaluated SE(3)-LIO against state-of-the-art methods (FAST-LIO2, DLIO, MA-LIO, LIO-EKF) on three datasets: NTU-VIRAL, Newer-College, and a custom Ground Vehicle dataset.

Quantitative Accuracy:
- NTU-VIRAL: SE(3)-LIO achieved the lowest Absolute Trajectory Error (ATE) across all sequences, including the highly dynamic spms sequence (rotational speed > 1.6 rad/s, linear speed > 5.0 m/s) where other methods degraded or diverged.
- Newer-College: Achieved the best performance in most sequences, even with downsampled (sparse) point clouds.
- Custom Dataset: In large-scale rough terrain, SE(3)-LIO reduced error by more than 50% compared to the second-best method in structured/unstructured mixed environments.
Ablation Study:
- Switching from $SO(3) \times \mathbb{R}^3$ to SE(3) propagation alone significantly reduced error in aggressive motion sequences.
- Adding UAMC consistently improved accuracy across all sequences, validating the importance of accurate uncertainty modeling.
Qualitative Results: Visual comparisons showed that SE(3)-LIO maintained tight loop closures and minimal drift in scenarios where competitors (like FAST-LIO2) exhibited significant trajectory drift.
Computational Efficiency:
- SE(3)-LIO runs at approximately 20.46 ms per frame on an Intel i9-14900K, comparable to FAST-LIO2 (15.90 ms) and significantly faster than DLIO (~99 ms).
- The proposed modules (SE(3) propagation and UAMC) add minimal overhead, accounting for only ~18% of the total computation time.

5. Significance

This paper makes a significant contribution to the field of robotic state estimation by rigorously addressing the geometric and statistical limitations of current IMU propagation techniques.

Theoretical Advancement: It demonstrates that decoupling rotation and translation in IMU propagation is a fundamental source of error in dynamic environments and that treating poses as a joint distribution on SE(3) is necessary for accurate uncertainty estimation.
Practical Impact: The proposed method enables LiDAR-inertial systems to operate robustly in highly dynamic, unstructured, and large-scale environments (e.g., aggressive drone flight, rough terrain driving) where previous methods often fail or drift.
Efficiency: By achieving state-of-the-art accuracy without sacrificing real-time performance, SE(3)-LIO provides a viable solution for deployment on resource-constrained autonomous platforms.