R-LVIO: Resilient LiDAR-Visual-Inertial Odometry for UAVs in GNSS-denied Environment

Abstract

In low-altitude, GNSS-denied scenarios, Unmanned aerial vehicles (UAVs) rely on sensor fusion for self-localization. This article presents a resilient multi-sensor fusion localization system that integrates light detection and ranging (LiDAR), cameras, and inertial measurement units (IMUs) to achieve state estimation for UAVs. To address challenging environments, especially unstructured ones, IMU predictions are used to compensate for pose estimation in the visual and LiDAR components. Specifically, the accuracy of IMU predictions is enhanced by increasing the correction frequency of IMU bias through data integration from the LiDAR and visual modules. To reduce the impact of random errors and measurement noise in LiDAR points on visual depth measurement, cross-validation of visual feature depth is performed using reprojection error to eliminate outliers. Additionally, a structure monitor is introduced to switch operation modes in hybrid point cloud registration, ensuring accurate state estimation in both structured and unstructured environments. In unstructured scenes, a geometric primitive capable of representing irregular planes is employed for point-to-surface registration, along with a novel pose-solving method to estimate the UAV’s pose. Both private and public datasets collected by UAVs validate the proposed system, proving that it outperforms state-of-the-art algorithms by at least 12.6%.

1. Introduction

Unmanned aerial vehicle (UAV) navigation and localization systems must be stable and accurate to complete various tasks. UAVs generally adopt the global navigation satellite system (GNSS) as the core navigation technology. However, the open frequency bands of GNSS satellite signals make them susceptible to interference, which can result in UAVs failing to complete their planned missions or even return to base [1,2,3,4]. Multi-sensor fusion-based localization techniques are essential for UAVs to achieve state estimation in low-altitude, GNSS-denied scenarios. In multi-sensor fusion frameworks, UAVs are typically equipped with light detection and ranging (LiDAR), cameras, and inertial measurement units (IMUs) to collect multi-source information, achieving 6-degree-of-freedom (DoF) state estimation [5,6].

In recent years, numerous UAV sensor fusion frameworks based on LiDAR, cameras, and IMUs have been presented to achieve superior estimation accuracy in artificial environments, such as streets, campuses, and factories [7,8]. In these frameworks, LiDAR is usually used as the core sensor due to its high-fidelity measurements and wide-range sensing capabilities [9,10]. However, in unstructured environments like corridors, deserts, and stadiums, the lack of LiDAR return points leads to insufficient constraints for pose estimation. The challenges encountered in unstructured environments include the following. (1) Irregular planes are difficult to represent accurately. Ground points in unstructured scenes make up a relatively large portion of the LiDAR point cloud. The ground points can be fitted to similar planes due to minimal height differences. However, using planes as primitives results in the loss of the ground’s uneven geometric properties. (2) Vertical constraints are inadequate for UAV height measurement. Unstructured scenes have too few LiDAR return points in the vertical direction, making the LiDAR module insensitive to altitude changes.

To tackle the problems in unstructured scenarios, we propose the following measures. (1) Select appropriate geometric primitives to represent irregular surfaces and adjust the point cloud alignment model. (2) Use short-term prediction of the IMU vertical direction instead of using the LiDAR module for height measurement. (3) Employ the IMU module as the core of the system to output the final state, thereby mitigating the risk of failure in the LiDAR or visual modules. This paper proposes a multi-sensor fusion-based odometry and mapping framework that relies on the complementary advantages of LiDAR, cameras, and IMUs, achieving low-drift and high robustness state estimation. The main contributions of the proposed system are as follows:

• Improve the accuracy of short-term IMU predictions by increasing the frequency of corrections from the LiDAR and visual modules. LiDAR pose frequency is boosted by sweep segmentation to synchronize the LiDAR input time with the camera sampling time.
• Devise an outliers rejection strategy of depth association between the camera image and LiDAR points to select accurate depth points by evaluating the reprojection error of visual feature points in a sliding window.
• Design a structure monitor to distinguish structured scenes and unstructured scenes by analyzing the vertical landmarks. The environmental structuring is quantified to switch the operating modes of the LiDAR module.
• Propose a novel point-to-surface model to register irregular surfaces in unstructured scenes, achieving three horizontal DoF state estimation. The vertical 3-DoF state is predicted by IMU relative measurement.

The paper is structured as follows. The related work of sensor fusion localization and point cloud registration is presented in Section 2. The problem statement is presented in Section 3. The details of the proposed system are illustrated in Section 4. Experimental results on public and private datasets are provided in Section 5. Conclusions and future work are given in Section 6. The notations are listed in

Table 1. List of used notations with their descriptions.

Notations	Descriptions
$t_i$, $t_j$	Input time of camera image and LiDAR sweep
$\mathcal{X}$	Set of all states up to moment $t_n\in {\mathcal{T}}_n$
${\mathit{x}}_i$	State at time $t_i$
${\mathbf{R}}_i$, ${\mathbf{p}}_i$ and ${\mathbf{v}}_i$	Rotation matrix, position vector, and linear velocity at time $t_i$
${\mathbb{C}}_i$, ${\mathbb{L}}_i$	Observations of camera and LiDAR at time $t_i$
${\mathbb{I}}_{i}j$	Set of IMU measurements between moments $t_i$ and $t_j$
$r_0$, $r_{{\mathbb{I}}_{i}j}$, $r_{z_{ic}}$, $r_{z_{il}}$	Residuals of prior, IMU preintegration, and visual and LiDAR feature
${\widehat{\mathit{a}}}_k^B$, ${\widehat{\mathit{w}}}_k^B$	Measurements value of accelerometer and gyroscope at time $t_k$
${\mathit{a}}_k^B$, ${\mathit{w}}_k^B$	Acceleration and angular velocity of platform motion
$\mathit{g}$	gravity
${\mathit{b}}_{a_k}$, ${\mathit{b}}_{w_k}$, ${\mathit{b}}_a$, ${\mathit{b}}_w$	Biases and noise of accelerometer and gyroscope
$\Delta {\overline{\mathbf{R}}}_{ij}$, $\Delta {\overline{\mathbf{p}}}_{ij}$, $\Delta {\overline{\mathbf{v}}}_{ij}$	Preintegrated measurements for orientation, translation, and velocity
$r_{\Delta {\mathbf{R}}_{ij}}^{\top }$, $r_{\Delta {\mathbf{v}}_{ij}}^{\top }$, $r_{\Delta {\mathbf{p}}_{ij}}^{\top }$	Preintegration error
$\Delta {\mathbf{R}}_{ij}^O$, $\Delta {\mathbf{p}}_{ij}^O$, $\Delta {\mathbf{v}}_{ij}^O$	Prediction states from IMU
$E_{ij}^C$, $E_{ij}^L$	IMU measurement errors with VIO and LIO
${\mathrm{\Omega }}_{ij}^C$, ${\mathrm{\Omega }}_{ij}^L$	Uncertain matrix of LiDAR and camera poses
$E_m$	Error of marginalized prior
$e_i^{rp}$	Reprojection residual error of the visual feature $z_{ic}$
${\mathcal{C}}_{po}$	Constraint from the back-propagated LiDAR pose
${\mathbb{T}}^C$	Set of camera poses
$\mathcal{S}\left({\mathit{p}}_i\right)$	Smoothness of the LiDAR point ${\mathit{p}}_i$
$\parallel {\mathcal{r}}_i\parallel$	The range of the point ${\mathit{p}}_i$.
${\mathbb{F}}_n^e$, ${\mathbb{F}}_n^p$	Edge and plane points of n-th sweep
${\mathbb{F}}_n$	All feature points of n-th sweep
${\mathcal{U}}_n$	Clustering results of n-th sweep
${\mathit{V}}_i$	The weighted distance vector of the i-th sector
${\mathcal{C}}_n$	Environmental structuring of n-th sweep
${\check{\mathbf{T}}}_k^L$	Initial LiDAR pose from prediction state
${\mathcal{S}}^{{\mathbb{F}}_k}$	Weighted point-to-feature distance
$r_{pf}$	Feature registration error
$r_{pg}$	Point-to-Gaussian surface error
$\mathit{W}$, $\mathit{N}$, $\mathrm{\Lambda }$	Covariance matrix, eigenvector matrix, and diagonal matrix
${\mathbf{T}}_n^L$	LiDAR pose at time $t_n$
$\mathbf{R}\left({\gamma }_n\right)$	Rotation matrix on $yaw$ angle
$\tilde{\mathit{J}}$, $\tilde{\mathit{H}}$	Jacobian matrix and Hessian matrix by differentiating the projected point ${\tilde{\mathit{f}}}_j$ to the pose ${\widehat{\mathbf{T}}}_n^L$

.

2. Related Works

2.1. Sensor Fusion Localization System

Recently, significant efforts have been made in the field of sensor fusion localization. Nguyen proposes VIRAL-fusion [11], which combines an IMU, an ultra wideband ranging sensor, and multiple on-board visual-inertial and LiDAR-ranging subsystems to implement an optimization-based comprehensive state estimator on UAVs. This estimator effectively mitigates the problem of pose position drift and robustness in low-texture environments. LIC-fusion2 [12] introduces a novel sliding-window planar feature tracking technique based on online spatio-temporal calibration for efficient processing of 3D LiDAR point clouds. A novel outlier rejection criterion is proposed in planar feature tracking to initialize feature points belonging to the same plane for high-quality data correlation. Shao [13] proposes a integrated LiDAR-visual-inertial framework that conducts coupling optimization in a factor graph manner, enabling a refined system state. Lin [14] proposes a novel multi-sensor fusion framework, called R3LIVE, that leverages the measurement strengths of LIDAR, inertial, and visual sensors to enable real-time reconstruction of accurate, dense, 3D, RGB-colored maps of the surrounding environment. Zheng [15] proposes a fast LiDAR-inertial-visual odometry system based on two tightly coupled direct subsystems: a VIO subsystem and a LIO subsystem. The LIO subsystem registers new sweep points onto an incrementally constructed point cloud map. The points on the map are also appended with image patches, which are then used in the VIO subsystem to align the new image by minimizing photometric errors. The system combines the advantages of sparse direct image alignment and raw point direct alignment to achieve accurate and reliable attitude estimation with low computational cost.

The aforementioned systems enable accurate state estimation in structured environments. However, in unstructured scenes, the LiDAR module with a point-to-plane model cannot build a consistent surrounding map, and the overall system perhaps fails due to LiDAR pose drift or map divergence. To tackle this problem, some researchers have utilized the property of multi-sensor fusion, which involves discarding the output of the LiDAR module and using other sensors’ poses as the system state. Zhang [16] introduces LiDAR-visual-IMU odometry, which starts with IMU preintegration measurement and ends with refined poses in a visual-inertial subsystem and LiDAR-inertial subsystem. Although the system fails to greatly improve the state accuracy compared with the LiDAR subsystem, the robustness is enhanced due to the coupling of the visual subsystem as a complement to the state estimation. Wisth [17] proposes VILENS, a factor graph-based odometry system for legged robots. By tightly fusing LiDAR, cameras, IMUs, and leg odometry together, reliable operation is achieved despite the fact that individual sensors can produce degraded estimates. To minimize legged odometry drift, the system extends the robot’s state using a linear velocity deviation term, which is estimated online by preintegrating measurement with the visual, LiDAR, and IMU factors. The system exhibits excellent localization performance and strong robustness in unstructured environments. Based on the above study, we propose a method to compensate for LiDAR drift by combining short-term IMU predictions with the pre-drift LiDAR pose for state estimation. The proposed system achieves synchronization between LiDAR and camera by segmenting the LiDAR sweep, which improves the accuracy of short-term IMU predictions by boosting the correction frequencies. In addition, the undegraded LiDAR module is also used to constrain the pose estimation of the visual module to improve the localization accuracy of the visual sub-module. In the case of LiDAR failure, the visual-inertial module is employed to generate the final state.

2.2. Point Cloud Registration

The majority of existing work accomplishes point cloud alignment by sweep matching, which involves using iterative closest point (ICP) and normal distribution transform (NDT) algorithms to solve sweep-to-sweep or sweep-to-map transformations [18,19,20]. Point-to-point, point-to-edge, point-to-plane, and point-to-probability model techniques are among the geometric primitives that are employed in it. By fully considering the sparsity and scene complexity of LiDAR point clouds, Cui [21] provides a linear keypoint representation for 3D LiDAR point clouds, which minimizes keypoint-to-keypoint distance to efficiently perform sweep-to-sweep alignment. Zhang [22] distinguishes plane and edge points by calculating the local smoothness, and then minimizes the point-to-plane distance and point-to-edge distance to achieve accurate point cloud alignment. On this foundation, Guo [23] extracts edge and plane features by the principal component analysis (PCA) method, which is employed in two-stage alignment to achieve, without loss of real-time performance, improved odometry accuracy and consistent mapping.

Regularized planes are commonly used as geometric primitives for matching in the above methods. However, uneven surfaces are widely found in unstructured and undeveloped environments. Still, utilizing a planar model for point cloud matching will produce large random errors. Choi [24] presents a fast and generalized feature-based LiDAR odometry method using local quadratic surface approximation and point-to-surface alignment. Unlike most matching methods based on point-to-plane distances, the method approximates the local geometry of the LiDAR scan as a quadratic surface to minimize performance degradation due to the inconsistency of feature classes with the local geometry of the map. Chen [25] presents a lightweight front-end LiDAR odometry solution that uses a point-to-point probabilistic model in a generalized ICP-based direct point cloud matching method to yield accurate state estimation in unstructured subterranean environments. Combining these two approaches, this paper approximates an uneven surface as a Gaussian surface, which is formulated as a Gaussian probability function consisting of neighboring points. The point-to-Gaussian surface distance is employed to point cloud matching, achieving low-drift LiDAR odometry in unstructured scenes.

3. Problem Statement

The state variables are represented by the following in the proposed system:

$$\begin{array}{c}{\mathcal{X}}_n={\left\{{\mathit{x}}_i\right\}}_{i\in {\mathcal{T}}_n}={\left\{{\mathbf{R}}_i,{\mathbf{p}}_i,{\mathbf{v}}_i,{\mathbf{b}}_{w_i},{\mathbf{b}}_{a_i}\right\}}_{i\in {\mathcal{T}}_n}.\hfill \end{array}$$

(1)

where ${\mathcal{X}}_n$ represents the set of all states. ${\mathbf{R}}_i$, ${\mathbf{p}}_i$, and ${\mathbf{v}}_i$ denote the orientation, the translation, and the motion velocity of the UAV at moment $t_i$, respectively. ${\mathbf{b}}_{w_i}$ and ${\mathbf{b}}_{a_i}$ are the IMU biases.

The camera observations at time $t_i$ are represented as ${\mathbb{C}}_i$, which include the extracted feature point, ${\mathit{z}}_{ic}$, from the images. The measurement data from the LiDAR are represented as ${\mathbb{L}}_i$, which include the salient points, ${\mathit{z}}_{il}$, to be matched. IMU measurements between adjacent camera sampling moments $t_i$ and $t_j$ are represented as ${\mathbb{I}}_{ij}$. The UAV state is estimated by minimizing the sum of squared observation residuals, as follows:

$$\begin{array}{c}{\mathcal{X}}_n=\arg \underset{{\mathcal{X}}_n}{\min }\left(-lnP\left({\mathcal{X}}_n|{\mathcal{Z}}_n\right)\right)=\arg \underset{{\mathcal{X}}_n}{\min }{∥r_0∥}^2\hfill \\ +{\displaystyle \sum _{(i,j)\in {\mathcal{T}}_n}}{∥r_{{\mathbb{I}}_{ij}}∥}^2+{\displaystyle \sum _{i\in {\mathcal{T}}_n}\sum _{c\in {\mathbb{C}}_i}}{∥r_{{\mathit{z}}_{ic}}∥}^2+{\displaystyle \sum _{i\in {\mathcal{T}}_n}\sum _{l\in {\mathbb{L}}_i}}{∥r_{{\mathit{z}}_{il}}∥}^2.\hfill \end{array}$$

(2)

where $r_0$ is the prior error. ${r_{\mathbb{I}}}_{ij}$, $r_{{\mathit{z}}_{ic}}$, $r_{{\mathit{z}}_{il}}$ represent the residuals of the associated measurements. Residuals are functions of the state variables and observations, quantifying the mismatch between the observations and the estimated values under the current state and prior constraints.

4. Proposed Method

4.1. System Overview

The goal of our proposed system is to estimate the UAV state and construct the surrounding map. The intrinsic parameters of these sensors are assumed to be known. The extrinsic parameters between the three sensors have been calibrated to share a common coordinate system, with the IMU frame designated as the primary coordinate system. The camera and LiDAR frames are considered sub-coordinate systems. The definitions of these coordinate systems are illustrated in Figure 1. According to the figure, C, L, and B represent the coordinate systems of the camera, LiDAR, and IMU, respectively.

An overview of our system is illustrated in Figure 2. The IMU and visual modules provide prior poses and constraints to the LiDAR module, which is the main component of the system that uses motion estimation from coarse to fine. Initially, the camera input frequency reconstructs a LiDAR sweep to guarantee synchronized transmission between various sensors. Then, using cross-validation to assess the projection errors of visual feature-associated LiDAR points under various perspectives, the depth of visual features with large approximation errors and measurement noise is eliminated by outliers rejection. By utilizing temporal synchronization, the vision module receives the LiDAR pose from the previous instant, which helps to provide a position consistency constraint that enhances the accuracy of the camera pose estimation. To reduce the drift of the feature-based LiDAR module in scenes with fewer features, a direct point cloud registration method with IMU-constrained point-to-Gaussian surface error is proposed to be incorporated into the pose estimation.

4.2. Imu Module with High Frequency Correction

In this module, the LiDAR point cloud and camera image are synchronized to implement the transmission between the LiDAR module and the visual module. IMU measurements between two consecutive frames are integrated to align with the LiDAR point cloud and camera image.

4.2.1. Time Synchronization Based on Sweep Segmentation

Temporal interpolation techniques are used in most existing research on sensor time synchronization and are effective for matching data between sensors with noticeably differing frequencies [26]. However, when using sensors with resembling input frequencies (e.g., LiDAR and camera), errors can arise because of the large gaps between their sample periods.

To solve the problem of information lag caused by different sampling rates among sensors, a LiDAR sweep reconstruction algorithm controlled by camera input frequency is adopted to avoid ambiguous transmission between the LiDAR module and the visual module. Specifically, image acquisition time serves as the starting point for reconstructing the LiDAR sweep, which is motivated by the continuous sampling nature of LiDAR. This synchronization allows for simultaneous processing of camera images and LiDAR sweeps, avoiding interpolation or approximation operations during LiDAR and visual information fusion. This not only facilitates the transfer of enhanced depth and backward propagated poses from LiDAR to the camera but also provides higher frequency corrections to the IMU bias to improve the accuracy of IMU short-term predictions.

4.2.2. IMU Kinetic Model

Let the timestamps of two consecutive frames, $F_i$ and $F_j$, be denoted as $t_i$, $t_j$. The measurements of the accelerometer and gyroscope during the time interval are described as:

$$\begin{array}{c}{\widehat{\mathit{a}}}_k^B={\mathit{a}}_k^B-\mathit{g}+{\mathit{b}}_{a_k}+{\mathit{n}}_a,{\widehat{\mathit{w}}}_k^B={\mathit{w}}_k^B+{\mathit{b}}_{w_k}+{\mathit{n}}_w.\hfill \end{array}$$

(3)

where measurements between clock times $t_i$ and $t_j$ are denoted by the $k=1,2,\dots ,n$ index. Motion measurements include gravity, $\mathit{g}$, motivation, ${\mathit{a}}_k^B$, and angular velocity, ${\mathit{w}}_k^B$. These measurements are interfered with biases ${\mathit{b}}_{a_k}$, ${\mathit{b}}_{w_k}$ and measurement noise ${\mathit{n}}_a$, ${\mathit{n}}_w$.

In this work, a discrete-time IMU preintegration method is employed to obtain the relative motion within the time interval $\left[t_i,t_j\right]$ [27]. The preintegrated measurements for orientation, $\Delta {\overline{\mathbf{R}}}_{ij}$, translation, $\Delta {\overline{\mathbf{p}}}_{ij}$, and velocity, $\Delta {\overline{\mathbf{v}}}_{ij}$, in the IMU frame are given by:

$$\begin{array}{cc}\hfill \Delta {\overline{\mathbf{R}}}_{ij}& ={\mathbf{R}}_i^{\top }{\mathbf{R}}_j\doteq \prod _{k=i}^{j-1}exp\left[\left({\widehat{\mathit{w}}}_k-{\mathit{b}}_{w_i}\right)\delta t\right],\hfill \\ \hfill \Delta {\overline{\mathbf{v}}}_{ij}& ={\mathbf{R}}_i^{\top }\left({\mathbf{v}}_k-{\mathbf{v}}_i-\mathit{g}\delta t\right)\doteq \sum _{k=i}^{j-1}\Delta {\mathbf{R}}_{ik}\left({\widehat{\mathit{a}}}_k-{\mathit{b}}_{a_i}\right)\delta t,\hfill \\ \hfill \Delta {\overline{\mathbf{p}}}_{ij}& ={\mathbf{R}}_i^{\top }\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\delta t-{\displaystyle \frac{1}{2}}\mathit{g}{\delta t}^2\right)\doteq \sum _{k=i}^{j-1}\left(\Delta {\mathbf{v}}_{ik}\delta t+{\displaystyle \frac{1}{2}}\Delta {\mathbf{R}}_{ik}\left({\widehat{\mathit{a}}}_k-{\mathit{b}}_{a_i}\right){\delta t}^2\right).\hfill \end{array}$$

(4)

where $\delta t$ is the time interval between adjacent IMU measurements. The preintegration error in the IMU frame ${r_{\mathbb{I}}}_{ij}={\left[r_{{\Delta \mathbf{R}}_{ij}}^{\top },r_{\Delta {\mathbf{v}}_{ij}}^{\top },r_{\Delta {\mathbf{p}}_{ij}}^{\top }\right]}^{\top }$ is naturally converted from (6):

$$\begin{array}{cc}\hfill r_{{\Delta \mathbf{R}}_{ij}}^{\top }& =log\left(\Delta \overline{\mathbf{R}}\left(b_{w_i}\right)\right){\mathbf{R}}_i^{\top }{\mathbf{R}}_j,\hfill \\ \hfill r_{\Delta {\mathbf{v}}_{ij}}^{\top }& ={\mathbf{R}}_i^{\top }({\mathbf{v}}_j-{\mathbf{v}}_i-\mathit{g}\Delta t_{ij})-\Delta {\overline{\mathbf{v}}}_{ij}({\mathit{b}}_{w_i},{\mathit{b}}_{a_i}),\hfill \\ \hfill r_{\Delta {\mathbf{p}}_{ij}}^{\top }& ={\mathbf{R}}_i^{\top }({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-{\displaystyle \frac{1}{2}}\mathit{g}\Delta t_{ij}^2)-\Delta {\overline{\mathbf{p}}}_{ij}({\mathit{b}}_{w_i},{\mathit{b}}_{a_i}).\hfill \end{array}$$

(5)

The relative states of the other sensors are predicted by the IMU preintegration measurements within the time interval between consecutive frames. Superscript O represents the target sensor frame including the LiDAR frame and camera frame. Then, the prediction state can be expressed as follows:

$$\begin{array}{cc}\hfill \Delta {\mathbf{R}}_{ij}^O& ={\left({\mathbf{R}}_i^O\right)}^{\top }{\mathbf{R}}_j^B\doteq \Delta {\overline{\mathbf{R}}}_{ij}^B\hfill \\ \hfill \Delta {\mathbf{v}}_{ij}^O& ={\left({\mathbf{R}}_i^O\right)}^{\top }\left({\mathbf{v}}_j^O-{\mathbf{v}}_i^O-\mathit{g}\delta t\right)\doteq {\mathbf{R}}_B^O\Delta {\overline{\mathbf{v}}}_{ij}^B\hfill \\ \hfill \Delta {\mathbf{p}}_{ij}^O& ={\left({\mathbf{R}}_i^O\right)}^{\top }\left({\mathbf{p}}_j^O-{\mathbf{p}}_i^O-{\mathbf{v}}_i^O\delta t-{\displaystyle \frac{1}{2}}\mathit{g}{\delta t}^2\right)\doteq {\mathbf{R}}_B^O\Delta {\overline{\mathbf{p}}}_{ij}^B\hfill \end{array}$$

(6)

where ${\mathbf{R}}_B^O$ is the rotation matrix in the external parameter between the target frame and the IMU frame. The solved relative states are leveraged to the feature tracking process in the visual module and the distortion elimination process in the LiDAR module.

IMU odometry error is triggered by slowly varying random drift in the accelerometer bias, ${\mathit{b}}_{a_i}$, and gyroscope bias, ${\mathit{b}}_{w_i}$. IMU measurement biases are jointly corrected using the LiDAR and visual poses. Additionally, the system state is optimized using multiple IMU measurement residuals, which are equivalent to the errors $\left\{E_{ij}^C,E_{ij}^L\right\}$ between the IMU measurements and poses from the visual and LiDAR modules. The state estimation problem in Equation (2) is converted to minimize the IMU measurement residuals, as follows:

$$\begin{array}{c}{\mathcal{X}}_n=\arg \underset{{\mathcal{X}}_n}{\min }{\displaystyle \sum _{(i,j)\in {\mathcal{T}}_n}}{\left(E_{ij}^C\right)}^{\top }{\mathrm{\Omega }}_{ij}^C{E}_{ij}^C+{\left(E_{ij}^L\right)}^{\top }{\mathrm{\Omega }}_{ij}^L{E}_{ij}^L+E_m.\hfill \end{array}$$

(7)

where ${\mathrm{\Omega }}_{ij}$ is the uncertainty matrix of LiDAR and visual poses, which can be calculated based on the sensor measurement noise. $E_m$ is the marginalized prior, consisting of states and observations before the oldest state in the sliding window. The factor graph is shown in Figure 3.

4.3. Visual Module with Position Consistency Constraint

The visual-inertial module is implemented based on the LiDAR module assistance. In this module, the point reprojection method is designed to remove outliers of depth points. The position from LiDAR back-propagation is incorporated into visual-inertial optimization, achieving a refined camera pose.

4.3.1. Depth Association by Outliers Rejection

By utilizing external parameters between LiDAR and camera, the multiple LiDAR sweeps are projected on the image plane to generate depth map. Each feature depth is associated with three adjacent points on a unit sphere frame. The depth value is typically solved by spherical interpolation [28]. However, points with large incident angles of LiDAR will produce deviation. Therefore, the relative poses in the sliding window are leveraged to cross-cut evaluate the projection errors of depth points under different perspectives. In detail, the 3D landmark of the visual features ${\mathit{z}}_{ic}$ and ${\mathit{z}}_{jc}$ with depths $d_i$ and $d_j$ in frames $F_i$ and $F_j$ can be defined as ${\mathbf{P}}^C$. The reprojection residual error related to ${\mathbf{P}}^C$ between frames $F_i$ and $F_j$ in the sliding window can be written as:

$$\begin{array}{c}{r}_{\mathbb{C}}\left({\mathit{z}}_{ic}\right)=e_i^{rp}({\mathbf{P}}^C,{\mathit{x}}_i)=\left[\begin{array}{c}{b}_1\\ b_2\end{array}\right]\left({\mathbf{P}}^C-{\pi }_c\left({\mathit{z}}_{jc}\right)\right),\\ {\mathit{z}}_{jc}={\mathbf{R}}_B^C({\mathbf{R}}_W^{B_j}({\mathbf{R}}_{B_k}^W({\mathbf{R}}_C^B{d}_i{\pi }_c^{-1}\left({\mathit{z}}_{ic}\right)+{\mathbf{t}}_C^B)+{\mathbf{t}}_{B_k}^W)+{\mathbf{t}}_W^{B_j})-{\mathbf{t}}_B^C\end{array}$$

(8)

where ${\pi }_c^{-1}\left(·\right)$ is the back projection function. ${\mathbf{R}}_B^C$ and ${\mathbf{t}}_B^C$ are the rotation matrix and translation vector of the transformation between camera frame and IMU frame. ${\mathbf{R}}_B^W$ and ${\mathbf{t}}_B^W$ represent poses in the world frame. $b_1$ and $b_2$ are two orthogonal bases that span the tangent plane of ${\mathit{z}}_{jc}$.

The depth of projected points from different perspectives is used to quantify the depth association:

$$\begin{array}{c}{\tau }_i=\exp (-\beta ·e_i^{rp}({\mathbf{P}}^C,{\mathit{x}}_i)).\hfill \end{array}$$

(9)

where the corresponding projection points are considered outliers if $\tau$ is greater than the preset threshold. $\beta$ is a decay coefficient that has been manipulated. The average depth value of the interior projected points is calculated as the final depth value. In addition, the visual features will be removed if the number of depth measurements is below a certain threshold. The final features with modified depth are further utilized to update the visual map.

4.3.2. Motion Estimation Assisted by LiDAR Odometry Back-propagation

The aims of the visual module are improving the robustness of the overall system and providing an initial guess for the LiDAR module. In our work, the LiDAR point clouds and visual images are synchronized and consistent. To improve the localization accuracy of the visual odometry, the position consistency constraint is proposed by incorporating backward propagated poses from the LiDAR module into the sliding window optimization. The definition of the above constraint is as follows:

$$\begin{array}{c}{\mathit{C}}_{po}\left({\mathbf{T}}_k^C\right)={∥{\left[{\mathbf{T}}_{k-1}^L{\mathbf{T}}_L^C{\left({\mathbf{T}}_{k-1}^C\right)}^{-1}{\mathbf{T}}_k^C\right]}_{:t}-{\left({\mathbf{T}}_k^C\right)}_{:t}∥}^2\end{array}$$

(10)

where ${\mathit{C}}_{po}\left({\mathbf{T}}_k^C\right)$ ensures that the location of the camera and the transformed LiDAR, using the external parameters ${\mathbf{T}}_L^C$, are consistent at the k-th keyframe. ${\mathbf{T}}_{k-1}^L$ and ${\mathbf{T}}_{k-1}^C$ are the 6-DoF poses of LiDAR and the camera frame at the $k-1$-th keyframe, respectively. ${\left(\right)}_{:t}$ represents the translation vector of the transformation matrix.

For each newly acquired camera keyframe, motion estimation is implemented by jointly adjusting the camera poses ${\mathbb{T}}^C=({\mathbf{T}}_1^C,\cdots ,{\mathbf{T}}_n^C)$ and the 3D observations, $\mathbb{C}$. This process is formulated by minimizing the sum of the feature observation residual error, $r_{\mathbb{C}}\left({\mathit{z}}_{kc}\right)$, IMU preintegration error, $r_{\mathbb{I}}$, and constraint, ${\mathit{C}}_{po}$, as follows:

$$\begin{array}{c}{\left({\mathbf{T}}_k^C\right)}^{*}=\arg \underset{{\mathbf{T}}_k^C}{\min }\left\{r_{\mathbb{I}}++{\displaystyle \sum _{c\in {\mathbb{C}}_k}}{∥r_{\mathbb{C}}\left({\mathit{z}}_{kc}\right)∥}^2+{\mathit{C}}_{po}\right\}\hfill \end{array}$$

(11)

The IMU states and visual features of the regular frames and removed keyframes are marginalized as prior to constrain the sliding window optimization [29]. In addition, if the LiDAR module fails completely, the visual pose is output as the final state to ensure robustness.

4.4. Adaptive LiDAR Module with Hybrid Registration Mode

Existing feature-based LiDAR-inertial odometry may fail to work in geometrically uninformative environments, pushing optimization toward divergence along weakly constrained directions. In this paper, the LiDAR module adopts a hybrid registration method, which utilizes a feature-based approach to obtain the rough pose in high-level structured scenes and performs the point-to-surface matching algorithm under IMU constraints in unstructured scenes, achieving accurate and robust pose estimation.

4.4.1. Structure Monitor Based on Vertical Landmarks

After receiving a new sweep, the firstly performed feature extraction is employed to divide the original point cloud into edge points, ${\mathbb{F}}_n^e$, and plane points, ${\mathbb{F}}_n^p$, according to the smoothness of the local surface [30]. The set of total features is denoted as ${\mathbb{F}}_n$ at the n-th frame. The smoothness, $\mathcal{S}\left({\mathit{p}}_i\right)$, of the LiDAR point ${\mathit{p}}_i$ is calculated as follows:

$$\begin{array}{c}\mathcal{S}\left({\mathit{p}}_i\right)={\displaystyle \frac{1}{\left|{\mathcal{B}}_i\right|}}\parallel {\sum }_{j\in {\mathcal{B}}_i,j\ne i}({\mathcal{r}}_j-{\mathcal{r}}_i)\parallel ,r_b<\parallel {\mathcal{r}}_i\parallel \le r_m.\end{array}$$

(12)

where $\parallel {\mathcal{r}}_i\parallel$ represents the range of the point ${\mathit{p}}_i$. $r_b$ and $r_m$ are the blind distance and the maximum distance of the return points, respectively. ${\mathcal{B}}_i$ is the set of consecutive points of $p_i$ from the same scan. $\left|{\mathcal{B}}_i\right|$ is the number of points in ${\mathcal{B}}_i$.

The edge points are extracted if $\mathcal{S}\left({\mathit{p}}_i\right)$ greater than preset threshold, ${\mathcal{S}}_{th}$. Then, all edge points of the current sweep are projected onto the horizontal plane, which is divided into $N_s$ regions. The direction from the LiDAR origin to the edge points is used to allocate the edge points on the horizontal plane. Assume the positions of the edge features in the horizontal plane follow a Gaussian distribution. In each region, the 2D positions will be used as samples for local kernel density estimation, iteratively moving in the direction of increasing density. As a result, the sample points will eventually converge at the local maximum density, and the points that converge to the same local maximum are considered members of the same cluster. The final clustering result is the set of points in the region with the highest density of edge points. Figure 4 illustrates the aforementioned aggregation process. The $N_s$-dimensional vector is obtained by forming the results of all regions:

$$\begin{array}{c}{\mathcal{U}}_n=\left[{\eta }_1{\mathit{V}}_1,{\eta }_2{\mathit{V}}_2,\cdots ,{\eta }_{N_s}{\mathit{V}}_{N_s}\right].\end{array}$$

(13)

where ${\eta }_i$ is the normalized factor: ${\eta }_i=N_i/N_{tatal}$. $N_i$ represents the number of points in the i-th sector in the horizontal plane. $N_{tatal}$ is the number of all points in the horizontal plane. If $N_i<N_{th}$, ${\eta }_i$ is set to zero, where $N_{th}$ is the preset threshold of points number in one sector. The weighted distance vector of the i-th sector can be calculated by:

$$\begin{array}{c}{\mathit{V}}_i={\displaystyle \sum _{k=1}^{{\mathit{N}}_i}}\exp (-{\displaystyle \frac{{\mathit{d}}_k^h}{{\sigma }^2}}).\end{array}$$

(14)

where $N_i$ is the number of points in the i-th sector in the horizontal plane. ${\mathit{d}}_k^h$ is the horizontal range of the k-th point in the i-th sector. $\sigma$ is the attenuation factor for adjusting distance weights.

The environmental structure, ${\mathcal{C}}_n$, is then quantified by the dispersion of the $N_s$-dimensional vector. A greater degree of dispersion indicates a higher level of environmental structuring. The details are illustrated by:

$$\begin{array}{c}{\mathcal{C}}_n=var\left({\mathcal{U}}_n\right)=\frac{1}{N_s}{\displaystyle \sum _{i=1}^{N_s}}({\eta }_i{\mathit{V}}_i-{\displaystyle \frac{1}{N_s}}{\displaystyle \sum _{j=1}^{N_s}}{\eta }_j{\mathit{V}}_j).\end{array}$$

(15)

The threshold ${\mathcal{C}}_{th}$, used in this method to distinguish between environmental structures, can be empirically set to 5. The quantitative environmental structuring is used to adjust the LiDAR module’s operating mode.

4.4.2. Hybrid Point Cloud Alignment

The sparsity of LiDAR sweeps may lead to imprecise vertical constraints, especially in open and unstructured environments, failing to estimate the altitude variables $roll$, $pitch$, and z in point cloud alignment. To tackle these problems, we propose a hybrid point cloud alignment strategy that performs two modes according to the output of the structure monitor. In structured scenes, the point-to-feature model with distance weight is minimized to solve the LiDAR pose. In unstructured scenes, a novel point-to-surface model is generated to register the non-planar surface, achieving refined pose estimation. The point cloud alignment strategy is shown in Algorithm 1.

Algorithm 1 Hybrid Point Cloud Registration

1:   Input:  ${\mathbb{F}}_k,{\mathcal{C}}_k,{\mathbf{T}}_{k-1}^L,{\mathbf{M}}_{k-1}^l,\Delta {\mathbf{R}}_{(k-1,k)}^L,\Delta {\mathbf{p}}_{(k-1,k)}^L$ 2:   Output:  ${\mathbf{T}}_k^L$ 3:   While  ${\mathbb{F}}_k\ne \varnothing$  do 4:         ${\check{\mathbf{T}}}_k^L\leftarrow \mathrm{InitialGuess}\left({\mathbf{T}}_{k-1}^L,{\mathbf{T}}_{k-1}^C,\Delta {\mathbf{R}}_{(k-1,k)}^L,\Delta {\mathbf{p}}_{(k-1,k)}^L\right)$ 5:         if ${\mathcal{C}}_k>{\mathcal{C}}_{th}$ then 6:           $S^{{\mathbb{F}}_k}\leftarrow \mathrm{Point}2\mathrm{FeatureDistance}\left({\mathbb{F}}_k,{\mathbf{M}}_{k-1}^l\right)$ 7:           $r_{pf}\leftarrow \mathrm{WeightedRegistrationError}\left(S^{{\mathbb{F}}_k},w\left({\mathbf{M}}_{k-1}^l\right)\right)$ 8:           ${\mathbf{T}}_k^L\leftarrow \mathrm{LiDARPose}.\mathrm{minimize}\left(r_{pf}\right)$ 9:         else 10:           $r_{pg}\leftarrow \mathrm{Point}2\mathrm{GaussianError}\left({\mathbb{F}}_k,{\mathbf{M}}_{k-1}^l\right)$ 11:           $\lambda ({\mathbb{F}}_k({\check{\mathbf{T}}}_k^L))\leftarrow \mathrm{ResidualErrorTransform}\left(r_{pg}\right)$ 12:           ${\mathbf{T}}_k^L\leftarrow \mathrm{LiDARPose}.\mathrm{minimize}(\lambda ({\mathbb{F}}_k({\check{\mathbf{T}}}_k^L)))$ 13:       end if 14:       return ${\mathbf{T}}_k^L$ 15: end

If ${\mathcal{C}}_n>{\mathcal{C}}_{th}$, the n-th sweep is considered structured. In this case, the j-th feature, ${\mathit{f}}_j^L$, in ${\mathbb{F}}_n$ is transformed from the m-th map point, ${\mathbf{P}}_m^G$, in the global frame by the pose ${\mathbf{T}}_n^L$, where ${\mathbf{T}}_n^L=({\mathbf{R}}_n^L,{\mathbf{t}}_n^L)$. The feature in the global frame is defined by:

$$\begin{array}{c}{\mathbf{P}}_m^G={\mathbf{R}}_n^L{\mathit{f}}_j^L+{\mathbf{t}}_n^L.\hfill \end{array}$$

(16)

According to the widely used ICP method [31,32], the rigid transformation, $\mathbf{T}$, between the prior map, ${\mathbf{M}}^l$, and the feature cloud, ${\mathbb{F}}_n$, at the n-th frame can be solved by minimizing the feature registration error, $r_{pf}\left({\mathbf{P}}_m^G\right)$, including the weighted point-to-feature distance $S^{{\mathbb{F}}_i}({\mathit{f}}_j,{\mathbf{P}}_m^G)$:

$$\begin{array}{c}{r}_{\mathbb{L}}\left({\mathit{z}}_{il}\right)=r_{pf}={\displaystyle \frac{{\sum }_{{\mathit{f}}_j^L\in {\mathbb{F}}_i}w\left({\mathbf{P}}_m^G\right)·S^{{\mathbb{F}}_i}({\mathit{f}}_j^L,{\mathbf{P}}_m^G)}{{\sum }_{{\mathit{f}}_j^L\in {\mathbb{F}}_i}w\left({\mathbf{P}}_m^G\right)}},\\ S^{{\mathbb{F}}_i}({\mathit{f}}_j^L,{\mathbf{P}}_m^G)={\mathbf{n}}_m^{\top }\left({\mathit{f}}_j^L-{\mathbf{P}}_m^G\right)+{\mathbf{l}}_m^{\wedge }\left({\mathit{f}}_j^L-{\mathbf{P}}_m^G\right).\end{array}$$

(17)

where $w\left({\mathbf{P}}_m^G\right)=e^{-{∥{\mathbf{P}}_m^G-c∥}^2/{\sigma }^2}$ is the weight, which declines as distance is gained. The parameter $\sigma$ is chosen to exclude features that are more than $3\sigma$ distances from the feature center, c, on the local map. $S^{{\mathbb{F}}_i}({\mathit{f}}_m^L,{\mathbf{P}}_m^L)$ is the distance function, where ${\mathbf{n}}_m$ and ${\mathbf{l}}_m$ represent dominant vectors of the corresponding feature.

Typically, point cloud alignment with point-to-feature error yields consistent matching results. However, in unstructured environments, edge points are insufficient to provide adequate constraints, and the numerous surfaces cannot be accurately represented by regular planes. The point-to-Gaussian distance is modeled to provide a more generalized representation with Gaussian mean and uncertainty. Figure 5 shows these two models in different scenes.

Let the neighbor region of ${\mathbf{P}}_m^G$ include a set of LiDAR points ${\mathit{q}}_i(i=1,\cdots ,M)$. ${\mathbf{P}}_m^G$ has an uncertainty, ${\Sigma }_{{\mathbf{P}}_m^G}$, due to LiDAR measurement noise and position estimation errors. The uncertainty model of an irregular surface is illustrated in Figure 5b. The Gaussian mean of ${\mathbf{P}}_m^G$ is set to its 3D position. The uncertainty is the inverse of the covariance, $\mathit{W}$, which is calculated from the neighbor points and is expressed as $\mathit{W}=1/M{\sum }_{i=1}^M{({\mathit{q}}_i-{\mathbf{P}}_m^G)}^{\top }({\mathit{q}}_i-{\mathbf{P}}_m^G)$. The cost function of the point-to-Gaussian surface is described as follows:

$$\begin{array}{c}{\left({\mathbf{T}}_n^L\right)}^{*}=\arg \underset{{\mathbf{T}}_n^L}{\min }{r}_{pg}({\mathbf{M}}^l,{\mathbb{F}}_n,{\mathbf{T}}_n^L)=\arg \underset{{\mathbf{T}}_n^L}{\min }{\displaystyle \sum _{j=1}}{\left(e^{\top }{\mathit{W}}^{-1}e\right)}_j\end{array}$$

(18)

where the Mahalanobis distance between the target frame point and the corresponding Gaussian surface point is minimized rather than the Euclidean distance.

The inverse matrix of covariance ${\mathit{W}}^{-1}$ is composed of an eigenvector matrix, $\mathit{N}$, and a diagonal matrix, $\mathrm{\Lambda }$, formed by eigenvalues. So, we can calculate the point-to-surface error by the decomposed $W^{-1}$:

$$\begin{array}{c}{r}_{pg}({\mathbf{M}}^l,{\mathbb{F}}_n,{\mathbf{T}}_n^L)={\displaystyle \sum _{j=1}}\left(e_j^{\top }\mathit{N}\mathrm{\Lambda }{\mathit{N}}^{\top }{e}_j\right)\hfill \\ ={\displaystyle \sum _{j=1}}{e}_j^{\top }[v_1,v_2,v_3]\mathrm{diag}({\lambda }_1,{\lambda }_2,{\lambda }_3){[v_1,v_2,v_3]}^{\top }{e}_j\hfill \\ ={\displaystyle \sum _{j=1}}{\lambda }_1\left(e_j^{\top }{v}_1{v}_1^{\top }{e}_j\right)+{\lambda }_2\left(e_j^{\top }{v}_2{v}_2^{\top }{e}_j\right)+{\lambda }_3\left(e_j^{\top }{v}_3{v}_3^{\top }{e}_j\right).\hfill \end{array}$$

(19)

where ${\lambda }_1,{\lambda }_2,{\lambda }_3$ are descending eigenvalues of ${\mathit{W}}^{-1}$, and $v_1,v_2,v_3$ are the corresponding eigenvectors. Then, the standard least squares definition of the point-to-surface cost function can be obtained:

$$\begin{array}{c}{r}_{pg}({\mathbf{M}}^l,{\mathbb{F}}_n,{\mathbf{T}}_n^L)={\displaystyle \sum _{k=1}\sum _{i=1}}{\lambda }_k\left({\tilde{\mathit{f}}}_j\left({\check{\mathbf{T}}}_n^L\right)\right)={\displaystyle \sum _{k=1}\sum _{j=1}}{\lambda }_k{∥v_k^{\top }({\check{\mathbf{R}}}_n^L{\mathit{f}}_j^L+{\check{\mathbf{t}}}_n^L-{\mathbf{P}}_m^G)∥}_2^2\hfill \end{array}$$

(20)

where ${\check{\mathbf{R}}}_n^L$ and ${\check{\mathbf{t}}}_n^L$ are the rotation and translation part of the initial transformation, ${\check{\mathbf{T}}}_n^L$. The initial pose, ${\check{\mathbf{T}}}_n^L$, can be jointly calculated by the previous pose, ${\mathbf{T}}_{n-1}^L$, and the prediction state $\left\{\Delta {\mathbf{R}}_{(n-1,n)}^L,\Delta {\mathbf{p}}_{(n-1,n)}^L\right\}$ from IMU. ${\tilde{\mathit{f}}}_j$ is the transformed point from ${\mathbb{F}}_n$ by ${\check{\mathbf{T}}}_n^L$. Further, the second-order Taylor expansion of the eigenvalue matrix ${\lambda }_k({\tilde{\mathit{f}}}_j({\check{\mathbf{T}}}_n^L))$ is denoted by:

$$\begin{array}{c}{\lambda }_k\left({\tilde{\mathit{f}}}_j\left({\check{\mathbf{T}}}_n^L\right)\right)\approx {\lambda }_k\left({\tilde{\mathit{f}}}_j\right)+\mathit{J}\left({\tilde{\mathit{f}}}_j\right)\delta \mathit{f}+{\displaystyle \frac{1}{2}}\delta {\tilde{\mathit{f}}}^{\top }H\left({\tilde{\mathit{f}}}_j\right)\delta \mathit{f}\hfill \end{array}$$

(21)

where $\mathit{J}\left({\tilde{f}}_j\right)$ and $\mathit{H}\left({\tilde{\mathit{f}}}_j\right)$ are the Jacobian matrix and the Hessian matrix of ${\lambda }_k\left({\tilde{\mathit{f}}}_j\left(\check{\mathbf{T}}\right)\right)$, respectively.

Additionally, due to the insufficient vertical constraints in unstructured scenes, the point-to-surface model is modified for 3-DoF state estimation, that is, only the three horizontal DoF poses will be estimated. In this case, projecting the corresponding pose ${\mathbf{T}}_n^L$ of ${\tilde{\mathit{f}}}_j$ on the tangent plane, we have:

$$\begin{array}{c}{\mathbf{T}}_n^L={\check{\mathbf{T}}}_n\oplus \delta \mathbf{T}=\left(\mathbf{R}\left({\gamma }_n\right)\exp \left({\delta \gamma }_n^{\wedge }\right),{\mathbf{t}}_n{|}_{x,y}+\delta \mathbf{t}\right)\\ \mathbf{R}\left({\gamma }_n\right)=\left[\begin{array}{ccc}cos{\gamma }_n& -sin{\gamma }_n& 0\\ sin{\gamma }_n& cos{\gamma }_n& 0\\ 0& 0& 1\end{array}\right]\approx \mathbf{I}+\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]{\gamma }_n\\ {\tilde{\mathit{f}}}_j=\mathbf{R}\left({\gamma }_n\right)\exp \left({\delta \gamma }_n^{\wedge }\right){\mathit{f}}_j^L+{\mathbf{t}}_n{|}_{x,y}+\delta \mathbf{t}\end{array}$$

(22)

By differentiating the projected point ${\tilde{\mathit{f}}}_j$ with respect to pose ${\widehat{\mathbf{T}}}_n^L$, we can obtain:

$$\begin{array}{c}\lambda ({\check{\mathbf{T}}}_n^L\oplus \delta \mathbf{T})\approx \lambda \left({\check{\mathbf{T}}}_n^L\right)+\underset{\tilde{\mathit{J}}}{\underbrace{\mathit{J}\mathcal{A}}}\delta \mathbf{T}+{\displaystyle \frac{1}{2}}\delta {\mathbf{T}}^{\top }\underset{\tilde{\mathit{H}}}{\underbrace{{\mathcal{A}}^{\top }\mathit{H}\mathcal{A}}}\delta \mathbf{T}\\ \mathcal{A}={\displaystyle \frac{\delta {\tilde{\mathit{f}}}_j}{\delta \mathbf{T}}}=\left[\begin{array}{cc}-{\left({\mathit{f}}_j^L\right)}^{\wedge }-\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]{\gamma }_n{\left({\mathit{f}}_j^L\right)}^{\wedge }& I\end{array}\right]\end{array}$$

(23)

The objective function of point cloud registration can be formulated by:

$$\begin{array}{c}(\tilde{\mathit{H}}\left({\mathbf{T}}_n^L\right)+\mu \mathbf{I})\delta {\mathbf{T}}^{*}=-\tilde{\mathit{J}}{\left({\mathbf{T}}_n^L\right)}^{\top }.\end{array}$$

(24)

where $\mu$ is employed to adjust the iterative process. Finally, we minimize the cost function to refine pose ${\mathbf{T}}_n^L$ by repeatedly calculating its second-order derivative using the Levenberg–Marquardt (LM) approach.

5. Experimental Results

The full system’s tests are conducted on an Intel Core i7-10700K CPU with 16 GB RAM. In this section, the accuracy and robustness of the proposed system are evaluated in off-line mode, and the runtime evaluation demonstrates that the proposed system can operate in real time. The UAV localization accuracy is tested between the proposed algorithm and the advanced algorithms in highly structured scenes. In robust testing, trajectory comparisons for structurally weaker scenes with two advanced LiDAR-visual-IMU odometry methods is performed. A public dataset and a private dataset are employed as test sets. The NTU-VIRAL dataset is collected by equipping the UAV with sensors such as 3D rotating LiDARs, global shutter cameras, IMUs, and ultra wideband ranging units [33]. This dataset records multiple sequences under several challenging indoor and outdoor conditions. In this experiment, nine outdoor sequences from NTU-VIRAL are used for localization accuracy evaluation. In addition, we acquire a campus sequence of the low-altitude environment for testing location accuracy, and construct a high-precision 3D map. Three structurally different outdoor scenes are captured using LiDAR, a pinhole camera, and IMU: grove, beach, and desert. The platform and the private dataset are shown in Figure 6.

5.1. Structure Monitor Evaluation

In this section, a vertical observation-based structural monitor is employed to quantify the structure of different scenarios. The sequences from both public and private datasets are quantified separately, as shown in

Table 2. Quantitative structuring on public and private datasets.

Data	rtp1	rtp2	rtp3	sbs1	sbs2	sbs3	tnp1	tnp2	tnp3	Camp	Grove	Beach	Desert
Result	5.21	5.53	5.39	6.03	5.86	5.15	6.84	6.67	5.51	6.30	4.13	2.26	0.04

. A data sequence is defined, structured if its quantitative result exceeds 5; otherwise, it is regarded as unstructured. As shown in Table 2, the quantitative results indicate that all sequences in the public dataset and the campus sequence in the private dataset were collected in structured environments, whereas the grove, beach, and desert sequences reflect unstructured environments. The quantitative results of the unstructured scenes are show in Figure 7. In the structured environments, localization accuracy is the primary concern, while, in unstructured scenes, the focus shifts to ensuring stable system operation.

5.2. System Localization Accuracy Evaluation

The evaluation results of localization accuracy are shown in

Table 3. Localization error on public and private datasets (UNIT: Meter).

Data	LIO-SAM	LVI-SAM	FAST-LIO2	FAST-LIVO	mVIL-Fusion	Ours
rtp1	${\underline{0.242}}^1$	${\mathrm{X}}^2$	${\mathbf{0}.\mathbf{148}}^3$	0.674	0.954	0.265
rtp2	0.177	X	0.195	0.861	0.673	0.201
rtp3	0.385	0.204	0.195	0.283	0.426	0.141
sbs1	0.214	0.215	0.223	0.351	0.213	0.114
sbs2	0.208	0.208	0.213	0.232	0.225	0.200
sbs3	0.179	X	0.210	0.210	0.193	0.175
tnp1	0.193	0.134	0.146	0.202	0.269	0.214
tnp2	0.192	0.180	0.169	0.124	0.229	0.168
tnp3	0.176	0.479	0.181	0.165	0.223	0.109
campus	0.466	1.641	0.457	0.395	${-}^4$	0.312
grove	5.237	6.318	5.047	5.215	-	4.880
beach	0.453	6.449	1.171	2.159	-	0.241
desert	X	X	X	X	-	10.87

¹ underlined numbers are the sub-optimal results. ² X means that the system failed to complete the full localization. ³ Bolded numbers are the best ones. ⁴ - means that the system is not applicable.

. The localization error is dictated by the root mean square error (RMSE) of translation estimation. As shown in Table 2, the proposed method has higher localization accuracy than state-of-the-art LiDAR-visual-IMU odometry in structured scenes such as rtp, sbs, and tnp data sequences. In the NTU-VIRAL public dataset, the proposed system can maintain near-optimal localization performance in most data sequence tests. However, its performance in the rtp data sequence is slightly inferior to that of the LiDAR-IMU system. This is because strong lighting variations interfere with the visual module’s localization accuracy, thus increasing the translation error. Additionally, the LiDAR module is not affected by sensor degradation in the rtp data sequence. Therefore, LiDAR-inertial odometry can obtain better location results than LiDAR-visual-inertial odometry with optical noise interference. Unlike other tightly coupled sensor fusion localization schemes such as FAST-LIVO and mVIL-Fusion [34], the proposed system uses the output of the visual module only as the initial value for the next pose estimation stage, making it less dependent on the visual module. Even if the visual module fails, the proposed system can still deliver accurate positioning. The real-time mapping results of rtp, sbs, and tnp are shown in Figure 8; the proposed system provides structured point clouds and signs without distortion to exhibit low-drift localization performance. The 3D point cloud map of the campus sequence in the private dataset is shown in Figure 9, where static objects can all be recognized clearly. In addition, Figure 10 illustrates the positioning error of the proposed system in the publicly available NTU-VIRAL dataset, with a focus on annotating the minimum error, average error, and maximum error between the proposed system and the ground truth trajectories. Overall, in testing with the public dataset, our proposed system reduced localization errors by 25.1%, 29.5%, 15.7%, 50.0%, and 55.8% compared with LIO-SAM, LVI-SAM, FAST-LIO2, FAST-LIVO, and mVIL Fusion, respectively, after excluding the failure and mismatch of each system.

5.3. System Robustness Evaluation

In this section, hierarchical unstructured scenarios are employed to test the localization accuracy of the proposed system. The localization error is shown in Table 2. The difficulty of grove, beach, and desert sequences increases progressively. The grove scene is the most structured, with minimal brightness variation, making the environment relatively easier. However, due to the grove sequence covering a distance of $983.5\mathrm{m}$, the cumulative error is relatively large. The beach scene covers only $137.3\mathrm{m}$ and, like the $rtp$ sequence in the public dataset, has strong visual interference. Due to its low level of structuring, the positioning error is larger compared with the $rtp$ sequence. The total distance of the desert sequence is $988.3\mathrm{m}$, with minimal changes in luminosity. However, the degree of structuring is extremely low, and there is almost no vertical observation. As a result, pose estimation can only rely on ground point clouds, leading to the complete failure of feature-based LiDAR odometry methods. Despite this, our method with point-to-surface registration can still maintain high accuracy in this scenario, ensuring that system localization does not fail. Figure 11 shows the positioning trajectory of the proposed system compared with FAST-LIVO and LVI-SAM in unstructured scenarios. Compared with these two state-of-the-art methods, the proposed system is closer to the ground truth and has relatively smaller positioning errors.

5.4. Runtime Evaluation

The runtime of the proposed method is evaluated in the grove sequence. As shown in

Table 4. Time consumption on each component (UNIT: MILLISECOND).

Component		Median	Mean	Std
Visual module	$\mathrm{Feature}\mathrm{tracking}$	17.11	17.26	5.16
	Feature management	17.22	18.31	4.39
	Pose estimation	13.19	12.53	5.33
LiDAR module	Preprocessing	9.49	11.61	3.92
	Feature extraction	1.15	1.29	1.72
	Structure monitor	0.15	1.56	3.67
	Hybrid point registration	22.55	24.71	17.43
IMU module	Preintegration	0.143	0.151	0.360
	Factor graph optimization	0.280	0.274	0.311

, the time consumption of the proposed system is divided into the visual module, LiDAR module, and IMU module. The visual module includes the extraction and matching of visual features, feature management, and data alignment, as well as image-based pose solving. The LiDAR module includes preprocessing, feature extraction, structure monitor, and hybrid point cloud tracking. The IMU module has an IMU measurement preintegration section and factor graph optimization section. These three modules run in parallel mode. According to the results in Table 3, the average running time of the vision module, LiDAR module, and IMU module is 48.1 $\mathrm{ms}$, 39.17 $\mathrm{ms}$, and 0.425 $\mathrm{ms}$, respectively, which is lower than the acquisition time of the camera and LiDAR. Overall, the experimental results demonstrate that the proposed system can operate in real time.

6. Conclusions

This article presents a sensor fusion system coupling LiDAR, cameras, and IMUs to reduce trajectory error and ensure robust operation of UAVs. The visual module is supported by LiDAR measurements and pose to estimate feature depth and visual pose. The estimated feature depth is validated by calculating the reprojection error using corresponding 3D observations within a sliding window in the visual module, which helps to remove outliers. Additionally, the LiDAR pose is back-propagated to constrain visual pose estimation, achieving an enhanced camera pose of the UAV. The LiDAR module employs a structure monitor to switch matching modes in various environments. In unstructured environments, we use Gaussian probability-based uncertainty to model irregular surfaces. This uncertainty is then decoupled into eigenvalues and eigenvectors, and a pose estimation objective function is constructed to achieve accurate localization. Finally, the IMU measurement errors with the LiDAR and visual modules are used to construct the odometry factor, which is incorporated into the factor graph optimization to complete the state solution of the UAV. Experimental results with UAVs demonstrate that the proposed system outperforms state-of-the-art algorithms by averagely reducing localization errors by at least 15.7%. In unstructured scenarios, the algorithm proposed in this paper leads the other control algorithms by at least 12.6%, effectively improving the localization accuracy of UAVs in unstructured environments. The system runs about 48.525 $\mathrm{ms}$ per frame, which meets the task requirements for real-time work. In future research, we will explore a sensor fusion architecture on UAVs that integrates satellite navigation and full-domain place recognition for long-term localization, aiming to maintain system accuracy and robustness.