LIO-GVM: an Accurate, Tightly-Coupled Lidar-Inertial Odometry with Gaussian Voxel Map

The important notations used in this letter are listed in Tab. I. Besides, we assume a static calibrated extrinsic matrix ${}^{L}\mathbf{T}_{I}=({}^{L}\mathbf{R}_{I},{}^{L}\mathbf{p}_{I})$ from IMU frame to LiDAR frame.

The proposed LIO-GVM is modeled as a discrete-time dynamical system sampled at the IMU rate. Its state $\mathbf{x}$ is defined on manifold $\mathcal{M}$ as:

where ${}^{G}\mathbf{v}_{I}$ and ${}^{G}\mathbf{g}$ are the velocity of IMU and gravity vector with reference to the global frame, $\mathbf{b}_{\boldsymbol{\omega}}$ and $\mathbf{b}_{\mathbf{a}}$ are the bias vectors of gyroscope and accelerometer of the IMU. The true state $\mathbf{x}$ of the system can be divided into two states:

where $\delta\mathbf{x}_{k}\in\mathbb{R}^{18}$ is the error state, $\hat{\mathbf{x}}_{k}\in\mathcal{M}$ is the nominal state, $\boxminus$ and $\boxplus$ are encapsulated operators that represent a bijective mapping between the manifold $\mathcal{M}$ and its local tangent space $\mathbb{R}^{18}$ [23]; ${}^{G}\delta\mathbf{r}_{I_{k}}={}^{G}\mathbf{R}_{I_{k}}\boxminus{}^{G}\hat{\mathbf{R}}_{I}=Log({}^{G}\mathbf{R}_{I_{k}}{}^{G}\hat{\mathbf{R}}_{I_{k}}^{-1})$ is the minimal representation of the error rotation. In this way, we can use the iterative error state Kalman filter (IESKF) to estimate the error state $\delta\mathbf{x}_{k}$ on Euclidean space and then project it back to the manifold using (2) for the maintenance of the true state $\mathbf{x}_{k}$.

Recalling the state defined in (1), we have the following discrete model based on continuous kinematics equations [23]:

where $\mathbf{u}_{i}$, $\mathbf{w}_{i}$, and $\mathbf{f}$ are the system input, system noise, and kinematics function respectively. Please refer to Appendix -A for the detailed formulations. Firstly, the mean of the nominal state $\hat{\mathbf{x}}_{i}$ will be predicted without considering the noise $\mathbf{w}_{i}$:

Consequently, this will result in the accumulation of inaccuracies. To cope with the problem, $\mathbf{w}_{i}$ is taken into account in the prediction of the error state $\delta\mathbf{x}_{i}$:

where $\mathbf{F}_{\delta\mathbf{x}}$ and $\mathbf{F}_{\mathbf{w}}$ are the Jacobian matrices of $\mathbf{f}()$ w.r.t. $\delta\mathbf{x}_{i}$ and $\mathbf{w}_{i}$ [4]. In the implementation, the system noise $\mathbf{w}_{i}$ is considered as Gaussian white noise following the normal distribution $\mathcal{N}(\mathbf{0},\mathbf{W})$, where $\mathbf{W}$ is the covariance matrix. The error state $\delta\mathbf{x}_{i}$ is an independent random vector with multivariate normal distribution: $\delta\mathbf{x}_{i}\sim\mathcal{N}\left(\mathbf{0},\mathbf{P}_{i}\right)$. As a consequence, the predicted covariance matrix $\hat{\mathbf{P}}_{i+1}$ of $\delta\mathbf{x}_{i+1}$ can be calculated as the linear transformation:

The IMU process module utilizes the state estimate from the previous scan as the initial condition and recursively predicts $\hat{\mathbf{x}}_{i+1}$, $\delta\mathbf{x}_{i+1}$ and $\hat{\mathbf{P}}_{i+1}$ till the end time of the current LiDAR scan $t_{k}$.

All the LiDAR points ${}^{L_{j}}\mathbf{p}_{j}$ with timestamps $\gamma_{j}\in\left[t_{k-1},t_{k}\right]$ are cached to form a point cloud $\mathcal{P}_{k}$. For most commercial LiDAR, the points are sampled under different timestamps, which will introduce motion distortion to the cloud $\mathcal{P}_{k}$. We seek to compensate for the distortion by projecting all the points in $\mathcal{P}_{k}$ to $L_{k}$, the LiDAR frame at $t_{k}$. To this end, we adopt the backward propagation proposed in [3] to obtain the relative motion between IMU frames sampled at $\gamma_{j}$ and $t_{k}$: ${}^{I_{k}}\mathbf{T}_{I_{j}}$. The point ${}^{L_{j}}\mathbf{p}_{j}$ can thus be projected to $L_{k}$ as:

Moreover, a representative descriptor for the point is required for robust correspondence matching. Thus, we represent the deskewd points as a collection of Gaussian distributions. For each point ${}^{L_{k}}\mathbf{p}_{j}$, we collect its nearest $y$ neighbors and fit them to a Gaussian distribution $\mathcal{N}({}^{L_{k}}\boldsymbol{\mu}_{j},{}^{L_{k}}\mathbf{C}_{j})$ [17]. Eventually, the LiDAR process module outputs a collection of the fitted Gaussian distributions for all the points: $\{{}^{L_{k}}\boldsymbol{\mu}_{j},{}^{L_{k}}\mathbf{C}_{j}\}$.

Upon completion of the IMU and LiDAR processes, the following steps for the IESKF-based LIO system involve correspondence matching, residual formulation, and Kalman update. However, existing filter-based LIO systems [3, 6, 5] typically adopt the nearest correspondence matching strategy and treat the ensuing point-to-feature distance as the residual. This strategy involves projecting ${}^{L_{k}}\mathbf{p}_{j}$ to the global frame as:

For long-term odometry, the inevitable accumulation of localization drifts introduces errors to ${}^{G}\bar{\mathbf{T}}_{I_{k-1}}$. Simultaneously, the presence of spike noise, a common issue in low-cost IMUs, leads to inaccuracies in ${}^{I_{k-1}}\hat{\mathbf{T}}_{I_{k}}$. These factors prevent ${}^{L_{k}}\mathbf{p}_{j}$ from being projected to its exact corresponding feature, generating mismatches (see Fig. 3). Besides, matching the nearest feature in the map could be computationally intensive if the map size grows too large in long-term odometry. To cope with these issues, we propose a new residual formulation for IESKF, which can mitigate incorrectly matched correspondences while maintaining computational efficiency (see Fig. 3). The details are described in the following subsections.

The global map is composed of voxels stored as a hash table: $\mathbb{Z}^{3}\rightarrow\mathbb{R}^{3}\times\mathbb{R}^{3\times 3}$, which builds a map from the voxel coordinate ${}^{G}\mathbf{v}_{i}$ to its centroid ${}^{G}\boldsymbol{\mu}_{i}$ and the covariance matrix ${}^{G}\mathbf{C}_{i}$. This structure demonstrates significant efficiency in querying, achieving an optimal $\mathcal{O}(1)$ time complexity for correspondence matching. This structure is similar to that of VGICP [19]. In contrast to VGICP, which only supports pair-wise registration and requires repeated voxelization of the source scan, our approach offers a complete solution for insert, update, and delete operations within a global target map. We also address the variable LiDAR sampling rate problem (e.g. the ground voxels are undersampled in the distance and degrade from the ”plane” to ”line” feature) by introducing an update scheme to ensure the continuous and accurate maintenance of the map.

For the initialization, the first scan is regarded as the global frame $G=L_{0}$. Each point in the scan is subsequently fitted to a Gaussian distribution as the procedure in IV-B to obtain $\left({}^{G}\hat{\mathbf{p}}_{j},\mathbf{C}_{j}\right)$. Consequently, the scan is voxelized with voxel size $r$. Suppose we have the voxel with coordinate ${}^{G}\mathbf{v}_{i}$ encompassing a set of points $\{{}^{G}\mathbf{p}_{m}\}$, the centroid and covariance matrix ${}^{G}\boldsymbol{\mu}_{i},{}^{G}\mathbf{C}_{i}$ will be calculated as the average of the distributions within. Once the current estimate $\bar{\mathbf{x}}_{k}$ is acquired, the current scan contributes to the global map. Since the projected Gaussian distributions have been obtained during the state estimation phase, we proceed to voxelize the current scan following (18), resulting in a temporary voxel map ${}^{G}\mathbf{v}_{j}\rightarrow(\bar{\boldsymbol{\mu}}_{j},\bar{\mathbf{C}}_{j})$. For each voxel present in this temporary voxel map, we query its coordinate ${}^{G}\mathbf{v}_{j}$ in the global map. If the global map doesn’t contain the voxel, we directly insert the voxel into the global map. This operation is time-efficient since the insert operation for a hash table has a time complexity of $\mathcal{O}(1)$. Otherwise, the global map will be updated as:

where $M$ is the recorded point number of voxel ${}^{G}\mathbf{v}_{j}$ in the global map, $N$ is the corresponding one in the temporary voxel map. In this way, the global map only necessitates the maintenance of the centroid and covariance matrix for each voxel, which is considerably efficient in terms of memory usage and time efficiency compared to maintaining a tree structure storing all points [4] or embedding additional structures into the voxels [5, 16]. Specifically, $M$ is updated using the formula $M=max(M,N)$. This is motivated by the goal of maintaining a continuously updated voxel scheme. This approach ensures that $M$ is constrained by the maximum number of points that can be sampled by LiDAR within a voxel. Otherwise, recalling (18), an unbounded $M$ would lead to $M\gg N$ after a few scans, thus preventing the update of voxel parameters. Consequently, this enables an incremental update of the global map without the necessity to delete voxels from the map. Nevertheless, the mapping scheme also provides the option to remove voxels out of the effective range of the LiDAR for operation in exceptionally large environments.

We evaluate the accuracy by comparing the KITTI metric [28]: the average translation error (ATE), and the average rotation error (ARE). We compare LIO-GVM with three state-of-the-art LiDAR (-inertial) odometry algorithms: DLO [29], VoxelMap [12], LIO-SAM [1], and FAST-LIO2 [4]. DLO is a GICP-based LO, VoxelMap is a probabilistic voxel-based LO while LIO-SAM and FAST-LIO2 are LIO systems. Parameters for all methods are kept constant across sequences for consistency. For LIO-GVM, the voxel size, $s_{t}$, and $\alpha$ are empirically set as 1.0 $m$, 0.70 and $10^{-6}$, respectively. Additionally, the neighbor searching scheme is set as 7-neighbors searching.

Two variations of the proposed system denoted as LIO-GVM(w/o s) and LIO-GVM(plane) are tested for ablation study. LIO-GVM(w/o s) is implemented without the outlier rejecting scheme proposed in Sec. IV-C1. LIO-GVM(plane) replaces the proposed new residual metric with the point-to-plane distance [4]. As shown in Tab. II, both LIO-GVM(w/o s) and LIO-GVM(plane) turn worse compared to the original system for all the sequences, indicating that the proposed correspondence matching scheme and residual metric both contribute to the accuracy of the system. The accuracy of LIO-GVM(plane) tends to be much worse than LIO-GVM(w/o s), which is because the residual metric inherently reduces the weights for mismatches. The drastic accuracy degradation of LIO-GVM(plane) is because, without the proposed residual metric, all the adopted correspondences contribute equally to the estimation, even those with low similarity.

As illustrated in Tab. II, LIO-GVM achieves the best results for most sequences due to its ability to reject mismatched distributions (the proposed correspondence matching) and evaluate the variance divergence in the distribution covariance (the proposed residual metric). For ntu_* sequences, FAST-LIO2 achieves comparable results with LIO-GVM, because the accurate IMU integration can approximately project source points to the correct feature in the target map. VoxelMap offers accurate pose estimation in some sequences but frequently diverges, especially for the nc_* sequences equipped with a 64-line LiDAR. This divergence is attributed to its map update scheme. When the LiDAR remains stationary for several scans, distant voxels converge prematurely and stay undersampled. This issue is particularly obvious for ground voxels, leading to the majority of divergence occurrences along the z-axis. Conversely, LIO-GVM employs a continuously updated mapping scheme, ensuring that the performance remains stable for motion and LiDAR sampling rate variations.

The performance of all LIO systems is negatively impacted in ntuo_* compared to ntu_* sequences due to the equipment of a low-cost IMU. Nevertheless, LIO-GVM is able to mitigate the impact of IMU noises through its proposed correspondence matching scheme, which can reject outliers. Additionally, LIO-GVM incorporates a new residual metric that takes similarity into account during the estimation phase, allowing more effective correspondences to have a greater influence on the estimation. Consequently, LIO-GVM exhibits the best accuracy compared to other systems. However, all the methods have a common limitation in accommodating the pronounced changes in elevation in the ntu_* sequences, as seen in Fig. 5 and Fig. 6.

Overall, LIO-GVM has demonstrated the best performance regarding odometry accuracy, showing the effectiveness of the error-rejecting correspondence matching scheme and the distribution-based residual metric in achieving accurate pose estimation across various sensor setups, especially in long-term applications or with cheaper IMUs.

In this section, we evaluate the storage consumption of the global map generated by LIO-GVM. Specifically, in LIO-GVM, we have defined a custom point type using the Point Cloud Library (PCL). Each point encapsulates all the elements of $(\boldsymbol{\mu}_{i},\mathbf{C}_{i},M)$, comprising a total of four integer values and nine floating-point values. All the voxels are stored in a compressed binary .pcd file. For other systems, we configure their local map size to be 500m, ensuring coverage of all areas within the two datasets, and save all the points in the map to a compressed binary .pcd file.

As illustrated in Tab. IV, the map storage consumption of LIO-GVM outperforms other systems for all the sequences. This is because other systems need to ensure that the downsampling filter of the map is not excessively large (usually $<$ 0.5m) to preserve the geometric information of the points. In contrast, our approach allows us to allocate a considerably larger voxel size (commonly $>$ 1m) compared to other systems while still offering a comprehensive representation of the intra-voxel point distribution.

In this section, we will evaluate the temporal efficiency of LIO-GVM, utilizing the same parameters as those deployed in our previous odometry evaluations for consistency. We first analyze the influence of the data structure on the mapping procedure, which involves correspondence matching and incremental updates. Given the superior performance of the ikd-Tree provided in [4] compared to other dynamic tree-based structures, our comparison will be limited to this structure. While the Gaussian fitting process does not directly participate in the mapping procedure, its time consumption is still accounted for in LIO-GVM for a fair comparison. We denote the data structure in LIO-GVM for the mapping procedure as GVM. The results are illustrated in Tab. III. GVM significantly outperforms ikd-Tree in correspondence matching because the time complexity of $k$-nearest neighbors searching for ikd-Tree is $\mathcal{O}(log(n))$, where $n$ is the tree size; but for GVM, managed in a hash table, the corresponding time complexity is $\mathcal{O}(1)$. ikd-Tree slightly performs better in the incremental update. Nonetheless, GVM demonstrates superior performance in total map time.

In Fig. 7, we illustrate the average total time consumed per scan for all the sequences. The total time we evaluated includes all the procedures: data processing, pose estimation, and map update. Since LIO-SAM and DLO run map update in different thread with the pose estimation thread, we only evaluate their time consumed for state estimation. Notably, VoxelMap consumes an average of 141.3 ms per scan due to its demand for dense point cloud data. Even though the mapping procedure of LIO-GVM is faster than FAST-LIO2, FAST-LIO2 achieves the shortest total time for almost all the sequences. This is because LIO-GVM requires more geometric information from the input scan. For example, in ntu_* sequences, FAST-LIO2 employs fewer feature points than LIO-GVM (4638 versus 5741). Consequently, LIO-GVM spends more time on pose estimation. However, as illustrated in Tab. 8, the fast performance of FAST-LIO2 and DLO comes with the trade-off of increased runtime RAM usage, while LIO-GVM can achieve comparable temporal efficiency with lower RAM usage.

Please find details in the supplemental material: supplemental_material.pdf.

Let’s denote $\left({}^{G}\hat{\boldsymbol{\mu}}_{j}^{n}-{}^{G}\boldsymbol{\mu}_{i}\right)$ as ${}^{G}\tilde{\boldsymbol{\mu}}$, then equate (15) with the second term in MAP problem, we’ll have:

To avoid the singularity issue, we first perform eigendecomposition on $(\hat{\mathbf{C}}_{j}^{n}+\mathbf{C}_{i}+\alpha\mathbf{I})^{-1}$ to have $(\hat{\mathbf{C}}_{j}^{n}+\mathbf{C}_{i}+\alpha\mathbf{I})^{-1}=\left(\mathbf{U}_{j}^{n}\right)^{T}\left(\boldsymbol{\Lambda}_{j}^{n}\right)^{-1}\mathbf{U}_{j}^{n}$. Subsequently, the eigenvalues are normalized and clamped at $10^{-4}$: $\lambda_{t,j}^{n}=max(\lambda_{t,j}^{n}/trace({\Lambda}_{j}^{n}),10^{-4}),t=1,2,3$. In order to obtain the same formula as the left-hand side, we perform matrix decomposition to have $\left({\Lambda}_{j}^{n}\right)^{-1}=\left(\mathbf{Q}_{j}^{n}\right)^{T}\mathbf{V}_{k}^{-1}\mathbf{Q}_{j}^{n}$, which is quite easy since $\left({\Lambda}_{j}^{n}\right)^{-1}$ is a diagonal matrix. Thus,

where: