Eigen Is All You Need: Efficient Lidar-Inertial Continuous-Time Odometry with Internal Association

TL;DR

This work tackles real-time continuous-time lidar-inertial odometry (CT-LIO) by replacing the traditional nonlinear least-squares solver with an efficient linear-on-manifold solver in a B-spline trajectory framework. The key innovation is a solve-associate-solve loop in which internal feature-to-map association is performed after each incremental step, enabling convergence in just a few iterations while maintaining high control-point density. The authors provide detailed derivations of Residuals and Jacobians for lidar and IMU factors over the B-spline control points, and implement a parallelized pipeline that achieves real-time performance on dense inputs. Empirical results on public CT-LIO benchmarks show competitive accuracy with substantial speedups over NLS-based approaches, and ablation confirms the value of per-iteration association for robustness and precision.

Abstract

In this paper, we propose a continuous-time lidar-inertial odometry (CT-LIO) system named SLICT2, which promotes two main insights. One, contrary to conventional wisdom, CT-LIO algorithm can be optimized by linear solvers in only a few iterations, which is more efficient than commonly used nonlinear solvers. Two, CT-LIO benefits more from the correct association than the number of iterations. Based on these ideas, we implement our method with a customized solver where the feature association process is performed immediately after each incremental step, and the solution can converge within a few iterations. Our implementation can achieve real-time performance with a high density of control points while yielding competitive performance in highly dynamical motion scenarios. We demonstrate the advantages of our method by comparing with other existing state-of-the-art CT-LIO methods. The source code will be released for the benefit of the community.

Figures (9)

• Figure 1: Comparison between discrete-time (top) and continuous-time (bottom) optimization schemes. In the discrete case, measurements are coupled with distinct state estimates on a sliding window. Often, measurements are not acquired at the state's timestamp, thus some techniques are needed to enforce this strict state-measurement coupling paradigm. For example, the IMU measurements (small green squares) need to be preintegrated to provide a factor that couples two consecutive discrete states (large green squares). On the other hand, for continuous-time optimization, measurements can be coupled directly with the trajectory estimate via the interpolated states. These interpolated states can be understood as time-weighted combination of the control points -- the actual decision variable in our optimization problem. In this illustration, we imply a spline order $N=3$, hence, each interpolated state is coupled with three control points (the pink segments).
• Figure 2: SLICT2 is a CT-LIO system with a simple yet efficient pipeline. $i$ is the index of the inner loop, starting from 0. We refer to the sequence 3.1 -- 3.2 -- 3.3 -- 2.2 -- 3.1 as the inner loop, and the outer loop refers to the sequence 2.1 -- (inner loops) --- 2.1. Description of each block is given in Sec. \ref{['sec: main workflow']}.
• Figure 3: The structure of the Jacobian (left) and residual (right). The coupling of state estimates with IMU and lidar measurements in this illustration reflect the graph at Fig. \ref{['fig: discrete vs continuous']}. Note that the block size represented by each cell is different for IMU and lidar factor. More detailed descriptions are given in Sec. \ref{['sec: residual and jacobian']} and Sec. \ref{['sec: parallel']}.
• Figure 4: The map and trajectory (red) of FAST-LIO (left) and SLICT2 (right) in xxx_day_01 sequence. We can see that FAST-LIO has a slight drift in the vertical direction, leading to its larger APE in this sequence.
• Figure 5: The environment in the tnp_02 sequence in NTU VIRAL dataset with significant noise from sun glare and glass wall. Also when the UAV is flying high (the trajectory of UAV is in red), there are few features that can constrain the vertical drift. This leads to the divergence of SLICT.

Theorems & Definitions (3)

• Remark 1
• Remark 2
• Remark 3