# GIF-LIO: Geometry-Intensity Fused LiDAR–Inertial Odometry With Degeneracy-Constrained State Estimation
Hekai Yao $^{ID}$, Xuetao Zhang $^{ID}$, Member, IEEE, Gang Sun $^{ID}$, Graduate Student Member, IEEE, Yisha Liu $^{ID}$, Member, IEEE, and Yan Zhuang $^{ID}$, Senior Member, IEEE
Abstract—Robust and accurate LiDAR odometry is crucial for enabling reliable navigation and mapping for autonomous robots in perceptually challenging environments, such as long corridors, tunnels, and open squares. This article proposes a novel geometry-intensity fused LiDAR-inertial odometry (LIO) framework that enables accurate localization in geometrically degenerate environments. Different from the existing methods, the key insight of the proposed method is that the redundancy-degeneracy-aware geometric measurements are fused with complementary intensity measurements to construct degeneracy-optimization constraints. This ensures that the system state is updated along the higher information directions, thus enhancing localization accuracy and system robustness. In particular, the novel geometric measurement model is proposed to minimize redundancy in scan-to-map matches and indicate nondegenerate directions for state optimization. In addition, the probabilistic adaptive voxel map is leveraged for more accurate geometric measurements and degeneracy detection. When the above geometric measurements and the complementary intensity measurements are both available, the joint update of the constrained iterative extended Kalman filter (CIEKF) is performed to obtain the state estimation. Comparative experiments on public datasets are conducted to demonstrate the superior performance of the proposed odometry in terms of localization accuracy and map consistency, particularly in degenerate environments, such as long tunnels and flat fields.
Index Terms—Constrained state estimation, geometry-intensity fusion, LiDAR-inertial odometry (LIO).
### I. INTRODUCTION
IDAR-INERTIAL odometry (LIO) serves as a core technique for navigation and localization in contemporary robotics applications [^1], [^2], [^3], [^4], [^5], [^6], [^7]. Nevertheless, these systems exhibit critical vulnerabilities in geometrically degenerate environments characterized by structural self-similarity or geometric feature sparsity, such as long tunnels or spacious terrains. The degenerate geometric measurements in these environments will induce cumulative state estimation drift or complete localization failure. Therefore, designing a more robust LIO for these environments is crucial.
The existing degeneracy-robust LIO systems usually inherit the following two categories: 1) geometric degeneracy-aware optimization [^8], [^9], [^10], [^11], which can detect geometric degeneracy and construct degeneracy-optimization constraints of state-update with uninformative directions and 2) LiDAR intensity-based information augmentation [^12], [^13], [^14], where the LiDAR intensity measurement model can provide complementary constraints for robust state optimization. However, given a challenging degenerate environment, there are still some problems listed as follows.
1) The intensity-augmented LIO systems [^12], [^13], [^14] are usually robust, which can integrate complementary LiDAR intensity information to mitigate the potential degeneracy in geometric measurements. However, these methods typically suffer from inaccurate geometric measurements, which limit the quality of geometric degeneracy detection. Such limitations deteriorate the intensity information efficacy, ultimately reducing both system robustness and localization accuracy.
2) For geometric degeneracy-aware optimization methods, the key step is to acquire geometric measurements and construct degeneracy-optimization constraints in state estimation. However, the geometric measurements in current systems may contain redundant and inaccurate measurements, diminishing the effectiveness of the constraints. At the same time, the constructed optimization constraints cannot be seamlessly applied to iterative extended Kalman filter (IEKF) odometry frameworks.
Motivated by the issues above, we propose a novel geometry-intensity fused LIO framework that enables robust and accurate localization in geometrically degenerate environments. Notably, the novel geometric measurement model from the information refinement module is proposed to minimize redundancy and inaccuracy in scan-to-map registration, which also indicates nondegenerate directions for state estimation. Meanwhile, the information augmentation module can select LiDAR intensity image features along degenerate directions to construct complementary intensity photometric measurements.
---
Besides, the probabilistic adaptive voxel mapping framework is leveraged to enable uncertainty-aware environmental representation for more precise scan-to-map geometric measurements and degeneracy detection. The main contributions are concluded as follows.
1) A novel geometry-intensity fused LIO framework is proposed for accurate and robust localization in degenerate environments. In particular, the information refinement module can provide nonredundant geometric measurements and construct optimization constraints. The information augmentation module can provide complementary intensity photometric measurements in degenerate directions. In addition, a probabilistic voxel map is leveraged to improve the accuracy of geometric measurements and degeneracy detection.
2) A redundancy-degeneracy-aware geometric measurement model is proposed to minimize redundancy in scan-to-map registration and construct optimization constraints. In particular, with the pruned LiDAR points and degenerate directions, we design a Tikhonov regularization term for scan-to-map registration residual, and the Hessian matrix of this term is leveraged to indicate nondegenerate directions in the joint update of constrained iterative extended Kalman filter (CIEKF).
3) The proposed system is evaluated on public datasets, which significantly outperforms the state-of-the-art methods. In addition, the ablation experiments on the Enwide dataset prove that the information refinement module can improve localization accuracy in degenerate environments.
The remainder of this article is organized as follows. In Section II, related work is introduced. The proposed framework and methodology are detailed in Sections III and IV, respectively. Section V presents experiment results and analysis. Section VI concludes this article.
### II. RELATED WORK
Current LIO methods can be broadly categorized into two main routes: deep learning-based approaches and traditional approaches. Using the data-driven paradigm, learning-based methods can enhance robustness in degenerate environments, especially for feature extraction and fusion. However, these methods typically involve heavy neural networks, leading to high computational complexity and memory consumption. As a result, they usually fail to satisfy the strict real-time constraints required in industrial and robotic navigation applications [^15], [^16]. Therefore, current methods still heavily rely on hand-crafted geometric features and constraints. Accordingly, our study continues to follow the traditional technical route.
### A. LiDAR–Intertial Odometry
Point cloud registration is a key step for LIO systems. Feature-based approaches are classical algorithms, such as LOAM [^17], extracting edges and planes in the point cloud for registration, and LEGO-LOAM [^18] improving LOAM by introducing ground segmentation. However, pose estimation based solely on geometric feature extraction and matching is prone to failure in challenging environments or during aggressive motion.
Incorporating inertial measurement units (IMUs) data into LiDAR-based odometry can enhance localization robustness and accuracy. LIO-SAM [^19] is a popular optimization-based method, which optimizes system state in a factor graph with LiDAR odometry factor and IMU preintegration factor [^20]. However, such methods are time-consuming. Therefore, filter-based methods have become more popular. FAST-LIO2 [^1] reformulates the Kalman gain and utilizes raw LiDAR points for registration, achieving state-of-the-art localization accuracy and real-time performance. Similarly, Point-LIO [^21] proposes a novel point-by-point LIO framework to increase the odometry output rate and improve robustness in aggressive motion. For more accurate point cloud registration, VoxelMAP [^22] designs a novel probabilistic voxel map for environment presentation. Although robustness of these methods is enhanced through fusion with IMU data, LiDAR-inertial systems continue to experience drifts or failures due to reliance on LiDAR measurements, which exhibit inherent geometric degeneracy.
### B. Degeneracy-Robust Odometry
To address the degeneracy issues for LiDAR-based odometry systems, many approaches have been proposed. The integration of visual and other sensor data can enhance the robustness of SLAM systems [^23], [^24], [^25], [^26]. However, these methods rely on additional sensor configurations and increase system complexity.
For LIO systems, some methods demonstrate improved robustness with a geometric degeneracy-aware optimization method or LiDAR intensity-based information augmentation method. Degeneracy detection and constraints construction are essential steps [^11] for geometric degeneracy-aware optimization. Zhang et al. [^8] propose a degeneracy factor and the solution remapping method to constrain state optimization. Based on this, Turcan et al. [^9] propose X-ICP to detect degeneracy with the correspondence information between source and target point cloud, which also adds hard constraints along the degenerate directions to reduce the pose estimation error. However, imprecise geometric registration limits the effectiveness of degeneracy detection and constraint construction. In addition, in some extremely degenerate conditions, constraints derived only from geometric information are not enough. Therefore, fusion of LiDAR intensity information [^12], [^13], [^14] is a viable method. However, these methods typically suffer from inaccurate environment representation and geometric measurements, which limit the quality of geometric degeneracy detection. Such limitations deteriorate the intensity information efficacy, ultimately reducing both system robustness and localization accuracy.
Although the intensity information fusion strategy in CoinLIO [^14] and the update mechanism of probabilistic voxel map [^22] introduce additional computational overhead, they can improve localization accuracy in degenerate environments while guaranteeing real-time performance. Motivated by this, a novel geometry-intensity fused LIO framework is proposed, which enables accurate localization in geometrically degenerate environments and preserves real-time performance. The
---
redundancy-degeneracy-aware geometric measurements and complementary intensity measurements are fused to construct degeneracy-optimization constraints. In addition, the probabilistic adaptive voxel map is leveraged for more accurate geometric measurements and degeneracy detection.
### III. SYSTEM OVERVIEW
Fig. 1 presents the framework of the proposed geometry-intensity fused LIO, which aims to enhance robustness and localization accuracy in geometrically degenerate environments. The system adopts the preprocessing module and tightly-coupled iEKF presented in the state-of-the-art LIO [^1] and extends it with some core modules, with the full system comprising the following.
1) Preprocessing Module: Processes raw IMU measurements and LiDAR scans to predict prior states using previous state estimation, while compensating motion distortion to generate undistorted point clouds.
2) Information Refinement Module: Selects maximally informative LiDAR points for scan-to-map registration by maximizing the entropy of the gradient flow [^25], then calculates the geometric residuals, Jacobian matrix, and detects the degenerate directions. On this basis, calculate the Tikhonov regularization term and construct optimization constraints.
3) Information Augmentation Module: Generates intensity images and extracts image features [^14] along geometrically uninformative directions to construct complementary photometric measurements.
4) Iterated Extended Kalman Filter Module: Utilizes optimization constraints to jointly optimize geometric and photometric measurements to update system state.
5) Probabilistic Voxel Mapping Module: Implements probabilistic adaptive voxel mapping [^20] to provide plane features for accurate scan-to-map geometric measurements and degeneracy detection.
### IV. DEGENERACY-CONSTRAINTED STATE ESTIMATION
The section adopts the same iEKF backbone from FAST-LIO2 [^27] and augments its measurement model with photometric residuals from LiDAR intensity images. However, the LiDAR–IMU extrinsic parameters (arm-length) in our system are assumed known, so it not estimated online. First, we introduce some notations used in this article. G, I, and L represent the Global, IMU, and LiDAR frames respectively, the global frame G is the initial pose of the IMU and the static calibrated extrinsic matrix $^L\mathbf{T}_I = (^L\mathbf{R}_I, ^L\mathbf{p}_I) \in SE(3)$ from IMU frame to LiDAR frame is known. Taking the IMU frame as the body frame of reference leads to a continuous kinematic model
$$ \begin{aligned}{{}^{G}\dot{\mathbf{p}}_{I}}&{{}={}^{G}\mathbf{v}_{I},\quad{}^{G}\dot{\mathbf{v}}_{I}={}^{G}\mathbf{R}_{I}\left(\mathbf{a}_{m}-\mathbf{b}_{\mathbf{a}}-\mathbf{n}_{\mathbf{a}}\right)+{}^{G}\mathbf{g}}\\ {{}^{G}\dot{\mathbf{g}}}&{{}=\mathbf{0},\quad{}^{G}\dot{\mathbf{R}}_{I}={}^{G}\mathbf{R}_{I}\lfloor\omega_{m}-\mathbf{b}_{\omega}-\mathbf{n}_{\omega}\rfloor_{\wedge}}\\ \end{aligned} $$
$$ \dot{\mathbf{b}}_{\omega}=\mathbf{n}_{\mathbf{b}\omega},\quad\dot{\mathbf{b}}_{\mathrm{a}}=\mathbf{n}_{\mathbf{b a}} $$
where the notation $[\mathbf{a}]_\wedge$ denotes the skew-symmetric matrix of vector $\mathbf{a} \in \mathbb{R}^3$ is defined on $\mathcal{M} = SO(3) \times \mathbb{R}^{15}$ as $\mathbf{x} \doteq [^G\mathbf{R}_I^\top^G\mathbf{p}_I^\top^G\mathbf{v}_I^\top^G\mathbf{b}_\omega^\top^G\mathbf{g}^\top_I^\top]$, where $^G\mathbf{R}_I$ and $^G\mathbf{p}_I$ denote the rotation matrix and translation vector of the IMU frame with respect to the global frame, $^G\mathbf{v}_I$ and $^G\mathbf{g}$ are the velocity of IMU and the gravity in global frame, and $\mathbf{b}_a$ and $\mathbf{b}_\omega$ are the accelerometer and gyro biases. With this continuous kinematic model and the system noises. The continuous process noise covariance matrix $\mathbf{Q}_c$ is
$$ Q_{c}=\mathrm{diag}\left(\sigma_{a}^{2}I_{3},\sigma_{\omega}^{2}I_{3},\sigma_{ba}^{2}I_{3},\sigma_{bg}^{2}I_{3}\right) $$
where $\sigma_a$ is accelerometer white noise density and $\sigma_\omega$ gyroscope measurement white noise density, and $\sigma_{ba}$ and $\sigma_{bg}$ are bias random-walk spectral densities. These values are reset based on the LiDAR characteristics. In the implementation, the continuous process noise covariance matrix $\mathbf{Q}_c$ needs to be discretized into the discrete noise covariance under the sampling time $\Delta t$. For example, if $\sigma_a$ is accelerometer white noise density, then per-sample std $=\sigma_a\sqrt{\Delta t}$, covariance $=\sigma_a^2\Delta t$.
For initialization, our system employs static initialization. The initial rotation is set to the identity matrix, while the initial position, velocity, and direction are initialized to zero. The gravity, gyroscope, and accelerometer biases are estimated from the first few frames of IMU data. For the initial state covariance matrix, relatively small variances are assigned to known or manually specified initial states, whereas larger variances are assigned to uncertain quantities.
### A. Preprocessing
This module possesses the capabilities of forward prediction and point cloud distortion correction. The predicted state $\hat{x}$ and covariance matrix $\hat{P}$ at the end time of the current LiDAR scan are calculated by recursively predicting the state using IMU data.
For LiDAR scan, since each point is sampled at different timestamps, motion distortion is unavoidable. Our system compensates for the motion of each LiDAR point from its individual sampling time to the end time of the scan using a backward propagation [^1] approach. The discrete kinematic model utilized in our system is defined as follows:
$$ \mathbf{x}_{i+1}=\mathbf{x}_{i}\boxplus(\Delta t\mathbf{f}(\mathbf{x}_{i},\mathbf{u}_{i},\mathbf{w}_{i})) $$
where $\boxplus$ is the encapsulation “plus” operator on the manifold. $\Delta t$ is the IMU sampling period, i is the index of the IMU measurements, the input vector $\mathbf{u}$, the system noises $\mathbf{w}$, and the function $\mathbf{f}$ are defined as follows:
$$ \mathbf{w}\doteq\left[\begin{matrix}{\mathbf{n}_{\omega}^{T}}&{\mathbf{n}_{\mathrm{a}}^{T}}&{\mathbf{n}_{\mathrm{b}\omega}^{T}}&{\mathbf{n}_{\mathrm{b a}}^{T}}\\ \end{matrix}\right]^{T},\quad\mathbf{u}\doteq\left[\begin{matrix}{\omega_{m}^{T}}&{\mathbf{a}_{m}^{T}}\\ \end{matrix}\right]^{T} $$
$$ \mathbf{f}\left(\mathbf{x}_{i},\mathbf{u}_{i},\mathbf{w}_{i}\right)=\left[\begin{matrix}{\mathbf{\omega}_{m_{i}}-\mathbf{b}_{\mathbf{\omega}_{i}}-\mathbf{n}_{\mathbf{\omega}_{i}}}\\ {^{G}\mathbf{v}_{I_{i}}}\\ {^{G}\mathbf{R}_{I_{i}}\left(\mathbf{a}_{m_{i}}-\mathbf{b}_{\mathbf{a}_{i}}-\mathbf{n}_{\mathbf{a}_{i}}\right)+{^{G}\mathbf{g}}_{i}}\\ {\mathbf{n}_{\mathbf{b}\mathbf{\omega}_{i}}}\\ {\mathbf{n}_{\mathbf{b}a_{i}}}\\ {\mathbf{0}_{3\times1}}\\ \end{matrix}\right]. $$
---
Fig. 1. Overview of the proposed system, consisting of five parts: a processing, an information refinement, an information augmentation, an iterated extended Kalman filter, and a probabilistic voxel map.
For the LiDAR sensor, assume that the end time of a LiDAR scan S is denoted as $t_{k}$ and the sampling time of an individual point $L_{p_{i}}$ within this scan is $t_{i}$. The pose of the LiDAR at any time t can be represented as
$$ {}^{G}\mathbf{T}_{L}(t)=\left[\begin{matrix}{{}^{G}\mathbf{R}_{L}(t)}&{{}^{G}\mathbf{p}_{L}(t)}\\ {\mathbf{0}}&{\mathbf{1}}\\ \end{matrix}\right]_{4\times4} $$
where $^G\mathbf{R}_L$ and $^G\mathbf{p}_L$ denote the rotation matrix and translation vector of the LiDAR frame with respect to the global frame.
In order to compensate for the motion, we need to project the LiDAR point $^L p_i$ sampled at $t_i$ to the scan-end time $t_k$, as follows:
$$ {^{L}}p_{k}={^{G}}\mathbf{T}_{L}^{\top}(t_{k}){^{G}}\mathbf{T}_{L}(t_{i}){^{L}}p_{i}={^{L_{t_{k}}}}\mathbf{T}_{L_{t_{i}}}{^{L}}p_{i}. $$
The relative pose $L_{t_k} \mathbf{T}_{L_{t_i}}$ between time $t_i$ and $t_k$ can be produced by a backward propagation model with IMU data, which is defined as follows:
$$ \mathbf{\check{x}}_{j-1}=\mathbf{\check{x}}_{j}\boxplus(-\Delta t\mathbf{f}(\mathbf{\check{x}}_{j},\mathbf{u}_{j},\mathbf{0})) $$
where $\mathbf{x}_j$ is the estimate of $\mathbf{x}_j$ relative to $\mathbf{x}_k$ in the back propagation, the backward propagation process starts from zero pose and rests states from the forward predicted state $\hat{\mathbf{x}}_k$. Based on the above steps, the motion distortion of the LiDAR scan will be eliminated.
### B. Information Refinement
For a filter-based LIO system, the measurement model can provide constraints for state estimation. In this section, we only focus on the geometric part of the entire measurements as detailed in Sections IV-C and IV-E. In order to minimize the redundant geometric measurements and suppress state updates along uninformative directions, the information refinement module is proposed. The main functions are presented in Fig. 2 and a detailed description is as follows.
Typically, the undistorted point cloud $\mathcal{P}$ is utilized to calculate the LiDAR scan-to-map measurement residuals. For each
Fig. 2. Main functions of the proposed information refinement module. (a) Prune LiDAR points. (b) Detect degeneracy directions (c) Refine optimization directions.
valid point $^L\mathbf{p}_i$, the measurement residual $\mathbf{z}_i^{geo}$ is defined as the distance between this LiDAR point and the corresponding plane feature. To sum up, the geometric measurement model could be presented in the following compact form:
$$ \begin{aligned}\mathbf{0}&=\mathbf{h}_{i}\left(\mathbf{x},\mathbf{w}_{i}\right)\simeq\mathbf{h}_{i}\left(\widehat{\mathbf{x}},\mathbf{0}\right)+\mathbf{J}_{i}^{geo}\delta\mathbf{x}+\mathbf{v}_{i}\\&=\mathbf{n}_{i}^{\top}\left(^{G}\hat{\mathbf{T}}_{L}{}^{L}\mathbf{p}_{i}-{}^{G}\mathbf{q}_{i}\right)+\mathbf{J}_{i}^{geo}\delta\mathbf{x}+\mathbf{v}_{i}\\&=\mathbf{z}_{i}^{geo}+\mathbf{J}_{i}^{geo}\delta\mathbf{x}+\mathbf{v}_{i}\\ \end{aligned} $$
where $\mathbf{h}$ is the LiDAR geometric measurement model and $\widehat{\mathbf{x}}$ denotes the predicted system state, containing $G\widehat{\mathbf{T}}_L$. The $\mathbf{J}_i^{geo}$ is Jacobian matrix of $\mathbf{h}_i(\widehat{\mathbf{x}} \otimes \delta \mathbf{x}, \mathbf{0})$ with respect to the error state $\delta \mathbf{x}$ evaluated at zero. In addition, $\mathbf{v}_i \in \mathcal{N}(\mathbf{0}, \mathbf{R}_i)$ is white gaussian measurement noise. Besides, $\mathbf{n}_i$ and $G\mathbf{q}_i$ represent the normal vector and a reference point of the associated plane feature in the global map $\mathcal{M}$, satisfying $\mathbf{n}_i^\top(G\mathbf{q}_i - G\mathbf{o}_i) = 0$ with $G\mathbf{o}_i = 0$ being another point in the same plane.
However, the above LiDAR measurement model may generate redundant measurements when too many points are from similar directions [^28]. These redundancies not only affect the real-time performance of the system but also lead to the accumulation of measurement errors along similar directions,
---
particularly under degenerate environments. Besides, the Jacobian matrix from this measurement model cannot provide adequate optimization constraints in degenerate directions. The following is a simple explanation.
Assume the residual of geometric scan-to-map registration induced by the pose parameters $\theta = \{R, t\}$ is as follows:
$$ r_{\theta}(P)=\sum_{(p,q)\in C_{P}^{Q}}\|r_{\theta}(p,q)\|_{2}^{2} $$
where $C_P^Q$ is the correspondence set and the environment is modeled as a set of atomic hyperplane surfaces $S = \{S_s\}_{s=1}^S$, each observed through a corresponding point subset $P_s \subset P$.
Thus, the total residual can be rewritten as a sum over surface subsets
$$ r_{\theta}(S)=\sum_{P_{s}\in S}\sum_{(p,q)\in C_{P_{s}}^{Q}}\|r_{\theta}(p,q)\|_{2}^{2}. $$
To describe the contribution of each surface independently of its cardinality, an average set-residual rate is defined as
$$ \bar{r}_{\theta}(S)=\sum_{P_{s}\in S}\frac{1}{|C_{P_{s}}^{Q}|}\sum_{(p,q)\in C_{P_{s}}^{Q}}\|r_{\theta}(p,q)\|_{2}^{2}. $$
Definition: Assuming zero noise, each hyperplane surface $S_{s}$ produces $|P_{s}| - 1$ redundant residuals.
That is, for any two points $p_i, p_j \in P_s$
$$ r_{\theta}(p_{i},q_{i})\parallel r_{\theta}(p_{j},q_{j}). $$
This means their corresponding residual vectors are parallel and of similar magnitude. Removing such redundancy points and residuals does not alter the optimum of the objective function, as all contribute to identical gradient and Hessian directions. For more proofs and derivations, please refer to [^28].
Accordingly, the redundancy-degeneracy-aware geometric measurement model is proposed. First, we adopt the redundancy-minimizing point cloud sampling (RMS) framework [^28] to acquire information-maximizing LiDAR points $P_{RMS}$ for scan-to-map registration through gradient flow entropy maximization. Next, with the predicted state, N retained points and the global voxel map, the cost function of the entire scan-to-map measurements residuals in (9) can be reformulated as a quadratic cost optimization problem and the Hessian matrix $A'$ can be divided as follows:
$$ \begin{aligned}{\mathbf{A}^{\prime}}&{{}=\left(\sum_{i=1}^{N}\underbrace{\left[\begin{matrix}{{\left(^{L}\mathbf{p}_{i}\times\mathbf{n}_{i}\right)}}\\ {\mathbf{n}_{i}}\\ \end{matrix}\right]}_{A}\underbrace{\left[\begin{matrix}{{\left(^{L}\mathbf{p}_{i}\times\mathbf{n}_{i}\right)}^{\top}\mathbf{n}_{i}^{\top}}\\ \end{matrix}\right]}_{A^{\top}}\right)}\\ {}&{{}=\left[\begin{matrix}{\mathbf{A}_{r r}^{\prime}}&{\mathbf{A}_{r t}^{\prime}}\\ {\mathbf{A}_{t r}^{\prime}}&{\mathbf{A}_{t t}^{\prime}}\\ \end{matrix}\right]_{6\times6}.}\\ \end{aligned} $$
For the translation components $\mathbf{A}_t'$, the eigenvalue decomposition is $\mathbf{A}'_t = \mathbf{U}_t \boldsymbol{\Sigma}_t \mathbf{U}_t^\top$, utilizing the degeneracy detection method X-ICP [^9], the set of uninformative degenerate directions $\mathbf{V}_t$ can be determined. The cost function with Tikhonov regularization for scan-to-map registration, as (15),
Fig. 3. Simplified optimization problem with Tikhonov regularization. The total loss, ordinary loss, and regularization loss are represented by the red surface, green surface, and blue surface, respectively. Without Tikhonov regularization, the solution $\dot{\mathbf{x}}$ (black point) may fall into the degenerate section, not the constrained optimal solution $\mathbf{x}^{*}$ (orange star).
can improve the ICP behavior in geometric degenerate environments. A simplified example is shown in Fig. 3
$$ \mathbf{E}(\mathbf{x})=\sum_{i=1}^{N}||\underbrace{\mathbf{n}_{i}^{\top}\left({}^{G}\mathbf{T}_{L}{}^{L}\mathbf{p}_{i}-{}^{G}\mathbf{q}_{i}\right)}_{\mathbf{e}_{i}}||_{2}+\lambda_{D}||\mathbf{V}_{t}\mathbf{x}||_{2}^{2}. $$
To minimize the cost, the Jacobian $\mathbf{J}_i$, Gradient $\nabla_{\mathbf{E}(\mathbf{x})}$, and Hessian $\mathbf{H}_{\mathbf{E}(\mathbf{x})}$ with respect to $\mathbf{x}$ can provide optimization directions, which are defined as follows:
$$ \begin{aligned}\mathbf{J}_{i}&=\frac{\partial\mathbf{e}_{i}}{\partial\mathbf{x}}\\\nabla_{\mathbf{E}(\mathbf{x})}&=\sum_{i=1}^{N}\left(2\mathbf{J}_{i}^{\top}\mathbf{e}_{i}\right)+2\lambda_{D}\mathbf{V}_{t}^{\top}\mathbf{V}_{t}\mathbf{x}\\\mathbf{H}_{\mathbf{E}(\mathbf{x})}&=\frac{\partial^{2}\mathbf{E}(\mathbf{x})}{\partial\mathbf{x}^{2}}=\sum_{i=1}^{N}\left(2\mathbf{J}_{i}\mathbf{J}_{i}^{\top}\right)+2\lambda_{D}\mathbf{V}_{t}^{\top}\mathbf{V}_{t}\end{aligned} $$
where $\lambda_{D}$ is the degeneracy regularization parameter.
In a filter-based LiDAR odometry framework, the Kalman gain can provide an update direction by balancing the prior and measurements model. Thus, we designed a regularization information refinement matrix with uninformative directions, aiming to provide optimization directions by adjusting Kalman gain associated with the measurement-dependent part. Following the above derivation, the Hessian matrix $\mathbf{H}_{(\mathbf{x})}$ of the regularization term $\lambda_D\|\mathbf{V}_t\mathbf{x}\|_2^2$ can be seen as the information refinement matrix utilized in Section IV-E. If we denote $\mathbf{L}(\mathbf{x}) = \lambda_D\|\mathbf{V}_t\mathbf{x}\|_2^2$, then Gradient and Hessian matrices of $\mathbf{L}(\mathbf{x})$ are defined as follows:
$$\begin{aligned}\nabla_{\mathbf{L}(\mathbf{x})}&=2\lambda_{D}\mathbf{V}_{t}^{\top}\mathbf{V}_{t}\mathbf{x}\\ \mathbf{H}_{\mathbf{L}(\mathbf{x})}&=\frac{\partial^{2}\mathbf{L}(\mathbf{x})}{\partial\mathbf{x}^{2}}=2\lambda_{D}\mathbf{V}_{t}^{\top}\mathbf{V}_{t}.\end{aligned}$$
### C. Information Augmentation
As degeneracy arises from insufficient constraints, fusion of geometric measurements with other modal data can improve system robustness. Recently, LiDAR intensity information has increasingly been integrated into LiDAR-based odometry systems.
---
To construct complementary intensity constraints in degenerate directions, we leverage the intensity measurement model proposed in Coin-LIO [^14]. The input LiDAR point cloud is first projected as an intensity image using a spherical projection. Irregular spacing in the elevation angles of LiDAR beams causes line artifacts, which are subsequently mitigated through image processing. To construct geometrically complementary residuals in detected degenerate directions $V_t$, the complementary intensity contribution $c_t$ of image patches is calculated as follows:
$$ \begin{aligned}\mathbf{s}_{i}&=\frac{\partial\Pi\left(^{L}\mathbf{p}_{j}\right)}{\partial^{L}\mathbf{p}_{j}}\cdot\mathbf{v}_{t,i}\in\mathbb{R}^{2}\quad\forall\mathbf{v}_{t,i}\in\mathbf{V}_{t}\\\mathbf{c}_{i}&=\frac{\mathbf{s}_{i}\cdot\mathbf{v}_{patch}}{\|\mathbf{s}_{i}\|_{2}^{2}}\end{aligned} $$
where $\Pi(*)$ denotes the spherical projection model. $\mathbf{v}_{patch}$ is the strongest eigenvector of the second image moment, which is seen as the intensity image informative direction of each patch. The $\mathbf{s}_{i}$ represents the projected image coordinate shift induced by directional point perturbation.
For each degenerate directions in $\mathbf{V}_t$, the patches in the image with the max contribution $\mathbf{c}_i$ is selected to construct complementary photometric constraints, the photometric residuals between projected intensity value $I_c(*)$ and measured intensity value $i_f$ of tracked point is $\mathbf{z}^{pho} = I_c(\Pi(L\mathbf{p}_f)) - i_f$. The Jacobian $\mathbf{J}^{pho}$ of the photometric measurement model and further details are provided in [^14].
### D. Probabilistic Voxel Mapping
Following the above discussions, degeneracy detection is required for both information refinement and information augmentation. Besides, the key step for degeneracy detection is to match the information-maximizing points with the global map, and the accuracy of point-to-plane matching determines the quality of degeneracy detection and geometric measurements. In addition, the uncertainties arising from both LiDAR point measurement noise and pose estimation error also influence the accuracy of geometric constraints and state update. Therefore, the probabilistic voxel mapping approach [^22] is adopted, which models LiDAR point measurement noise and pose estimation error within the plane features of the global map. It also designs a probabilistic method for accurate point-to-plane match and measurement noise calculation.
Given a prior LiDAR point $^G\mathbf{p}_i$, a plane with normal $^G\mathbf{n}_i$ and center point $^G\mathbf{q}_i$. The distance $d_i$ of the point-to-plane is
$$ d_{i}={}^{G}\mathbf{n}_{i}^{\top}\left({}^{G}\mathbf{p}_{i}-{}^{G}\mathbf{q}_{i}\right). $$
Incorporates noises $\delta_{\mathbf{n}_i}, \delta_{\mathbf{p}_i}, \delta_{\mathbf{q}_i}$ of $^G\mathbf{n}_i$, $^G\mathbf{p}_i$, $^G\mathbf{q}_i$ into the (19), we can get
$$ \begin{align*}d_{i}&=\left({^{G}\mathbf{n}_{i}^{gt}}\boxplus\delta_{\mathbf{n}_{i}}\right)^{\top}\left[\left({^{G}\mathbf{p}_{i}^{gt}}+\delta_{G\mathbf{p}_{i}}\right)-{^{G}\mathbf{q}_{i}^{gt}}-\delta_{G\mathbf{q}_{i}}\right]\\&\approx\underbrace{\left({^{G}\mathbf{n}_{i}^{gt}}\right)^{\top}\left({^{G}\mathbf{p}_{i}^{gt}}-{^{G}\mathbf{q}_{i}^{gt}}\right)}_{=0}\\&\quad+\mathbf{J}_{G\mathbf{n}_{i}}\delta_{G\mathbf{n}_{i}}+\mathbf{J}_{G\mathbf{q}_{i}}\delta_{G\mathbf{q}_{i}}+\mathbf{J}_{G\mathbf{p}_{i}}\delta_{G\mathbf{p}_{i}}.\end{align*} $$
This implies $d_i \sim \mathcal{N}(0, \Sigma_{\mathbf{w}_i})$, where $\mathbf{w}_i = \mathbf{J}_{c_{\mathbf{n}_i}} \delta_{c_{\mathbf{n}_i}} + \mathbf{J}_{c_{\mathbf{q}_i}} \delta_{c_{\mathbf{q}_i}} + \mathbf{J}_{c_{\mathbf{p}_i}} \delta_{c_{\mathbf{p}_i}}$ and $\mathbf{J}_{c_{\mathbf{n}_i}}, \mathbf{J}_{c_{\mathbf{q}_i}}, \mathbf{J}_{c_{\mathbf{p}_i}}$ are the corresponding Jacobians. If a point belongs to a plane, the distance between them is within $3\sigma$, where $\sigma = \sqrt{\Sigma_{\mathbf{w}_i}}$. In addition, the noise $\mathbf{v}_i$ of a valid point-to-plane distance measurement in (9) is calculated as $\mathbf{J}_{c_{\mathbf{n}_i}} \delta_{c_{\mathbf{n}_i}} + \mathbf{J}_{c_{\mathbf{q}_i}} \delta_{c_{\mathbf{q}_i}} + \mathbf{J}_{c_{\mathbf{p}_i}} \delta_{c_{\mathbf{p}_i}}$. A comprehensive discussion can be found in [^22].
```pseudo
// Algorithm 1: State Estimation
Input: Last estimation $\bar{\mathbf{x}}_{k-1}$, $\bar{\mathbf{P}}_{k-1}$;
```
LiDAR points $\mathcal{P}$, IMU inputs $(\mathbf{a}_m, \omega_m)$;
Geometric map $\mathcal{M}$, Intensity features $\mathcal{F}$;
Output: Current estimation $\bar{\mathbf{x}}_k$, $\bar{\mathbf{P}}_k$;
1: $\hat{\mathbf{x}}_k$, $\hat{\mathbf{P}}_k \leftarrow$ Predict $(\bar{\mathbf{x}}_{k-1}, \bar{\mathbf{P}}_{k-1}, \mathbf{a}_m, \omega_m)$;
2: $\mathcal{P}_{un} \leftarrow$ CompensateMotion $(\mathcal{P}, \mathbf{a}_m, \omega_m)$;
3: $\text{Img} \leftarrow$ SphericalProjection $(\mathcal{P})$;
4: Preprocess (Img);
5: $\mathcal{P}_{RMS} \leftarrow$ InforMaxSelection $(\mathcal{P}_{un})$;
6: $\kappa \leftarrow -1$, $\hat{\mathbf{x}}_k^{\kappa=0} \leftarrow \hat{\mathbf{x}}_k$;
7: repeat
8: $\kappa \leftarrow \kappa + 1$;
9: $\mathbf{H}^K \leftarrow \frac{\partial (\hat{\mathbf{x}}_k^{\kappa} \oplus \hat{\mathbf{x}}_k^{\tau}) \oplus \hat{\mathbf{x}}_k}{\partial \hat{\mathbf{x}}_k^{\tau}} | \hat{\mathbf{x}}_k^{\tau}=0$;
10: $\mathbf{P} \leftarrow (\mathbf{H}^{\kappa})^{-1} \hat{\mathbf{P}}_k(\mathbf{H}^{\kappa})^{-\top}$;
11: $\mathbf{z}^{geo}, \mathbf{J}^{geo} \leftarrow$ GeocMeasure $(\hat{\mathbf{x}}_k^K, \mathcal{P}_un, \mathcal{M})$;
12: $\mathbf{V}_t \leftarrow$ DedeneracyDetection $(\mathbf{J}^{geo})$;
13: $\mathbf{z}^{pho}, \mathbf{J}^{pho} \leftarrow$ PhcMeasure $(\hat{\mathbf{x}}_k^K, \mathcal{P}_img, \mathcal{F}, \mathbf{V}_t)$;
14: $\mathbf{H}_{L(x)} \leftarrow 2\lambda_D \mathbf{V}_t^{\top} \mathbf{V}_t$;
15: $\mathbf{J} \leftarrow [\mathbf{J}^{geo}, \mathbf{J}^{pho}];$ // Combine Measurements
16: $\mathbf{z} \leftarrow [\mathbf{Z}^{geo}, \mathbf{Z}^{pho}]$;
17: $\mathbf{K} \leftarrow (\mathbf{J}^{\top} \mathbf{R}^{-1} \mathbf{J} + \mathbf{H}_L(x) + \mathbf{P}^{-1})^{-1} \mathbf{J}^{\top} \mathbf{R}^{-1}$;
18: $\hat{\mathbf{x}}_k^{\kappa+1} \leftarrow \hat{\mathbf{x}}_k^{\kappa} \oplus (\mathbf{-Kz} - (\mathbf{I} - \mathbf{KH})(\mathbf{J})^{-1} \left( \hat{\mathbf{x}}_k^{\kappa} \oplus \hat{\mathbf{x}}_k^{\tau} \right))$;
19: until $\|\hat{\mathbf{x}}_k^{\kappa+1} \oplus \hat{\mathbf{x}}_k^{\kappa}\| < \epsilon$;
20: $\hat{\mathbf{x}}_k \leftarrow \hat{\mathbf{x}}_k^{\kappa+1}$; $\hat{\mathbf{P}}_k \leftarrow (\mathbf{I} - \mathbf{KH})\mathbf{P}$;
21: UpdateMapAndFeatures $(\mathcal{F}, \mathcal{M}, \hat{\mathbf{x}}_k, \mathcal{P}, \text{Img})$;
By the way, the sparsity of the pruned points makes it unsuitable for representing plane features within the environment. Therefore, the original undistorted LiDAR points are utilized for mapping, and pruned points for scan-to-map match and degeneracy detection.
### E. Constrained State Update
After obtaining the geometric residuals $\mathbf{z}^{geo}$, the photometric residuals $\mathbf{z}^{pho}$, and the corresponding Jacobian matrices, these measurements can be combined to construct the combined measurements as follows:
$$ \begin{aligned}r c l\mathbf{J}&=\left[\mathbf{J}_{1}^{\mathrm{e w}^{\top}},\ldots,\mathbf{J}_{m}^{\mathrm{g e o}^{\top}},\lambda\mathbf{J}_{1}^{\mathrm{p h o}^{\top}},\ldots,\lambda\mathbf{J}_{n}^{\mathrm{p h}\omega^{\top}}\right]^{\top}\\ \mathbf{z}&=\left[\mathbf{z}_{1}^{\mathrm{g e o}},\ldots,\mathbf{z}_{m}^{\mathrm{g e v}},\lambda\mathbf{z}_{1}^{\mathrm{p h o}},\ldots,\lambda\mathbf{z}_{n}^{\mathrm{p h o}^{\top}}\right]\end{aligned} $$
where $\lambda$ is the scaling factor between geometric and photometric measurements. Combining the state prior with all effective measurements, we can obtain the state estimation problem as
$$ \min_{\delta\mathbf{x}}\left(\|\mathbf{x}_{k}\boxminus\widehat{\mathbf{x}}_{k}\|_{\widehat{\mathbf{P}}_{k}^{-1}}^{2}+\sum_{j=1}^{m+n}\|\mathbf{z}_{j}+\mathbf{J}_{j}\delta\mathbf{x}\|_{\mathbf{R}_{j}^{-1}}^{2}\right) $$
where $\|x\|_{\mathrm{M}}^{2} = x^{\top} \mathrm{M} x$, let $\mathbf{R} = \text{diag}(\mathbf{R}_1, \ldots, \mathbf{R}_{m+n})$ and the normal Kalman gain $\mathbf{K} = (\mathbf{J}^{\top} \mathbf{R}^{-1} \mathbf{J} + \mathbf{P}^{-1})^{-1} \mathbf{J}^{\top} \mathbf{R}^{-1}$. To
---
provide optimization directions through the information refinement matrix $\mathbf{H}_{\mathrm{L}(x)}$, the measurement-dependent part within the Kalman gain $\mathbf{K}$ need to be adjusted. Since only the degeneracy in the translation direction is detected in our system, if we denote the translation submatrix of $\mathbf{J}^{\top}\mathbf{R}^{-1}\mathbf{J}$ as $\mathbf{G} = (\mathbf{J}^{\top}\mathbf{R}^{-1}\mathbf{J})_{3\times3}$, $\mathbf{G}$ need to be adjusted as $\mathbf{G} + \mathbf{H}_{\mathrm{L}(x)}$. This can be expressed in a compact equation with padding $\mathbf{H}_{\mathrm{L}(x)}^{*}$ as follows:
$$ \mathbf{K}=\left(\mathbf{J}^{\top}\mathbf{R}^{-1}\mathbf{J}+\mathbf{H}_{\mathbf{L}(\mathbf{x})}^{*}+\mathbf{P}^{-1}\right)^{-1}\mathbf{J}^{\top}\mathbf{R}^{-1}. $$
The above process repeats until convergence, and we can register the LiDAR points into the probabilistic voxel map with the updated state. The whole state estimation flow is summarized in Algorithm 1.
### V. EXPERIMENT
To evaluate the performance of the proposed method, we conduct both quantitative and qualitative comparative experiments against several state-of-the-art open-source LIO: Fast-LIO2 [^1], a real-time high-accuracy LIO system; Point-LIO [^21], a point-by-point LIO; VoxelMap [^22], a LIO system with probabilistic adaptive voxel map; Coin-LIO [^14], an advanced LIO in degenerate environments, which introduces LiDAR intensity for complementary measurements.
In this section, we quantitatively evaluate the localization accuracy against other methods. The root-mean-square error (RMSE) of absolute translation error (ATE) is selected as the evaluation metric of localization, where the lower RMSE value indicates higher localization accuracy. To qualitatively assess map quality, we employ the mean map entropy (MME) [^29], an information-theoretic metric for local consistency. Points are color-coded by MME values, where lower values indicate higher consistency.
We conduct experiments on the FusionPortable V2 [^30] and Enwide [^14] datasets. The two datasets are both recorded with a 128-beam Ouster LiDAR, and the LiDAR points can be projected into a reliable intensity image with the metadata provided by the Ouster driver. In addition, we conduct additional comparative experiments on the NTU-viral dataset with the Information Augmentation Module disabled. The NTU-viral dataset is a multisensor aerial robotics dataset collected on the NTU campus using a DJI M600 platform, featuring both indoor and outdoor environments with precise ground-truth tracking. All experiments are done in the robots' operating system (ROS) on a computer with an Intel i9-13900K CPU at 5.80 GHz and 32 GB of RAM. Given that the baseline methods and our approach share analogous algorithmic frameworks, the shared parameters among them are set identically, and the other parameters are provided by the baseline methods.
### A. Experiments in Universal Environments
In this section, we conduct experiments on the universal FusionPortable V2 dataset. This dataset covers varied motions and extensive environments [^30]. In our experiments, we only focus on the scenarios with fewer geometric structures.
1) Localization Accuracy: The localization accuracy results of the FusionPortable V2 and NTU-viral dataset, calculated with the toolbox are listed in Tables I and II. The comparison of the trajectory with the ground-truth is shown in Figs. 4 and 5. From these experimental results, it can be demonstrated that the proposed system exhibits optimal localization accuracy in most scenarios. Besides, leveraging the probabilistic voxel map for geometric measurements, VoxelMap demonstrated superior localization accuracy compared to the ikd-tree structure employed in Fast-LIO2 and Point-LIO. While Coin-LIO utilizes LiDAR intensity to construct complementary measurements in the FusionPortable V2 dataset, the inaccurate geometric measurements and intensity noise reduce the localization accuracy.
2) Map Consistency: The mapping results of ugv-campus01 and legged-grass00 sequences are presented in Fig. 6. The point cloud map constructed by the proposed method preserves finer details of trees and walls while exhibiting reduced blurring and ghosting artifacts. Moreover, the MME visualization reveals a higher proportion of warm-colored areas, signifying enhanced local map consistency. As the geometric measurements depend on the accurate scan-to-map match, the higher local map consistency enhances match accuracy, thereby improving both degeneracy detection precision and localization accuracy.
### B. Experiments in Degenerate Environments
Although our system achieves the best performance in the universal environments, the principal incentive for the method is to improve localization and mapping performance in geometrically degenerate environments. Thus, in this section, we perform experiments on the challenging Enwide dataset. This dataset is collected in typical geometrically degenerate environments. All sequences of the dataset contain long sections of geometric degeneracy, which also provides reliable LiDAR intensity information. In addition, to verify the effectiveness of the proposed information refinement module and the adopted probabilistic voxel map, an ablation study is conducted on this dataset. We remove the information refinement module and probabilistic voxel map separately, denoted as ours (w/o IR) and ours (w/o VM) relatively.
1) Localization Accuracy: Table III presents the localization accuracy results, and Fig. 7 shows the comparison of the trajectory with the ground-truth. Due to the lack of geometric degeneracy mitigation mechanisms, Fast-LIO2, Point-LIO, and VoxelMap fail in some severely degenerate environments, such as tunnel and runway. Because of the integration of LiDAR intensity information, Coin-LIO, and the proposed method successfully run on all sequences. Comparing our system without the information refinement module and the adopted probabilistic voxel map, the entire system demonstrates a higher localization accuracy. In almost all sequences, the information refinement module can reduce the localization error, especially in runway and intersection sequences. In addition, it can be seen that leveraging a probabilistic voxel map for geometric measurements can improve the localization accuracy.
---
(a)
(b)
Fig. 4. Trajectory comparison results on FusionPortable V2 dataset. The zoomed-in regions are shown in the red boxes. (a) ugv-campus01. (b) legged-grass00. TABLE I
ATE (RMSE, METERS) ON FUSIONPORTABLE V2 DATASET
| Sequence | Name | h-grass00 | h-underground00 | l-grass00 | u-campus01 | u-parking01 | u-parking02 | u-parking03 |
| Method | Fast-LIO2 | 0.467262 | 0.766680 | 0.372376 | 0.677929 | 0.483688 | 1.865421 | 1.393853 |
| Point-LIO | - | 1.800630 | 0.183620 | 0.566013 | 0.316874 | 0.845095 | 1.849024 |
| VoxelMap | 0.064231 | 0.827814 | 0.471301 | 0.468336 | 0.315034 | 0.410986 | 0.477338 |
| Coin-LIO | 0.663138 | 0.501824 | 0.318176 | 0.726021 | 0.323063 | 0.664624 | 0.831481 |
| Ours | 0.148206 | 0.448867 | 0.084614 | 0.443145 | 0.300281 | 0.404877 | 0.498481 |
Note: “-” denotes that the system failed or exhibited significant drift. The best result is displayed in bold, and the second-best result is underlined.
Fig. 5. Trajectory comparison results on NTU-viral dataset spms-01 sequence. The zoomed-in regions are shown in the red boxes.
TABLE II
ATE (RMSE, METERS) ON NTU-VIRAL DATASET
| Method | eee-01 | nya-02 | rtp-02 | spms-01 | sbs-01 |
| Coin-LIO | 0.181815 | 0.171741 | 0.146022 | 2.196588 | 0.184311 |
| Point-LIO | 0.188277 | 0.215078 | 0.363191 | 1.186950 | 0.191895 |
| Fast-LIO2 | 0.158396 | 0.177101 | 0.153968 | 1.827605 | 0.183802 |
| VoxelMap | 4.112456 | 0.381465 | 0.935816 | 1.910679 | 1.015116 |
| Ours | 0.177492 | 0.177734 | 0.128440 | 0.274400 | 0.180789 |
$^{*}$Note: The best result is displayed in bold, and the second-best result is underlined.
2) Map Consistency: The map consistency visualization results of RunwayS and IntersectionD sequences are presented in Fig. 8, while the mapping results of the other environments in the Enwide dataset are shown in Fig. 9. These comparative results demonstrate that our entire system demonstrates a higher map consistency, especially in long segments characterized by geometric degeneracy. These results confirm that the information refinement module can improve map local consistency in severe geometric degeneracy scenarios.
### C. Regularization Parameter Analysis
In principle, increasing $\lambda_{D}$ to a certain extent will introduce additional uncertainty along the degenerate directions, thereby reducing the state update of the system in those directions. However, since both degeneration detection and state uncertainty are influenced by the type of LiDAR and the structural characteristics of the environment, a fixed $\lambda_{D}$ value does not guarantee optimal accuracy for each individual sequence. However, for fairness in our experiments, we do not fine-tune the $\lambda_{D}$ value for each sequence but instead set it to 20 uniformly.
In addition, to evaluate the influence of the degeneracy regularization parameter $\lambda_{D}$, we conduct a sensitivity analysis across multiple Enwide sequences by varying $\lambda_{D}$ from 0 to 100 with a step of 10. The statistical results are summarized in Table IV. Furthermore, min-max normalization is applied to the RMSE values across all sequences, and the average normalized RMSE(m) variation with respect to parameter $\lambda_{D}$ is shown in Fig. 10. The analysis reveals that the optimal $\lambda_{D}$ differs across environments, indicating that its value is environment-dependent rather than universal. However, in most of the experiments, setting the parameter $\lambda_{D}$ within
---
(a)
(b)
Fig. 6. Mapping comparison results on FusionPorable V2 dataset. (a) ugv-campus01. (b) legged-grass00.
(a)
(b)
Fig. 7. Trajectory comparison results of our method and Coin-LIO on Enwide dataset. (a) RunwayS. (b) IntersectionD.
the range of 10–30 yields relatively favorable localization performance.
### D. Real-Time Analysis
To assess the real-time performance, the processing time of each module of our system is evaluated on the Enwide dataset. As summarized in Table V, the system achieves an average processing time of 0.072 s per LiDAR scan (around 13.9 Hz), which exceeds the 10 Hz scanning rate of the Ouster OS0-128 sensor. Although the redundancy minimization and voxel map modules introduce additional overhead compared to COIN-LIO (0.021 s per scan), the system remains capable of real-time operation with satisfactory computational efficiency.
### E. Discussions and Applicability
In this work, the degeneracy-aware formulation focuses on translation degeneracy in LiDAR measurements and thus is primarily applicable to scenarios where high-rate inertial rotation measurements are provided by the IMU and the dominant degeneracy arises from the translation. In addition, our experimental results indicate that the optimal value of $\lambda_{D}$ is closely related to the underlying scene structure, rather than being universally optimal across different
---
TABLE III
ATE (RMSE, METERS) ON ENWIDE DATASET
| Method | Fast-LIO2 | Point-LIO | VoxelMap | Coin-LIO | ours (w/o IR) | ours (w/o VM) | ours |
| TunnelS | - | - | - | 0.776831 | 0.730282 | 0.736179 | 0.705223 |
| TunnelD | - | - | - | 0.505336 | 0.426861 | 0.489148 | 0.339451 |
| IntersectionS | 0.446384 | 0.183806 | 0.149982 | 0.328971 | 0.240054 | 0.457720 | 0.170944 |
| IntersectionD | - | - | 0.347427 | 1.650612 | 0.390446 | 3.689573 | 0.254946 |
| RunwayS | - | - | 3.210004 | 1.814010 | 0.580743 | 1.301178 | 0.539720 |
| RunwayD | - | - | - | 2.235924 | 2.081241 | 2.666165 | 1.641159 |
| FieldS | 0.213750 | 0.984559 | 0.191844 | 0.205698 | 0.172558 | 0.265523 | 0.205306 |
| FieldD | 12.011100 | - | 0.185444 | 0.526876 | 0.203913 | 0.385250 | 0.232325 |
| KatzenseeS | 4.141862 | 0.489176 | 0.245581 | 1.172257 | 0.309386 | 0.506755 | 0.315927 |
| KatzenseeD | 6.036565 | - | 0.196991 | 0.348262 | 0.179833 | 0.618113 | 0.147715 |
Note: “-” denotes that the system failed or exhibited significant drift. The best result is displayed in bold, and the second-best result is underlined. IR denotes the Information Refinement Module.VM denotes the Probabilistic Voxel Mapping Module.
(a)
(b)
Fig. 8. Mapping results of our method and Coin-LIO on Enwide dataset. (a) RunwayS. (b) IntersectionD.
TABLE IV
DISTRIBUTION OF RMSE FOR EACH SEQUENCE
| Sequence | Min | Max | Range | Mean | Std |
| FieldS | 0.192365 | 0.305410 | 0.113045 | 0.244995 | 0.036276 |
| IntersectionS | 0.244648 | 0.285907 | 0.041259 | 0.267306 | 0.013637 |
| KatzenseeS | 0.106669 | 0.119995 | 0.013326 | 0.11171 | 0.003985 |
| RunwayS | 0.706157 | 1.312839 | 0.606682 | 0.944527 | 0.197481 |
| TunnelD | 0.441583 | 0.512136 | 0.070553 | 0.471745 | 0.023941 |
| TunnelS | 0.655489 | 0.693984 | 0.674555 | 0.038495 | 0.011382 |
TABLE V
REAL-TIME PERFORMANCE ANALYSIS
| Computation Time | Average[s] | Max[s] | Min[s] |
| Total | 0.0726950581 | 0.1715988720 | 0.0591839850 |
| Rredundancy minimizing | 0.0170426966 | 0.0279797190 | 0.0138959010 |
| State Update Time | 0.0031382527 | 0.0051271680 | 0.0019688940 |
| Track Intensity Features | 0.0011082522 | 0.0026169430 | 0.0007360890 |
| Degeneracy Detection | 0.0000187596 | 0.0000513050 | 0.0000116240 |
| Calculate Regularization Matrix | 0.0000003700 | 0.0000026620 | 0.0000000220 |
| Update VoxelMap | 0.0360651181 | 0.1378541830 | 0.0268253270 |
environments. In practice, the selection of $\lambda_{D}$ is largely and exhibits noticeable variation with respect to environmental characteristics and geometric conditions. Consequently, developing adaptive or scene-aware strategies for tuning $\lambda_{D}$ represents an important and promising direction for future research.
---
Fig. 9. Mapping results of our method on the Enwide dataset.
Fig. 10. Average normalized RMSE(m) variation with respect to parameter $\lambda_{D}$.
### VI. CONCLUSION
This article proposes a geometry-intensity fused LIO framework that enables robust and accurate localization in geometrically degenerate environments. The information refinement module is designed to reduce redundant and inaccurate geometric measurements, which can also indicate optimization directions. The information augmentation module selects LiDAR intensity image features along geometric degenerate directions to construct complementary photometric measurements. Experiments on public datasets are conducted to show the superior performance of the proposed system in terms of accuracy and map consistency when compared to the state-of-the-art methods, especially in extremely degenerate environments.
Future work will also explore deep learning integration to enhance SLAM performance in degenerate environments with weak geometric constraints. By learning highly discriminative point cloud features, the system could maintain localization in feature-sparse or repetitive scenes. Furthermore, efforts will aim to reduce the environmental specificity of system parameters, enabling more adaptive and generalizable SLAM solutions.
## REFERENCES
[^1]: W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, “FAST-LIO2: Fast direct LiDAR-inertial odometry,” IEEE Trans. Robot., vol. 38, no. 4, pp. 2053–2073, Aug. 2022.
[^2]: J. Lv, K. Hu, J. Xu, Y. Liu, X. Ma, and X. Zuo, "CLINS: Continuous-time trajectory estimation for LiDAR-inertial system," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Sep. 2021, pp. 6657–6663.
[^3]: H. Gao, Q. Qiu, H. Liu, D. Liang, C. Wang, and X. Zhang, "ERPoT: Effective and reliable pose tracking for mobile robots using lightweight polygon maps," IEEE Trans. Robot., vol. 41, pp. 3799–3819, 2025.
[^4]: Q. Wu et al., “UA-LIO: An uncertainty-aware LiDAR-inertial odometry for autonomous driving in urban environments,” IEEE Trans. Instrum. Meas., vol. 74, pp. 1–12, 2025.
[^5]: L. Gong et al., “PreciseSLAM: Robust, real-time, LiDAR-inertial-ultrasonic tightly-coupled SLAM with ultraprecise positioning for plant factories,” IEEE Trans. Ind. Informat., vol. 20, no. 6, pp. 8818–8827, Jun. 2024.
[^6]: D. Liu, J. Wu, Y. Du, R. Zhang, and M. Cong, “SBC-SLAM: Semantic bioinspired collaborative SLAM for large-scale environment perception of heterogeneous systems,” IEEE Trans. Instrum. Meas., vol. 73, pp. 1–10, 2024.
[^7]: J. Wen, X. Zhang, H. Gao, J. Yuan, and Y. Fang, "CAE-RLSM: Consistent and efficient redundant line segment merging for online feature map building," IEEE Trans. Instrum. Meas., vol. 69, no. 7, pp. 4222–4237, Jul. 2020.
[^8]: J. Zhang, M. Kaess, and S. Singh, “On degeneracy of optimization-based state estimation problems,” in Proc. IEEE Int. Conf Robot. Autom. (ICRA), Aug. 2016, pp. 809–816.
[^9]: T. Tuna, J. Nubert, Y. Nava, S. Khattak, and M. Hutter, “X-ICP: Localizability-aware LiDAR registration for robust localization in extreme environments,” IEEE Trans. Robot., vol. 40, pp. 452–471, 2024.
[^10]: J. Hatleskog and K. Alexis, “Probabilistic degeneracy detection for point-to-plane error minimization,” IEEE Robot. Autom. Lett., vol. 9, no. 12, pp. 11234–11241, Dec. 2024.
[^11]: W. Chen et al., "P2D-DO: Degeneracy optimization for LiDAR SLAM with point-to-distribution detection factors," IEEE Robot. Autom. Lett., vol. 10, no. 2, pp. 1489–1496, Feb. 2025.
[^12]: H. Li, B. Tian, H. Shen, and J. Lu, “An intensity-augmented LiDAR-inertial SLAM for solid-state LiDARs in degenerated environments,” IEEE Trans. Instrum. Meas., vol. 71, pp. 1–10, 2022.
[^13]: Y. Zhang et al., “RI-LIO: Reflectivity image assisted tightly-coupled LiDAR-inertial odometry,” IEEE Robot. Autom. Lett., vol. 8, no. 3, pp. 1802–1809, Mar. 2023.
[^14]: P. Pfreundschuh, H. Oleynikova, C. Cadena, R. Siegwart, and O. Andersson, "COIN-LIO: Complementary intensity-augmented LiDAR inertial odometry," in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2024, pp. 1730–1737.
[^15]: W. Hu, K. Feng, X. Ye, H. Ding, and L. Wen, “CODN-GS: Coupled optimization of depth and normal in 3D Gaussian splatting for scene reconstruction,” IEEE Trans. Autom. Sci. Eng., vol. 22, pp. 20746–20758, 2025.
[^16]: K. Feng, H. Xiao, J. Zhang, and Q. Ni, "A digital twin methodology for vibration-based monitoring and prediction of gear wear," Wear, vol. 571, Jun. 2025, Art. no. 205806.
[^17]: J. Zhang et al., “Loam: LiDAR odometry and mapping in real-time,” in Proc. Robotics, Sci. Syst., 2014, vol. 2, no. 9, pp. 1–9.
[^18]: T. Shan and B. Englot, “LeGO-LOAM: Lightweight and ground-optimized LiDAR odometry and mapping on variable terrain,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Oct. 2018, pp. 4758–4765.
[^19]: T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, "LIOSAM: Tightly-coupled LiDAR inertial odometry via smoothing and mapping," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Oct. 2020, pp. 5135–5142.
[^20]: C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual-inertial odometry,” IEEE Trans. Robot., vol. 33, no. 1, pp. 1–21, Feb. 2017.
[^21]: D. He, W. Xu, N. Chen, F. Kong, C. Yuan, and F. Zhang, “Point-LIO: Robust high-bandwidth light detection and ranging inertial odometry,” Adv. Intell. Syst., vol. 5, no. 7, Jul. 2023, Art. no. 2200459.
[^22]: C. Yuan, W. Xu, X. Liu, X. Hong, and F. Zhang, “Efficient and probabilistic adaptive voxel mapping for accurate online LiDAR odometry,” IEEE Robot. Autom. Lett., vol. 7, no. 3, pp. 8518–8525, Jul. 2022.
[^23]: C. Zheng et al., “Fast-livo2: Fast, direct LiDAR-inertial-visual odometry,” IEEE Trans. Rob., vol. 41, pp. 326–346, 2025.
[^24]: T. Shan, B. Englot, C. Ratti, and D. Rus, "LVI-SAM: Tightly-coupled LiDAR-visual-inertial odometry via smoothing and mapping," in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2021, pp. 5692–5698.
[^25]: J. Lin and F. Zhang, “R $^{3}$LIVE: A robust, real-time, RGB-colored, LiDAR-inertial-visual tightly-coupled state estimation and mapping package,” in Proc. IEEE Int. Conf Robot. Autom. (ICRA), Apr. 2022, pp. 10672–10678.
[^26]: S. Zhao, H. Zhang, P. Wang, L. Nogueira, and S. Scherer, “Super odometry: IMU-centric LiDAR-visual-inertial estimator for challenging environments,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Sep. 2021, pp. 8729–8736.
---
[^27]: W. Xu and F. Zhang, “FAST-LIO: A fast, robust LiDAR-inertial odometry package by tightly-coupled iterated Kalman filter,” IEEE Robot. Autom. Lett., vol. 6, no. 2, pp. 3317–3324, Apr. 2021.
[^28]: P. Petracek, K. Alexis, and M. Saska, “RMS: Redundancy-minimizing point cloud sampling for real-time pose estimation,” IEEE Robot. Autom. Lett., vol. 9, no. 6, pp. 5230–5237, Jun. 2024.
[^29]: X. Hu et al., “MapEval: Towards unified, robust and efficient SLAM map evaluation framework,” IEEE Robot. Autom. Lett., vol. 10, no. 5, pp. 4228–4235, May 2025.
[^30]: H. Wei et al., “FusionPortableV2: A unified multi-sensor dataset for generalized SLAM across diverse platforms and scalable environments,” Int. J. Robot. Res., vol. 44, no. 7, pp. 1093–1116, Jun. 2025.
Hekai Yao received the B.Eng. degree in artificial intelligence from Dalian University of Technology, Dalian, China, in 2023, where he is currently pursuing the M.Eng. degree in artificial intelligence with the Intelligent Robot Laboratory.
Gang Sun (Graduate Student Member, IEEE) received the B.Eng. degree in automation from Dalian University of Technology, Dalian, China, in 2021, and the M.Eng. degree in control science and engineering from Dalian University of Technology, where he is currently pursuing the Ph.D. degree in artificial intelligence with the Intelligent Robot Laboratory.
His current research interests include LiDAR SLAM and state estimation for autonomous rotorcraft UAVs.
His current research interests include motion planning and perception-aware planning.
He is currently an Associate Professor at the Intelligent Robotic Laboratory, School of Artificial Intelligence, School of Control Science and Engineering, Dalian University of Technology, Dalian, China. His current research interests include autonomous explo
Xuetao Zhang (Member, IEEE) received the B.Eng. degree in electronic science and technology from Hebei University of Technology, Tianjin, China, in 2015, and the Ph.D. degree in control science and engineering from Nankai University, Tianjin, in 2020.
ration, motion planning, visual serving, and state estimation for autonomous rotorcraft UAVs.
Yisha Liu (Member, IEEE) received the B.S. and Ph.D. degrees in control theory and control engineering from Dalian University of Technology, Dalian, China, in 2005 and 2011, respectively.
She is currently a Professor with the College of Information Science and Technology, Dalian Maritime University. Her current research interests include mobile robots and UAV's road detection, path planning, object detection and tracking, and outdoor scene understanding.
Yan Zhuang (Senior Member, IEEE) received the B.S. and M.S. degrees in control theory and control engineering from Northeastern University, Shenyang, China, in 1997 and 2000, respectively, and the Ph.D. degree in control theory and control engineering from Dalian University of Technology, Dalian, China, in 2004.
He is currently a Professor with the School of Control Science and Engineering and the College of Artificial Intelligence. His current research interests include mobile robot 3-D environment perception and mapping, outdoor scene understanding, and machine learning in robotics.