CT-SGAL-LIO: Causal Continuous Time Refinement of Discrete LiDAR Inertial Odometry
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.
From the abstract. Our summary is in progress.
Full paper
CT-SGAL-LIO: Causal Continuous Time Refinement of Discrete LiDAR Inertial Odometry
Published under CC BY 4.0. Reproduced with attribution; original at arXiv:2611.16918 (PDF).
Abstract
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 \mathrm{SGal}(3) estimator without modifying the estimator itself. The key novelty is to perform continuous-time trajectory refinement directly on the projected \mathrm{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.
I Introduction
LiDAR–inertial odometry (LIO) underlies mapping, planning, and control on mobile robots, and downstream consumers routinely query the pose stream at rates decoupled from the LiDAR scan rate. State-of-the-art tightly coupled iterated filters, most recently formulated on the Galilean manifold \mathrm{SGal}(3) [1], achieve strong global trajectory precision. However, the state stream that such filters publish alternates between IMU-propagated states and LiDAR-corrected states, which can exhibit different local error characteristics. As a result, a globally accurate trajectory can still contain short-horizon oscillations, visible even on our own baseline: on the GRAND-TOUR [2] HAUS_1 sequence, the discrete estimator we build on already achieves a low global error, yet its relative motion error over short intervals still fluctuates, and CT-SGAL-LIO reduces this short-term error by 67% without altering the global trajectory (Fig. 1).
SE(3) tangent chart. At the delayed query time t_{k}^{\star}=t_{k}-\ell, the fitted trajectory provides the refined pose and its linear and angular velocities. The orange segment marks the illustrated local support.Existing remedies for this local inconsistency modify the discrete estimator itself. One direction raises the state-update rate by restructuring the filter to process individual points rather than whole scans. A second direction replaces the discrete state altogether with a continuous-time trajectory representation fitted jointly to raw LiDAR and, where available, inertial residuals. This approach works well, but it has two drawbacks, discussed in Section II. Smooth parametric trajectories can struggle with aggressive motion, and fitting them with future measurements introduces delay. Neither direction considers a simpler alternative: keep the discrete LIO estimator and its map untouched, and refine only the pose stream after it is produced.
We answer this question with CT-SGAL-LIO, an online continuous-time refinement layer that treats the discrete posterior as an observation rather than as a state to be re-derived. CT-SGAL-LIO maintains a persistent, bounded-memory buffer of recent posterior poses and fits a robust cubic trajectory over this window in a local \mathrm{SE}(3) Lie-algebra chart. The refined trajectory is evaluated at a fixed lag, placing the query time inside the observed window rather than at its boundary. This lets us obtain the pose by interpolation rather than extrapolation, without sacrificing causality. Because CT-SGAL-LIO consumes only the timestamped \mathrm{SE}(3) projection of the estimator’s posterior state and feeds nothing back into the filter or its map, it is decoupled from the underlying estimator and can in principle attach to any discrete LiDAR-inertial system that exposes a pose stream. In this paper, we instantiate it on top of LIMOncello [1], the estimator whose \mathrm{SGal}(3) posterior it refines.
Our contributions are as follows.
- We propose a model-agnostic, causal, fixed-lag continuous-time refinement layer that operates purely on the posterior pose stream of a discrete LIO estimator, without coupling to raw sensor residuals or to the map.
- We formulate this refinement as robust cubic trajectory fitting in a local
\mathrm{SE}(3)Lie-algebra chart, combining iteratively reweighted least squares with a curvature prior tailored to short, bounded temporal windows. - We show that attaching CT-SGAL-LIO to LIMOncello’s
\mathrm{SGal}(3)posterior consistently improves short-term motion accuracy while generally preserving global trajectory accuracy across 17 sequences from three public benchmarks (GRAND-TOUR, MCD [3], and GEODE [4]). Specifically, CT-SGAL-LIO reduces RTE by up to 70% on GRAND-TOUR and by 9.9–51.1% on all seven GEODE sequences, while ranking first or second in RTE on all six evaluated MCD sequences.
The remainder of this paper is organized as follows. Section II reviews discrete-time LIO, manifold state representations, continuous-time trajectory estimation, and fixed-lag smoothing. Section III overviews the system architecture. Section IV details the fixed-lag continuous-time refinement formulation. Section V presents our experimental evaluation. Section VI concludes the paper.
II Related Work
II-A Discrete-time LIO on Manifold State Representations
Most LiDAR odometry systems follow LOAM [5]: extract geometric features, estimate scan-to-pose correspondences, and register to a map. Tightly coupled LiDAR–inertial designs then became standard, either as sliding-window smoothers [6] or as iterated filters standardized by IKFoM [7]. FAST-LIO [8] introduced IMU back-propagation with a Kalman gain that scales with the state dimension rather than the measurement dimension, and FAST-LIO2 [9] removed hand-engineered feature extraction by registering raw points directly to an incremental k-d tree, ikd-Tree [10]. This family publishes only one pose per scan, and Point-LIO [11, 12] shows that this scan-rate update imposes a Nyquist-limited bandwidth. The state representation from this work is typically a bundle manifold SO(3)\times\mathbb{R}^{6}, which ignores the coupling between rotation and translation; richer Lie groups restore this coupling along the nested hierarchy SO(3)<SE(3)<SE_{2}(3)<SGal(3) [13], with SE_{2}(3) underpinning invariant filtering [14] and SGal(3) additionally carrying a temporal coordinate. LIMOncello [1] is, to our knowledge, the first tightly coupled LIO system on SGal(3) within IKFoM, and is the estimator we build on: because SE(3) is a proper subgroup of SGal(3), the pose \Pi_{SE(3)}(G_{i}) is a well-defined projection, and it is the quantity on which our refinement layer operates. We leave this discrete-time estimator unchanged and instead regularize the state sequence that it already produces.
II-B Continuous-time trajectory estimation
Continuous-time methods represent the trajectory as a function of time, so measurements can be fused at their true timestamps and motion distortion is estimated jointly with the motion [15]. Two parameterizations are commonly used. Piecewise-linear models assume constant velocity over short intervals, as in standard deskewing. Cubic B-splines use more parameters but give a smoother trajectory. CT-ICP [16] uses a begin and an end pose per scan and deliberately allows inter-scan discontinuity, while Traj-LO [17] regularises short linear segments. Cubic B-splines are used in sliding-window LiDAR–inertial fusion by CLIC and SLICT2 [18, 19] and, more recently, inside a recursive filter, RESPLE [20], which embeds spline control points directly in an iterated Kalman filter and avoids sliding-window optimization altogether. DLIO [21] instead sidesteps control-point fitting entirely, integrating a closed-form constant-jerk model. Smooth trajectory methods like CT-ICP, DLIO, and Point-LIO argue that control-point fitting can smooth out rapid motion, while RESPLE finds cubic splines sufficiently flexible in practice. Continuous-time methods also introduce latency because future measurements must be collected before the trajectory parameters are well constrained and the state can be queried [11]. CT-ICP, CLIC, SLICT2, and RESPLE optimize their trajectory parameters directly against raw LiDAR (and, where used, inertial) residuals, so the continuous-time representation is their estimator rather than a separable layer on top of it. CT-SGAL-LIO instead operates only on the timestamped pose stream that LIMOncello already publishes, with no feedback into the filter or its map, which lets the refinement layer attach to any discrete LiDAR-inertial estimator that exposes a pose stream.
II-C Smoothing and Fixed-Lag Estimation in LiDAR Odometry
Fixed-lag smoothing bounds the estimation delay by withholding the output until a short buffer of subsequent measurements becomes available [22]. In LiDAR odometry, smoothing is usually applied instead as a factor-graph back-end [6] or a sliding window of spline control points with marginalisation [18, 19, 17]. A related formulation is Wildcat [23], which fits a cubic B-spline to sample poses by IRLS, following the offline systems of Bosse and Zlot [24]; there, however, the spline is internal to the estimator, since the sample poses are themselves optimized against IMU and surfel residuals and the interpolated trajectory is fed back to update correspondences. Because Wildcat fits the spline inside its pipeline, the spline is part of the estimator rather than a separate refinement stage. Comparing our method directly with Wildcat would therefore confound the effect of trajectory refinement with differences in the underlying estimator. To isolate the effect of our refinement, we compare against LIMOncello and RESPLE, the discrete estimators whose posterior pose streams serve as input to our method. CT-SGAL-LIO treats these posterior poses as observations, fits a continuous trajectory over a bounded history window, and evaluates the trajectory at a fixed lag so that the query time lies inside the observed interval. This keeps the refinement causal while leaving the original LiDAR–inertial estimator and map unchanged.
t_{k}^{\star}=t_{k}-\ell to obtain the refined state.III System Overview
The overall architecture of CT-SGAL-LIO is illustrated in Fig. 2. A discrete SGal(3)-based LiDAR–inertial estimator provides high-rate posterior states after IMU propagation and LiDAR correction. Instead of treating each posterior state independently, CT-SGAL-LIO keeps a short history of recent states and fits a continuous trajectory through them; CT-SGAL-LIO uses the recent posterior poses to fit a cubic trajectory that maps each pose to a local tangent space, and its covariance determines how strongly it influences the fit. A temporal regularizer limits unnecessary curvature. The trajectory is evaluated at the interior delayed time t_{k}^{\star}=t_{k}-\ell and mapped back to the pose manifold to obtain the refined pose and motion state. This design keeps the continuous-time refinement module separate from the LiDAR front end while remaining causal and suitable for online use.
IV Methodology
IV-A Problem Formulation
We consider a LiDAR–inertial estimator whose posterior state at time t_{i} evolves on the special Galilean group,
\mathbf{G}_{i}=\begin{bmatrix}\mathbf{R}_{i}&\mathbf{v}_{i}&\mathbf{p}_{i}\\
\mathbf{0}^{\top}&1&t_{i}\\
\mathbf{0}^{\top}&0&1\end{bmatrix}\in\mathrm{SGal}(3),where \mathbf{R}_{i}\in\mathrm{SO}(3), \mathbf{p}_{i}\in\mathbb{R}^{3}, and \mathbf{v}_{i}\in\mathbb{R}^{3} are the estimated orientation, position, and velocity. We use LIMOncello [1] as the discrete underlying estimator. CT-SGAL constructs a continuous-time readout from its pose posterior \mathbf{T}_{i}=(\mathbf{R}_{i},\mathbf{p}_{i})\in\mathrm{SE}(3) and the associated covariance \boldsymbol{\Sigma}_{i}.
The estimator produces timestamped posterior states \{(t_{i},\mathbf{T}_{i},\boldsymbol{\Sigma}_{i})\}_{i=1}^{k}. Because IMU propagation and LiDAR correction occur at different rates, the sampled trajectory can contain short-term variations around correction events. These variations become visible when the trajectory is queried at a higher rate than the LiDAR updates.
CT-SGAL-LIO processes this stream online using a time-bounded pose buffer. At the arrival of state k, the new posterior (t_{k},\mathbf{T}_{k},\boldsymbol{\Sigma}_{k}) is inserted into the buffer \mathcal{M}_{k}, while states older than the memory horizon \Delta_{m} are removed by \mathcal{P}_{\Delta_{m}}. The output is placed at the fixed-lag time t_{k}^{\star}=t_{k}-\ell, and the active window is defined as
\mathcal{W}_{k}=\left\{(t_{i},\mathbf{T}_{i},\boldsymbol{\Sigma}_{i})\in\mathcal{M}_{k}\;\middle|\;t_{k}^{\star}-W\leq t_{i}\leq t_{k}\right\}.Here, W is the history retained before the query time, and \ell is the output lag. Unlike pairwise interpolation, each CT-SGAL estimate uses all posterior states in \mathcal{W}_{k}, while \mathcal{M}_{k} persists across successive updates.
The active interval [t_{k}^{\star}-W,t_{k}] is normalized before fitting. Its center is c_{k}=(t_{k}^{\star}-W+t_{k})/2, with half-width h=(W+\ell)/2. Each sample is represented by the normalized coordinate
u_{i}=\frac{t_{i}-c_{k}}{h},\qquad u_{k}^{\star}=\frac{t_{k}^{\star}-c_{k}}{h}=\frac{W-\ell}{W+\ell}.This maps [t_{k}^{\star}-W,t_{k}] to [-1,1]. For W>0 and \ell>0, the query lies strictly inside the observed interval. The fit therefore uses states both before and after t_{k}^{\star} without accessing measurements later than the current processing time t_{k}.
Directly fitting rotation matrices does not preserve \mathrm{SO}(3). The first pose in \mathcal{W}_{k} serves as the local reference, \mathbf{T}_{r,k}=(\mathbf{R}_{r,k},\mathbf{p}_{r,k}). Each pose is represented by its translation and rotation relative to this reference:
\mathbf{z}_{i}=\begin{bmatrix}\mathbf{p}_{i}-\mathbf{p}_{r,k}\\
\operatorname{Log}\!\left(\mathbf{R}_{r,k}^{\top}\mathbf{R}_{i}\right)\end{bmatrix}\in\mathbb{R}^{6}.The resulting six-dimensional coordinates are modeled by a cubic polynomial. Using \mathbf{b}(u)=[1,u,u^{2},u^{3}]^{\top} and \mathbf{A}_{k}=[\mathbf{A}_{p,k}^{\top}\ \mathbf{A}_{R,k}^{\top}]^{\top}, with \mathbf{A}_{p,k},\mathbf{A}_{R,k}\in\mathbb{R}^{3\times 4}, the fitted trajectory is
\hat{\mathbf{z}}_{k}(u)=\mathbf{A}_{k}\mathbf{b}(u).The corresponding continuous pose is
\hat{\mathbf{T}}_{k}(u)=\begin{bmatrix}\mathbf{R}_{r,k}\operatorname{Exp}\!\left(\mathbf{A}_{R,k}\mathbf{b}(u)\right)&\mathbf{p}_{r,k}+\mathbf{A}_{p,k}\mathbf{b}(u)\\
\mathbf{0}^{\top}&1\end{bmatrix}.The exponential map ensures that the reconstructed orientation remains in \mathrm{SO}(3) for every u.
Let \mathcal{I}_{k} contain the indices of the observations in \mathcal{W}_{k}, and define \mathbf{r}_{i}(\mathbf{A})=\mathbf{z}_{i}-\mathbf{A}\mathbf{b}(u_{i}). We estimate the polynomial coefficients by
\displaystyle\mathbf{A}_{k}^{\star}=\arg\min_{\mathbf{A}} \\ \displaystyle\sum_{i\in\mathcal{I}_{k}}\rho_{\delta}\!\left(\|\mathbf{r}_{i}(\mathbf{A})\|_{\boldsymbol{\Omega}_{i}}\right) \\ \displaystyle+\sum_{q\in\mathcal{Q}}\left\|\mathbf{A}\ddot{\mathbf{b}}(q)\right\|_{\boldsymbol{\Lambda}}^{2}+\lambda_{d}\|\mathbf{A}\|_{F}^{2},where \|\mathbf{x}\|_{\boldsymbol{\Omega}}=\sqrt{\mathbf{x}^{\top}\boldsymbol{\Omega}\mathbf{x}}, \rho_{\delta} is the Huber loss [25], and \mathcal{Q}=\{-1,-0.5,0,0.5,1\} contains the curvature evaluation points. We use \boldsymbol{\Lambda}=\operatorname{diag}(\lambda_{p}\mathbf{I}_{3},\lambda_{R}\mathbf{I}_{3}) to tune translational and rotational curvature separately.
The first term fits the recent posterior states using their uncertainty. The Huber loss limits the effect of abrupt correction events, while the curvature term suppresses oscillatory solutions. Thus, each update combines multiple propagation and correction states rather than refining the newest pose independently.
IV-B Online Estimation and Implementation Details
CT-SGAL maintains \mathcal{M}_{k} as a time-ordered buffer. A state with the same timestamp as the newest entry replaces that entry, while out-of-order states are discarded. Fitting starts once the buffer spans the required interval and is repeated whenever the query time t_{k}^{\star} advances.
The input covariance controls the weight of each posterior pose. We define a minimum covariance \boldsymbol{\Sigma}_{0}=\operatorname{diag}(\sigma_{p}^{2}\mathbf{I}_{3},\sigma_{R}^{2}\mathbf{I}_{3}). Before inversion, a valid covariance is conditioned as
\displaystyle\tilde{\boldsymbol{\Sigma}}_{i} \\ \displaystyle=\operatorname{EigFloor}\!\left(\operatorname{sym}(\boldsymbol{\Sigma}_{i})+\boldsymbol{\Sigma}_{0},\epsilon\right), \\ \displaystyle\boldsymbol{\Omega}_{i} \\ \displaystyle=\tilde{\boldsymbol{\Sigma}}_{i}^{-1}.where \operatorname{sym}(\boldsymbol{\Sigma})=(\boldsymbol{\Sigma}+\boldsymbol{\Sigma}^{\top})/2, and \operatorname{EigFloor}(\cdot,\epsilon) replaces eigenvalues smaller than \epsilon by \epsilon. If the input covariance is unavailable or invalid, we use \tilde{\boldsymbol{\Sigma}}_{i}=\boldsymbol{\Sigma}_{0}. Thus, posterior poses with lower uncertainty receive greater weight in the fit.
The robust objective in Eq. (7) is solved using iteratively reweighted least squares (IRLS). At iteration j, the residual of observation i is \mathbf{r}_{i}^{(j)}=\mathbf{r}_{i}(\mathbf{A}_{k}^{(j)}). Its covariance-normalized magnitude is
e_{i}^{(j)}=\sqrt{\mathbf{r}_{i}^{(j)\top}\boldsymbol{\Omega}_{i}\mathbf{r}_{i}^{(j)}}.The corresponding Huber weight is
w_{i}^{(j)}=\begin{cases}1,&e_{i}^{(j)}\leq\delta,\\[2.84526pt]
\delta/e_{i}^{(j)},&e_{i}^{(j)}>\delta.\end{cases}Thus, \boldsymbol{\Omega}_{i} accounts for the uncertainty of each posterior state, while w_{i}^{(j)} reduces the influence of residuals that exceed the Huber threshold.
With the robust weights fixed, the cubic coefficients form a 24-dimensional weighted least-squares problem. We vectorize \mathbf{A}_{k} as \mathbf{a}_{k}=\operatorname{vec}(\mathbf{A}_{k}^{\top})\in\mathbb{R}^{24} and solve the normal system
\mathbf{H}_{k}^{(j)}\mathbf{a}_{k}^{(j+1)}=\mathbf{g}_{k}^{(j)}.Here, \mathbf{H}_{k}^{(j)} and \mathbf{g}_{k}^{(j)} collect the weighted data term, the curvature term, and the damping term in Eq. (7). Since the cubic trajectory has four coefficients for each of the six pose coordinates, the system size remains 24\times 24 regardless of the total trajectory length.
After the final IRLS iteration, the fitted trajectory is evaluated at the fixed-lag coordinate u_{k}^{\star}. The refined pose is
\hat{\mathbf{T}}_{k}^{\star}=\hat{\mathbf{T}}_{k}(u_{k}^{\star}).Because u=(t-c_{k})/h, we have du/dt=1/h. The translational velocity therefore follows directly from the derivative of the cubic basis:
\hat{\mathbf{v}}_{k}^{\star}=\mathbf{A}_{p,k}^{\star}\dot{\mathbf{b}}(u_{k}^{\star}),\qquad\dot{\mathbf{b}}(u)=\frac{1}{h}[0,1,2u,3u^{2}]^{\top}.For rotation, we use a centered Lie-group difference around the query time. With \Delta u=\Delta t/h, the angular velocity is
\hat{\boldsymbol{\omega}}_{k}^{\star}=\frac{1}{2\Delta t}\hat{\mathbf{R}}_{k}^{\star}\operatorname{Log}\!\left(\hat{\mathbf{R}}_{k}(u_{k}^{\star}-\Delta u)^{\top}\hat{\mathbf{R}}_{k}(u_{k}^{\star}+\Delta u)\right).The online output is (\hat{\mathbf{T}}_{k}^{\star},\hat{\mathbf{v}}_{k}^{\star},\hat{\boldsymbol{\omega}}_{k}^{\star}), with all quantities obtained from the same fitted trajectory.
V Evaluations
In this section, we present an extensive experimental evaluation of CT-SGAL-LIO, focusing on both global trajectory accuracy and short-term motion consistency. The evaluation is conducted on GRAND-TOUR, MCD, and GEODE, which collectively provide diverse sensing configurations, motion patterns, and challenging geometric conditions. Our experiments investigate whether the proposed fixed-lag continuous-time refinement can reduce local trajectory oscillations while preserving the global accuracy of the underlying discrete-time estimator. In addition to the benchmark evaluation, we use a long-duration R-Campus deployment to assess the long-horizon stability of CT-SGAL and its effect on the global structure of the underlying LIO trajectory.
V-A Experimental Setup
We evaluated CT-SGAL-LIO on a total of 17 sequences from three public datasets: four GRAND-TOUR sequences, six sequences from the NTU Campus subset of MCD, and seven challenging sequences from GEODE. The selected datasets also span different LiDAR–inertial sensing configurations: the GRAND-TOUR sequences use a Hesai XT32 LiDAR with a NovAtel CPT7 inertial system; for MCD NTU Campus we use the Livox Mid-70 LiDAR with a VectorNav VN100 IMU; and for GEODE we use the \alpha platform, a Velodyne VLP-16 LiDAR with an Xsens MTi-30 IMU. Together, these datasets cover a wide range of sensing configurations, motion patterns, and environmental conditions, including indoor and outdoor scenes, day and night operation, unstructured terrain, and geometrically challenging scenarios.
\Delta t. CT-SGAL-LIO preserves a trajectory close to the baseline while substantially reducing short-term relative motion error.p_{95}(r_{k}) 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.We compare CT-SGAL-LIO with its discrete-time baseline LIMOncello, together with RESPLE and FAST-LIO2. Trajectory accuracy is evaluated using the absolute trajectory error (ATE) RMSE and the relative trajectory error (RTE) RMSE at a temporal interval of 0.1\,\mathrm{s}, following standard trajectory evaluation practice [26] and computed with the evo toolbox [27]. Estimated trajectories are synchronized with the corresponding ground truth, and ATE is computed after a global rigid \mathrm{SE}(3) alignment. ATE measures global trajectory accuracy, whereas RTE characterizes short-term relative motion consistency and is therefore particularly relevant for evaluating the proposed continuous-time refinement. Lower values indicate better performance for both metrics.
V-B Qualitative Results
Figure 3 illustrates the effect of CT-SGAL-LIO on the MCD day01 sequence. While the refined and baseline trajectories remain nearly identical at the global scale, CT-SGAL-LIO consistently reduces the relative motion error over short evaluation intervals. This indicates that the proposed fixed-lag reconstruction primarily removes high-frequency trajectory fluctuations without noticeably altering the underlying global estimate.
- ATE
- RTE0.1s
- ATE
- RTE0.1s
- ATE
- RTE0.1s
- ATE
- RTE0.1s
- No data
- 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
- ATE
- RTE0.1s
- ATE
- RTE0.1s
- ATE
- RTE0.1s
- ATE
- RTE0.1s
- ATE
- RTE0.1s
- ATE
- RTE0.1s
- No data
- 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
- Method
- ATE
- RTE0.1
- ATE
- RTE0.1
- ATE
- RTE0.1
- ATE
- RTE0.1
- ATE
- RTE0.1
- ATE
- RTE0.1
- ATE
- RTE0.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
Finally, Fig. 4 examines the long-horizon behavior of the proposed refinement. Over a 19.5-min R-Campus deployment containing approximately 120.8k posterior states, CT-SGAL remains tightly anchored to the underlying LIO trajectory, with only a 4.7 mm change in the 3-D end-to-start gap. At the local scale, however, the reconstructed trajectory is considerably more temporally consistent: the 95th percentile of the consecutive-state position-rate statistic r_{k} decreases from 16.5 to 2.4 m/s, corresponding to an 85.4\% reduction. This result shows that CT-SGAL can continuously suppress posterior jumps without introducing noticeable long-term trajectory deformation.
V-C Quantitative Results
Tables I–III report the ATE and RTE results across GRAND-TOUR, MCD, and GEODE. Overall, CT-SGAL-LIO shows its clearest advantage in short-term motion consistency. On GRAND-TOUR, it achieves the lowest RTE on all four evaluated sequences, while also obtaining the best ATE on Con_3 and HAUS_1. In particular, the RTE reductions over LIMOncello are substantial across all four sequences, confirming that the continuous-time refinement consistently suppresses local posterior fluctuations.
A similar trend is observed on MCD. CT-SGAL-LIO ranks first or second in RTE on all six day and night sequences, while its ATE remains close to that of the underlying LIMOncello estimator. The greatest improvements occur on day01 and night13, whereas on day10 and night08 the two methods perform almost identically. This suggests that the refinement remains stable across different operating conditions and does not trade global accuracy for local smoothness.
On GEODE, CT-SGAL-LIO reduces RTE at \Delta t=0.1 s relative to LIMOncello on all seven sequences, with reductions ranging from 9.9% on Stairs to 51.1% on Offroad2. It achieves the lowest RTE on five sequences and the second-lowest on the remaining two. ATE remains close to the input posterior and is lowest on all three Offroad sequences and on Waterways Long and Medium. These results match the intended behavior of CT-SGAL: the largest change occurs in short-horizon relative motion, while the globally aligned trajectory remains anchored to the discrete estimator.
Across the three benchmarks, the results show that CT-SGAL-LIO does not systematically alter the global trajectory estimate; instead, it improves local motion consistency while generally preserving the ATE of the discrete-time estimator.
V-D Ablation Study
- Input posterior
- 1.0000
- 1.0000
- 1.0000
- No output lag (\ell=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
We ablate the output lag in Eq. (2), the curvature term in Eq. (7), and the Huber loss in Eq. (7) on the seven GEODE sequences. The full model uses (W,\ell)=(0.3,0.2) s. For the no-output-lag variant, we set (W,\ell)=(0.5,0) s such that the same 0.5-s history is retained. The other two variants remove the curvature term and replace the Huber loss with ordinary least squares, respectively.
As shown in Table IV, the full model reduces RTE0.1 by 36.3% with negligible change in ATE. The fixed output lag yields a further 7.9% reduction in RTE over the zero-lag variant, while the curvature term reduces the p99 residual by approximately half. Removing the Huber loss changes the nominal RTE by less than 1%, indicating that isolated posterior outliers are uncommon in these sequences.
We further evaluate its robustness by injecting identical 0.2-m translation impulses into 0.5\% of the posterior states on three Offroad sequences. Under these perturbations, the Huber loss reduces the geometric-mean p99 residual from 0.721 to 0.482 mm, while ATE changes from 0.35365 to 0.35373 m and RTE from 0.03048 to 0.03110 m. These results indicate that the Huber loss primarily suppresses isolated posterior errors rather than affecting nominal trajectory accuracy.
VI Conclusions
We presented CT-SGAL-LIO, a continuous-time refinement layer for the posterior pose stream of a discrete LiDAR–inertial estimator with causality. The method leaves the estimator and its map unchanged, using a history of posterior poses to fit a robust cubic trajectory in a local SE(3) and evaluating it at a fixed lag so that the query lies within the observed interval. Across 17 sequences from GRAND-TOUR, MCD, and GEODE, CT-SGAL-LIO mainly improves short-term motion consistency while preserving the global trajectory. It reduces RTE on all GRAND-TOUR sequences, with gains of up to 70%, while on GEODE it achieves the best results on 10 of 14 metrics and second-best on 3, including the best ATE and RTE across all three Offroad sequences. A 19.5-min R-Campus run also shows an 85.4% reduction in posterior jumps with only a 4.7 mm change in the end-to-start gap. The gains are not uniform, however, suggesting that refinement is most useful when the discrete posterior contains noticeable short-term fluctuations. Future work will test the approach with other LIO estimators and adapt the lag and fitting window to the incoming posterior’s behavior.