Continuous-Time Fixed-Lag Smoothing for
LiDAR-Inertial-Camera SLAM

Continuous-time trajectory representation includes linear interpolation, wavelets, Gaussian process and splines, in this paper we focus on B-spine based representation as explained in Sec. III-A. Pioneering work on solving B-spline based continuous-time SLAM problem is first systematically derived in  [38], where the authors propose to represent the states as a weighted sum of continuous temporal basis function and illustrate its application case in the extrinsic calibration between IMU and camera. Thereafter, B-spline based continuous-time trajectory formulation has been widely applied to SLAM-relevant applications, such as event camera odometry [20], and LiDAR-inertial odometry [17, 21], multi-camera SLAM system [24] and LIC fusion [16]. Lowe et al. [16] tightly couple LIC measurements to estimate state in continuous-time trajectory, however they assume no timeoffset between sensors which does not hold well in real world application, and prior information is discarded without any explanation. Our previous work [17] proposes a continuous-time based method for LI system, which manages the measurements within a local window and presents a two-stage loop closure strategy to obtain global-consistent trajectory. Some IMU measurements older than current scan are included in local window rather than applying marginalization technique to retain information about the old measurements, and in addition, it uses automatic derivation to solve the NLS problem. Due to these two aspects, the system is not able to run in real time.

In this paper, we propose a continuous-time based method that supports fusing measurements from LiDAR, IMU and camera, which is very easy to expand to fuse more other sensors to improve the accuracy of localization and mapping, such as fusing GPS in outdoor cases and RFID [39] in indoor cases.

We employ B-spline to parameterize continuous-time trajectory as it has the good property of locality and closed-form analytic time derivatives, $C^{k-1}$ smoothness for a spline of order $k$ (degree $k-1$) [40]. Specifically, we parameterize the continuous-time 6-DoF trajectory with uniform cumulative B-splines in a split representation format [41]. The translation $\mathbf{p}(t)$ of $k$ order over time $t\in[t_{i},t_{i+1})$ is controlled by the temporally uniformly distributed translational control points $\mathbf{p}_{i}$, $\mathbf{p}_{i+1}$, $\dots$, $\mathbf{p}_{i+k-1}$, and the matrix format [42] could be written as:

with difference vectors $\mathbf{d}_{j}^{i}=\mathbf{p}_{i+j}-\mathbf{p}_{i+j-1}\in\mathbb{R}^{3}$. The cumulative spline matrix $\widetilde{\mathbf{M}}^{(k)}$ of uniform B-spline only depends on the B-spline order. We further define $\bm{\lambda}(t)=\widetilde{\mathbf{M}}^{(k)}\mathbf{u}$ thus Eq. (1) can be written as

To parameterize the 3D rotation in $\mathit{SO}(3)$, we adopt the cumulative B-splines in Lie groups with the following expression over time $t\in[t_{i},t_{i+1})$:

where $\mathbf{R}_{i}\in\mathit{SO}(3)$ are the control points for rotation. The difference vector between two rotations is defined as $\mathbf{d}^{i}_{j}=\operatorname*{Log}\left(\mathbf{R}_{i+j-1}^{-1}\mathbf{R}_{i+j}\right)\in\mathbb{R}^{3}$ and $\mathbf{A}_{j}(t)=\operatorname*{Exp}\left(\lambda_{j}(t)\cdot\mathbf{d}_{j}\right)$ where omitting the $i$ to simplify notation, Eq. (3) can be written in the following concise equation:

In this paper, we select cubic (degree = 3) B-spline and the corresponding cumulative spline matrix $\widetilde{\mathbf{M}}^{(k)}$ is

As mentioned before, B-spline provides closed-form analytic derivatives, enabling the proposed system to fuse high-frequency IMU measurements seamlessly. Continuous-time trajectory of IMU in global frame $\{G\}$ is denoted as ${}^{G}_{I}\mathbf{T}(t)=\left[{}^{G}_{I}\mathbf{R}(t),{}^{G}\mathbf{p}_{I}(t)\right]$, and its time derivatives can be derived:

where $\dot{\mathbf{A}}_{j}=\operatorname*{Exp}\left(\dot{\lambda}_{j}(t)\cdot\mathbf{d}_{j}\right)$. Naturally, it’s straightforward to compute the linear accelerations and angular velocities in local IMU frame:

where ${}^{G}\mathbf{g}\in\mathbb{R}^{3}$ denotes the gravity vector in global frame.

In the following section, we model the continuous-time trajectory of IMU sensor in $\{G\}$ frame, termed as ${}^{G}_{I}\mathbf{T}(t)$. The global frame $\{G\}$ is determined by the first IMU measurement after system initialization by aligning its z-axis with the gravity direction, and the gravity can be denoted as $[0,0,9.8]$ in $\{G\}$. We assume the extrinsic rigid transformations between sensors are precalibrated [43], and we can get the camera trajectory ${}^{G}_{C}\mathbf{T}(t)$, and LiDAR trajectory ${}^{G}_{L}\mathbf{T}(t)$ handily by transferring IMU trajectory ${}^{G}_{I}\mathbf{T}(t)$ with the known extrinsic transformations.

We fuse heterogeneous IMU, LiDAR, and camera measurements in a factor graph optimization framework with the continuous-time trajectory formulation. Figure. 2 (a) and (b) show the factor graphs of the LI system and LIC system, respectively. Here we only illustrate the factor graph for LIC system, and it is straightforward to apply to LI system with minor adaptions. Specifically, our estimator in the temporal sliding window (detailed in Sec. V-A2) over $\left[t_{\kappa-1},t_{\kappa}\right)$ aims to estimate the following states:

which include active control points of B-splines $\bm{\Phi}(t_{\kappa-1},t_{\kappa})=\{\mathbf{x}_{R}^{\kappa},\mathbf{x}_{p}^{\kappa}\}$, the IMU biases $\mathbf{x}_{I_{b}}^{\kappa}$, the parameters of visual landmarks $\mathbf{x}_{\lambda}^{\kappa}$, and the temporal offset between LiDAR and IMU $t_{I}$ or camera $t_{C}$. Notably, LiDAR is taken as the base sensor in our multi-sensor fusion system, thus we need to align IMU and camera timestamps to LiDAR. Sec. V-C2 provides more details about the timeoffset estimation.

The factor graph needed to be solved consists of LiDAR factors $\mathbf{r}_{L}$, IMU factors $\mathbf{r}_{I}$, one bias factor $\mathbf{r}_{I_{b}}$, visual factors $\mathbf{r}_{C}$, and one prior factor $\mathbf{r}_{\text{prior}}$ induced from marginalization, the details of factors are provided in the following sections. With the LiDAR-inertial measurements during $\left[t_{\kappa-1},t_{\kappa}\right)$ and tracked visual features in the keyframe sliding window (detailed in Sec. V-B2), we formulate the following Nonlinear Least-Squares (NLS) problem:

and solve the NLS problem by iterative optimization methods. At each iteration, the system is linearized at current estimate $\hat{\mathbf{x}}$, and we define its error state as $\tilde{\mathbf{x}}=\mathbf{x}-\hat{\mathbf{x}}$, where $\mathbf{x}$ is the true state. In practice we adopt the Levenberg-Marquardt algorithm from the Ceres Solver [44] library and employ analytical derivatives to speed up the NLS problem-solving.

A LiDAR point measurement ${}^{L}\mathbf{p}_{t}$ with noise $\mathbf{n}_{L}$, measured at time $t_{\ell}$, is associated with a 3D plane in closest point parameterization [14, 45], ${}^{G}\bm{\pi}={}^{G}d_{\pi}{}^{G}\mathbf{n}_{\pi}$, where ${}^{G}d_{\pi}$ and ${}^{G}\mathbf{n}_{\pi}$ denote the distance of the plane to origin and unit normal vector, respectively. We can transform LiDAR point to global frame by

and the point-to-plane distance is given by:

where $\tau_{\ell}=t_{\ell}$ is the measure time of LiDAR point, $\mathbf{H}_{\ell}$ is Jacobian matrix w.r.t error state $\tilde{\mathbf{x}}$ (see our supplementary file for detail). $\mathbf{n}_{L}$ is assumed to be under independent and identically distributed (i.i.d.) white Gaussian noise in our experiments. $\mathbf{G}_{\ell}$ is the Jacobian with respect to $\mathbf{n}_{L}$ which can be easily computed.

Considering raw IMU measurements at $t_{m}$ with angular velocity ${}^{I}\bm{\omega}_{m}$ and linear acceleration ${}^{I}\mathbf{a}_{m}$, and the true angular velocity and linear acceleration are denoted by ${}^{I}\bm{\omega}$ and ${}^{I}\mathbf{a}$, respectively. The following equations hold:

where $\mathbf{n}_{\omega},\,\,\mathbf{n}_{a}$ are zero-mean Gaussian white noise. The gyroscope bias $\mathbf{b}_{\omega}$ and accelerometer bias $\mathbf{b}_{a}$ are modeled as random walks, driving by the white Gaussian noises $\mathbf{n}_{b_{\omega}}$ and $\mathbf{n}_{b_{a}}$, respectively. With the timeoffset $t_{I}$ of IMU between LiDAR, we have the following IMU factor

and bias factor

where $\tau_{m}=t_{m}+t_{I}$ is the corrected IMU timestamp, and $\mathbf{H}_{I_{m}}^{\kappa},\mathbf{H}_{I_{b}}^{\kappa}$ are Jacobian matrices with respect to states (see supplementary file), and $\mathbf{G}_{I_{m}},\mathbf{G}_{I_{b}}$ are Jacobian matrices with respect to noise. By substituting the derivative of continuous-time trajectory (Eq. (9)) at time instant $\tau_{m}$ into Eq. (LABEL:eq:imu_factor), we can optimize the continuous-time trajectory by raw IMU measurements directly, avoiding the efforts of IMU propagation or pre-integration.

A landmark $\mathbf{p}_{\jmath}$ observed in its anchor keyframe $\mathcal{F}_{a}$ at timestamp $t_{a}$ and observed again in frame $\mathcal{F}_{b}$ at timestamp $t_{b}$, can be given the initialized inverse depth as $\lambda_{\jmath}$ through triangulation (as Sec. V-B1). Let $\bm{\rho}_{\jmath}^{a}$ denotes 2D raw observation in $\mathcal{F}_{a}$ with noise $\mathbf{n}_{c}$, the estimated position of landmark in frame $\mathcal{F}_{b}$ is

where $\pi_{c}(\cdot)$ denotes the back projection which transforms a pixel to the normalized image plane. $\tau_{a},\tau_{b}$ are corrected timestamps to remove timeoffsets. The corresponding visual factor based on reprojection error is defined as:

where $\mathbf{e}_{i}$ denotes a $3\times 1$ vector with its $i$-th element to be 1 and the others to be 0, and $\bm{\rho}_{\jmath}^{b}$ describes 2D raw observation in $\mathcal{F}_{b}$. $\mathbf{H}_{\jmath}^{b}$ denotes Jacobian matrix (see supplementary file).

To bound the size of sliding window in our continuous-time fixed-lag smoother, marginalization has to be resorted to. Although marginalization is universally leveraged in discrete-time sliding-window estimator [23, 3], it is rarely investigated in continuous-time sliding-window estimator. By utilizing probabilistic marginalization, information on the active states can be well reserved in the estimator, when measurements and old states are removed from the sliding window. Linearizing the factors at the best estimate of the state gives:

where we organize the reserved parameters in $\mathbf{x}_{\alpha}$, and $\mathbf{x}_{\beta}$ will be marginalized. Using the Schur-Complement [46], the following equation holds:

and by introducing new notations, we can denote the above equation by:

which is exactly the prior factor. Figure. 2 (b) displays the entries needed to be marginalized out of the LI temporal- and visual keyframe-sliding windows. The entries needed to be marginalized vary at different statuses. We marginalize out the control points and IMU biases when sliding the LI temporal window, and marginalize out the inverse depths of landmarks when sliding the oldest keyframe out of visual keyframe sliding window, producing a prior on

where $\bm{\Phi}(t_{\kappa-1},t_{\kappa})\cap\bm{\Phi}(t_{\kappa},t_{\kappa+1})$ denotes the affected control points in next sliding window. The prior factor induced from marginalization is shown in Fig. 2 (c) and will be involved in future factor-graph optimization.

The main purpose of incorporating camera to the multi-sensor fusion estimator is to improve the robustness of LI system. We expect the LiDAR-Inertial-Camera system not to have degraded performance in structureless scenarios, which is a significant challenge for LI systems.

We evaluate the following three publicly-available datasets and our self-collected datasets:

• •

  VIRAL [52]: The Visual-Inertial-RAnging-Lidar Dataset contains a variety of sensors, including two 16-beam Ouster LiDARs at 10Hz, two monocular cameras at 10Hz, and a VectorNav VN100 IMU at 385Hz, etc. The dataset comprises nine sequences collected indoors and outdoors by a MAV (Micro Aerial Vehicle).

• •

  NCD [53]: The Newer College Dataset is collected using a handheld device that consists of a 64-beam Ouster LiDAR at 10Hz with its internal IMU at 100Hz, and a stereo camera at 30Hz. The dataset combines built environments, open spaces and vegetated areas.

• •

  LVI-SAM [6]: The LVI-SAM Dataset features both handheld and vehicle (Jackal) platforms in outdoor open vegetated environments including geometrically degenerate surroundings with 16-beam LiDAR at 10Hz, camera at 20Hz, and IMU at 500Hz.

• •

  YQ: The self-collected YuQuan Dataset collected on our university campus consists of seven sequences using an electric car with a sensor rig mounted on the top shown in Fig. 6. The sensor rig comprises a 16-beam LiDAR at 10Hz, a camera at 20Hz, and an IMU at 400Hz. The trajectories of different sequences overlaid on Google map are shown in Fig. 7. GPS measurements are collected to provide groundtruth for this outdoor dataset.

• •

  Vicon Room: The self-collected Vicon Room Dataset shares the identical sensor rig as YQ dataset. This indoor dataset is collected with hand-held random motion, and a motion capture system is leveraged to provide groundtruth trajectories.

In this experiment, we evaluate the accuracy of continuous-time trajectory estimation of the CLIO system enabled by the proposed continuous-time fixed-lag smoothing. The results on VIRAL dataset are shown in Tab. III, CLIO outperforms other LI methods in most sequences and achieves an average RMSE of 0.034m. CLINS is much more accurate than all the discrete-time methods, including LIO-SAM [50], MILIOM [37], and VIRAL [51]. Compared to continuous-time method, CLINS [17], which only optimizes control points of trajectory within the time span of the newest LiDAR scan and lacks probabilistic marginalization, the proposed CLIO, optimizing all the control points in a sliding window involved with several LiDAR scans (see Fig. 4 and Fig. 8) and benefiting from marginalization, can achieve higher accuracy.

We further conduct evaluations in large-scale scenarios on the LVI-SAM dataset and NCD dataset (Tab. V and IV), CLIO achieves higher accuracy on most sequences compared to LIO-SAM. Especially, it is worth noting that on the NCD_06 sequence with the handheld sensor shaking vigorously, CLIO shows significant superiority over the discrete-time LIO-SAM. The improvement in accuracy could be attributed to our continuous-time trajectory formulation as it adequately addresses the motion distortion of LiDAR point cloud, which is of significant importance in highly-dynamic motion scenarios.

The presented continuous-time fixed-lag smoothing method supports fusion of multiple sensors conveniently. In Tab III, We also showcase the pose estimation accuracy of a system with the fusion of IMU and two LiDARs, dubbed CLIO2. We can see that CLIO2 has on-par accuracy with CLIO on most of the sequences, and shows more accurate pose estimation over the outdoor sequences (nya_xx).

In this section, we discuss the accuracy of the LiDAR-Inertial-Camera pose estimation system enabled by continuous-time fixed-lag smoothing. The experimental results on the VIRAL and LVI datasets are summarized in Tab. III and Tab. V. In addition, the evaluation results on our self-collected indoor and outdoor datasets are shown in Tab. VI and VII. CLIC tightly fuses LiDAR, IMU and camera measurements, and successfully generates good pose estimation in Handheld sequences collected over large open areas while CLIO fails, achieving significantly higher accuracy compared to the discrete-time method LVI-SAM [6]. It indicates that visual factors fused into CLIO can help improve the system’s applicability. Figure. 10 showcases a snapshot on the Handheld sequence where the LiDAR could only detect the ground while camera can track stable visual features to further constrain the optimization problem. As discussed in Sec. V-B2, we only optimize the control points within the LI temporal sliding window for computation efficiency reasons, and the other control points involved in visual keyframe sliding window are kept fixed during optimization, which might degrade the effects of visual factors. In Tab. VI and  VII, LIC-Fusion 2.0 [10] shows pleasing performance in both indoor and outdoor scenarios, which benefits from reliable sliding-window plane-feature tracking. However, we can see that LIC-Fusion 2.0 has degraded performance in outdoor scenarios (see Tab. VI). The possible reason is that LIC-Fusion 2.0 relies on stably-tracked plane features to update LiDAR poses, while the outdoor scenarios are filled with tree clumps, and can fail to provide sufficient structural planes compared to indoors.

The accuracy of CLIC can be further improved when enabling online timeoffset calibration between different sensors (see Tab. V). In Fig. 9, we also show some representative estimated trajectories of different methods aligned with the ground truth trajectories.

In this section, we examine the performance of online temporal calibration in the proposed multi-sensor fusion systems. Experiments are conducted on the NCD_01 sequence of NCD dataset [53]. We manually add additional timeoffsets of $-20\sim 20$ ms to the raw timestamps, and the timeoffest $t_{I}$ provided from NCD dataset is 0ms. Fig. 11 shows the error of estimated timeoffset over time. We start to estimate the timeoffset 5 seconds after the start of the system, and the IMU timeoffset converges quickly, with most of the trials converging within 3 seconds. In addition, the final estimated timeoffset mean(std) is -2.0ms(2.7ms).

We investigate runtime of the main modules in CLIO and CLIC in eee_01 sequence of VIRAL dataset [51]. Importantly, we notice the average runtime of proposed method remains almost constant whether in a long sequence or a short sequence. The method is implemented in C++ and executed on the desktop PC with an Intel i7-7700K and 32GB RAM, and Tab. VIII summarizes the time consumption of CLINS [17], CLIO and CLIC. The term Update Local Map in Tab. VIII represents update local LiDAR map for associating LiDAR feature, Update Trajectory represents solving the problem in Eq. 12 and Update Trajectory represents the marginalization process. The term Others includes feature extraction of image and LiDAR cloud, trajectory initialization, data association, et al. With analytical Jacobian computations for optimization and using sliding window and marginalization to bound computation complexity by the continuous-time fixed-lag smoothing, the enabled multi-sensor fusion methods (CLIO and CLIC) achieve real-time capability. In contrast, the continuous-time method, CLINS [17], consumes much more time and fails to run in real time. Here, real-time performance refers to the total elapsed time to process measurements from the sensors is less than the sensor data collection time. We also note that our current implementation is not optimal, and there is still significant space to further improve efficiency.

We follow the notation in [15] to give the Jacobians of the trajectory value to the control points:

for $j>0$ and for $j=0$ we obtain

where $\mathbf{f}$ is the queried pose or velocity. Note that we follow the recursive scheme in [15] to efficient compute Jacobians of queried pose $\{\frac{\partial{}^{G}_{I}\mathbf{R}(\tau_{\ell})}{\partial\tilde{\mathbf{x}}_{R}^{\kappa}},\frac{\partial{}^{G}\mathbf{p}_{I}(\tau_{\ell})}{\partial\tilde{\mathbf{x}}_{p}^{\kappa}}\}$, angular velocity $\frac{\partial{}^{I}\bm{\omega}(\tau_{m})}{\partial\tilde{\mathbf{x}}_{R}^{\kappa}}$ and linear acceleration $\frac{\partial{}^{G}\mathbf{a}(\tau_{m})}{\partial\tilde{\mathbf{x}}_{R}^{\kappa}}$.

Given the IMU factor defined at Eq. (18), the Jacobians w.r.t error state is

where

and

Given the Bias factor defined at Eq. (19), the Jacobians w.r.t error state is

where

Given the LiDAR factor defined at Eq. (14), the Jacobians w.r.t error state is

where

and

The visual factor (Eq. (21)) is related to two poses at time instants, $\tau_{a}$ and $\tau_{b}$, and the Jacobians w.r.t error state is

where

and