CT-SGAL-LIO: Causal Continuous Time Refinement of Discrete LiDAR Inertial Odometry
Authors: Nam Tran, Thu Tran, Toan Nguyen, William J Beksi, Tuan Dang
Organizations: Department of Electrical Engineering and Computer Science, University of Arkansas, Fayetteville, AR, USA · VinRobotics · Department of Computer Science, University of Texas at Dallas, Richardson, TX, USA
State-of-the-art LiDAR-inertial odometry methods, including those on richer state manifolds, achieve strong global accuracy, yet their pose stream alternates between propagation and correction and can oscillate over short horizons even when globally accurate. Most existing methods follow these approaches: they either modify the filter to enable faster updates or estimate a continuous-time trajectory from raw LiDAR and inertial data. The latter involves extra optimization, delaying state updates and becoming less robust during rapid rotations or accelerations. We propose CT-SGAL-LIO, a continuous-time refinement module that smooths the posterior pose stream of a discrete SGal(3) estimator without modifying the estimator itself. The key novelty is to perform continuous-time trajectory refinement directly on the projected SGal(3) posterior: we fit a cubic trajectory over a bounded window of recent poses and evaluate it at a small fixed lag. This places the query time within the observed interval, so the refined pose is obtained by interpolation rather than extrapolation, while preserving causal operation. CT-SGAL-LIO improves relative trajectory accuracy on 17 sequences from three public benchmarks. Across 17 sequences from GRAND-TOUR, MCD, and GEODE, CT-SGAL-LIO improves short-term motion accuracy while largely preserving global accuracy. RTE improves by up to 70% on GRAND-TOUR and on all seven GEODE sequences, with 9.9-51.1% reductions. A 19.5-min R-Campus run also reduces posterior jumps by 85.4% with only a 4.7 mm change in the end-to-start gap. We plan to release our source code and results to support community validation and reproducibility.
Figures & tables
Fig. 1: CT-SGAL-LIO maintains a persistent fixed-lag memory of incoming LIO posterior states and reconstructs a continuous trajectory by robust metric-weighted cubic fitting in a local SE(3) tangent chart. At the delayed query time tk⋆=tk−ℓ , the fitted trajectory provides the refined pose and its linear and angular velocities. The orange segment marks the illustrated local support.
Fig. 2: Overview of CT-SGAL-LIO. Posterior states from the discrete SGal(3) LIO estimator are maintained in a bounded temporal memory and reconstructed by covariance-aware robust cubic fitting. The resulting continuous trajectory is evaluated at the delayed query time tk⋆=tk−ℓ to obtain the refined state.
Fig. 3: Qualitative comparison between LIMOncello and CT-SGAL-LIO on the day01 sequence of the MCD NTU Campus dataset. (a) Estimated trajectories compared with the ground truth, (b) ATE over time, and (c) RTE evaluated at different time intervals Δt . CT-SGAL-LIO preserves a trajectory close to the baseline while substantially reducing short-term relative motion error.
Fig. 4: Long-duration online behavior of CT-SGAL on the R-Campus deployment. (a) The refined trajectory remains closely aligned with the LIO posterior over a 19.5-min sequence, while the inset shows a locally continuous CT trajectory reconstructed from discrete posterior samples. (b) CT-SGAL strongly attenuates consecutive-state posterior jumps, reducing p95(rk) from 16.5 to 2.4 m/s ( 85.4% ), while introducing only a 4.7 mm change in the 3-D end-to-start gap.
Method
ARC1
Con_1
Con_3
HAUS_1
ATE
RTE 0.1s
ATE
RTE 0.1s
ATE
RTE 0.1s
ATE
RTE 0.1s
LIMOncello
0.0686
0.0296
0.0678
0.0310
0.0236
0.0158
0.0277
0.0257
RESPLE
0.0251
0.0305
0.0630
0.0343
0.0635
0.0236
0.0393
0.0307
FAST-LIO2
0.0250
0.0284
0.0311
0.0321
15.6995
0.0843
0.0214
0.0285
CT-SGAL-LIO
0.0654
0.0123
0.0635
0.0090
0.0208
0.0085
0.0189
0.0084
TABLE I: Quantitative comparison on the GRAND-TOUR dataset in terms of ATE RMSE and RTE RMSE at a time interval of 0.1 s. All values are reported in meters. Lower is better, and the best result for each metric is highlighted in bold .
Method
day01
day02
day10
night04
night08
night13
ATE
RTE 0.1s
ATE
RTE 0.1s
ATE
RTE 0.1s
ATE
RTE 0.1s
ATE
RTE 0.1s
ATE
RTE 0.1s
LIMOncello
0.7416
0.0810
0.2246
0.0185
3.1404
0.0312
0.8877
0.0221
1.0046
0.0232
1.6388
0.0690
RESPLE
0.4693
0.0276
0.2634
0.1712
1.6488
0.1335
–
–
8.6461
0.1082
1.1414
0.0442
FAST-LIO2
0.8075
0.0213
0.3176
0.1161
0.9861
0.1404
0.9041
0.0936
1.4239
0.1219
2.8225
0.0595
CT-SGAL-LIO
0.7389
0.0244
0.2245
0.0168
3.1403
0.0327
0.8878
0.0208
1.0045
0.0235
1.6376
0.0501
TABLE II: Quantitative comparison on the NTU Campus subset of the MCD dataset. We evaluate day and night sequences recorded using a Livox Mid70 LiDAR and a VN100 IMU. ATE RMSE and RTE RMSE at a time interval of 0.1 s are reported in meters. Lower is better. The best and second-best results for each metric are highlighted in bold and underlined , respectively.
Waterways Long
Waterways Med.
Waterways Short
Offroad2
Offroad4
Offroad7
Stairs
Method
ATE
RTE 0.1
ATE
RTE 0.1
ATE
RTE 0.1
ATE
RTE 0.1
ATE
RTE 0.1
ATE
RTE 0.1
ATE
RTE 0.1
LIMOncello
44.8979
0.0619
14.3201
0.0682
34.8550
0.0635
0.4449
0.0592
0.2814
0.0587
0.3636
0.0576
3.8750
0.0481
RESPLE
105.6537
0.1278
137.5077
0.1596
1.9073
0.0812
0.4627
0.0643
0.2968
0.0654
0.3803
0.0615
4.4882
0.0952
FAST-LIO2
140.7757
0.1040
65.9198
0.0901
30.9266
0.0484
0.4580
0.0518
0.2883
0.0539
0.3708
0.0506
1.3639
0.0292
CT-SGAL-LIO
44.8954
0.0420
14.3195
0.0387
34.8558
0.0515
0.4398
0.0289
0.2798
0.0310
0.3608
0.0337
3.8693
0.0433
TABLE III: Quantitative comparison on GEODE. ATE RMSE and RTE RMSE at Δt=0.1 s are reported in meters. All methods are evaluated on the same ground-truth associations within each sequence. Lower is better; best and second-best results are shown in bold and underlined, respectively.
Variant
ATE
RTE 0.1
p 99 residual
Input posterior
1.0000
1.0000
1.0000
No output lag ( ℓ=0 )
0.9964
0.6918
0.0173
No curvature term
0.9961
0.6394
0.0081
No Huber loss
0.9962
0.6314
0.0035
Full CT-SGAL
0.9963
0.6370
0.0035
TABLE IV: GEODE ablation on nominal data. Values are geometric-mean ratios to the input posterior over seven sequences (input =1 ; lower is better). The p 99 residual is computed from a 50-ms local linear fit at 100 Hz.
Maintaining stable and accurate localization during fast motion or on rough terrain remains highly challenging for mobile robots with onboard resources. Currently, multi-sensor fusion methods based on continuous-time representation offer a potential and effective solution to this challenge. Among these, spline-based methods provide an efficient and intuitive approach for continuous-time representation. Previous continuous-time odometry works based on B-splines either treat control points as variables to be estimated or perform estimation in quaternion space, which introduces complexity in deriving analytical Jacobians and often overlooks the fitting error between the spline and the true trajectory over time. To address these issues, we first propose representing the increments of control points on matrix Lie groups as variables to be estimated. Leveraging the feature of the cumulative form of B-splines, we derive a more compact formulation that yields simpler analytical Jacobians without requiring additional boundary condition considerations. Second, we utilize forward propagation information from IMU measurements to estimate fitting errors online and further introduce a hybrid feature-based voxel map management strategy, enhancing system accuracy and robustness. Finally, we propose a re-estimation policy that significantly improves system computational efficiency and robustness. The proposed method is evaluated on multiple challenging public datasets, demonstrating superior performance on most sequences. Detailed ablation studies are conducted to analyze the impact of each module on the overall pose estimation system.
LiDAR-inertial odometry (LIO) systems differ in whether they continue estimating gravity after initialization. We compare four gravity-bias state configurations in each of FAST-LIO2 and LIO-SAM, then separately test a gravity-direction factor. Across 12 dataset sequences evaluated with FAST-LIO2, fixing gravity under continuous LiDAR correction produces mean paired changes in vertical and 3D position errors with 90% confidence intervals within ±2%. Tests on 4 sequences with LIO-SAM likewise show no consistent benefit from online gravity. Multi-second LiDAR outages, unlike reduced range or field of view, reveal trajectory-dependent costs of fixing gravity. A history-matched 23D-to-21D switch places the repeatable 3D error increase after LiDAR updates resume. Under 5-s outages, a direction factor from the same IMU used for preintegration improves accuracy on Hall05 but worsens both errors with online gravity on TUHH. Dynamic-start tests also show fixed-bias failures at particular starting phases. We recommend keeping gravity and accelerometer bias online for robustness; use a direction factor only after verifying vertical and 3D accuracy gains under the intended operating conditions.
This paper introduces 2Fast-2Lamaa, a lidar-inertial state estimation framework for odometry, mapping, and localization. Its first key component is the optimization-based undistortion of lidar scans, which uses continuous IMU preintegration to model the system's pose at every lidar point timestamp. The continuous trajectory over 100-200ms is parameterized only by the initial scan conditions (linear velocity and gravity orientation) and IMU biases, yielding eleven state variables. These are estimated by minimizing point-to-line and point-to-plane distances between lidar-extracted features without relying on previous estimates, resulting in a prior-less motion-distortion correction strategy. Because the method performs local state estimation, it directly provides scan-to-scan odometry. To maintain geometric consistency over longer periods, undistorted scans are used for scan-to-map registration. The map representation employs Gaussian Processes to form a continuous distance field, enabling point-to-surface distance queries anywhere in space. Poses of the undistorted scans are refined by minimizing these distances through non-linear least-squares optimization. For odometry and mapping, the map is built incrementally in real time; for pure localization, existing maps are reused. The incremental map construction also includes mechanisms for removing dynamic objects. We benchmark 2Fast-2Lamaa on over 750km of public and self-collected datasets from both automotive and handheld systems. The framework achieves state-of-the-art performance across diverse and challenging scenarios, reaching odometry and localization errors as low as 0.22% and 0.06 m, respectively. The real-time implementation is publicly available at https://github.com/clegenti/2fast2lamaa.
Cedric Le Gentil, Raphael Falque, Daniil Lisus +1
Robotics Institute, University of Toronto Institute for Aerospace Studies, Canada · Robotics Institute, University of Technology Sydney, Australia