NI-LIO: A Hybrid Approach Combining ICP and NDT for Improving Simultaneous Localization and Mapping Performance

Abstract

The accuracy and stability of front-end point cloud registration algorithms are crucial for the mapping and localization precision in laser SLAM (simultaneous localization and mapping) systems. Traditional point-to-line and point-to-plane iterative closest point (ICP) registration algorithms, widely used in SLAM front ends, often suffer from low efficiency, significant data dependency during the matching process, and a propensity for local optima. This registration method exhibits a more pronounced local optimum issue in large-scale SLAM mapping, thereby diminishing matching accuracy and increasing reliance on initial values. To address these limitations, this paper introduces NI-LIO, a novel SLAM algorithm that integrates ICP with normal distributions transform (NDT) to enhance localization accuracy, computational efficiency and robustness. By combining the precision of ICP with the robustness of NDT, the proposed algorithm significantly improves system stability and localization accuracy. The analysis of mapping and localization experiments indicates a significant reduction in errors compared to traditional SLAM algorithms, with experiments showing a REMS value decrease of over 20%. Compared to ALOAM, FAST_LIO2 and Lego-LOAM algorithms, the new NI-LIO algorithm shows improvements in both accuracy and stability, enabling the construction of a more precise and consistent global map. This algorithm exhibits excellent adaptability to various environments.

1. Introduction

Simultaneous localization and mapping (SLAM) technology is essential for addressing the challenges of robot localization and map construction in unknown environments [1], enabling robots to perform tasks such as path planning, autonomous exploration, and navigation [2]. SLAM achieves this by utilizing sensor data from cameras, LiDAR, and IMUs [3] to perceive self-pose and incrementally build environmental maps based on changes in self-pose [4]. In recent years, the rapid advancement in LiDAR technology has significantly benefited SLAM applications due to its high measurement accuracy, extensive range, and insensitivity to lighting conditions, making it a primary sensor for mobile robots [5]. Consequently, improving the performance of LiDAR-based SLAM, including localization accuracy, mapping precision, and robustness, has become a major research focus [6]. The core components of laser SLAM, as illustrated in Figure 1, include sensor data retrieval, front-end odometry, back-end optimization, loop closure detection, and map construction. Front-end odometry employs point cloud registration [7] for motion estimation, while back-end optimization addresses accumulated errors using front-end estimates and loop closure detection [8]. Loop closure detection identifies matching laser frames from different times within the same environment, providing spatial constraints to eliminate errors [9].

The overall map is constructed based on the spatial maps generated from motion trajectories, which are obtained through both the front-end and back-end processing stages. Among these components, the accuracy, and robustness of front-end odometry point cloud registration and back-end nonlinear optimization are critical for the overall performance of laser SLAM algorithms.

Point cloud registration, a fundamental process in laser SLAM, involves solving coordinate transformation relationships to align two or more continuous frames of laser point clouds into the same coordinate system (scan-to-scan) or to align the current scan point cloud with an established map (scan-to-map) [10]. This alignment is crucial for recovering changes in the robot’s position and orientation [11]. Prominent algorithms for point cloud registration include the iterative closest point (ICP) algorithm, the normal distributions transform (NDT) algorithm, and so on. The ICP algorithm, widely used in front-end point cloud registration modules, focuses on determining relative pose transformation by minimizing point cloud distances [12]. The NDT algorithm, based on probability distribution, offers high reliability, efficiency, and a broad convergence range [9].

In a complete SLAM system, the LiDAR odometry and mapping in real-time(LOAM) algorithm, proposed by Zhang J. et al. [13], relies on feature point cloud matching for pose calculation, achieving good accuracy and real-time performance through a combination of high-frequency low-precision and low-frequency high-precision calculations. However, it lacks loop closure detection. To address this, Shan T. et al. introduced the Lego-LOAM algorithm, optimizing LOAM’s computation efficiency for faster pose calculation but with a slight decrease in accuracy [14]. Due to the LOAM algorithm’s reliance solely on 3D LiDAR for SLAM mapping, or its use of loosely coupled LIO to merge IMU and 3D LiDAR data, LIO_mapping [15] introduced a novel tightly coupled integration approach that unifies IMU and LiDAR data. Tests conducted on this data fusion indicate that, despite substantial degradation in LiDAR data accuracy, significant drift was avoided. This tightly coupled methodology markedly enhances system precision compared to prior loosely coupled structures. Subsequently, the authors of Lego-SLAM innovated a graph optimization framework, on which they implemented a tight coupling of LiDAR and IMU, leading to the development of the LIO-SAM [16] algorithm. This algorithm utilizes a factor graph approach for back-end optimization rather than traditional filtering methods. The advantages of tight coupling have led to a proliferation of such algorithms in recent years; for instance, Xu W et al. [17] released the FAST-LIO algorithm, which offers higher real-time performance than LIO-SAM and enables rapid optimization via GPU. However, conventional tightly coupled algorithms like LIO-SAM and FAST-LIO employ an indirect method to process raw LiDAR point cloud data, necessitating feature extraction steps for registration. This feature extraction phase can inhibit the utilization of fine environmental details and may diminish compatibility with new LiDAR systems. Consequently, FAST-LIO2 [18] was developed, refining the original kd-Tree into an ikd-Tree, thereby enhancing the algorithm’s accuracy and stability. Following the launch of FAST-LIO, C. Bai et al. [19] updated the ikd-Tree data structure to ivox, utilizing VECTOR or PHC structures for storage and further improving algorithm efficiency.

The accuracy of the point cloud registration algorithm significantly impacts the trajectory precision, localization accuracy, and map construction accuracy of the entire SLAM system. As a result, enhancing point cloud registration algorithms has become a focal point of current research. Prevalent methodologies such as the FAST-LIO2 algorithm employ tightly coupled LIO approaches for SLAM systems, which, while offering higher precision, also involve considerable data processing that can hinder algorithm operational efficiency and show excessive reliance on multi-sensor data like IMUs. Conversely, loosely coupled algorithms have been relatively underexplored in recent years. To contribute to this area of research, I propose improvements based on the Lego-LOAM algorithm.

To enhance localization accuracy, computational efficiency, and robustness of point cloud registration, this study proposes a novel SLAM algorithm, NI-LIO. By integrating the robust NDT algorithm and the high-precision ICP algorithm, NI-LIO addresses the computational efficiency and accuracy dependence on initial values observed in the Lego-LOAM algorithm [20]. The proposed method employs coarse registration using NDT in the front-end odometry (scan-to-scan) phase of Lego-LOAM, followed by precise registration using ICP. This hybrid approach overcomes the limitations of single nearest point matching in Lego-LOAM, leading to improved overall performance in SLAM applications.

The key contributions of this work include:

• Hybrid registration method: By integrating NDT for coarse registration and ICP for fine-tuning, NI-LIO effectively mitigates the limitations of single-method approaches, such as sensitivity to initial conditions and convergence to local optima;
• Improved performance metrics: Experimental results showed significant improvements in localization accuracy and computational efficiency, validating the effectiveness of the proposed approach;
• Robustness across conditions: The algorithm’s robustness was proven by its consistent performance across varying environmental conditions and long sequences.

The rest of this paper is organized as follows: Section 2 describes relevant algorithms, Section 3 outlines the methodology, Section 4 presents the experimental results and analysis, and Section 5 concludes the paper.

2. Description of Relevant Algorithms

The novel NI-LIO algorithm integrates the iterative closest point (ICP) and normal distributions transform (NDT) algorithms for point cloud registration, necessitating a prior analysis of their fundamental processes and principles. Understanding the core mechanics of ICP and NDT is crucial for appreciating the enhancements introduced by NI-LIO in SLAM performance. Due to the existence of three methods for point cloud registration in ICP, a brief introduction to each method follows.

2.1. Point-to-Point ICP Point Cloud Registration Algorithm

The ICP algorithm is a widely used method for point cloud registration due to its simplicity and precision [21]. It iteratively minimizes the distance between corresponding points in two-point clouds to determine the optimal transformation (rotation and translation) that aligns them [22]. The basic steps of ICP involve:

Step 1. Initialization: Starting with an initial guess for the transformation.

Step 2. Correspondence matching: Finding the closest points between the source and target point clouds.

Step 3. Transformation estimation: Computing the transformation that minimizes the sum of squared distances between corresponding points.

Step 4. Transformation application: Applying the estimated transformation to the source point cloud.

Iteration: Repeating the correspondence matching and transformation estimation steps until convergence criteria are met.

Figure 2 shows the conceptual diagram of the ICP algorithm, which can intuitively obtain the registration method and registration principle.

Despite its precision, ICP has several limitations, including a dependence on the initial guess, sensitivity to outliers, and a tendency to converge to local optima. ICP estimates corresponding points using the nearest neighbor method, as shown in Figure 2. The essence of ICP involves a spatial transformation between two corresponding sets of point cloud data, denoted as $P$ and $Q$, where a Euclidean transformation minimizes the distance between the two-point cloud models.

Two-point sets are selected from the overlapping region of the two-point cloud data models for registration, representing the source point set and the target point set. Here, $P={\{p}_1,p_2,\dots ,p_n\}$ is chosen as the source point set, and $Q=\{q_1,q_2,\dots ,q_n\}$ as the target point set, where n represents the number of points in the point cloud. Introducing the rotation matrix $R$ and translation matrix $t$, $\mathrm{E}\left(R,t\right)$ represents the error between the source point set $P$ transformed by the transformation matrix $\left(R,t\right)$ and the target point set $Q$, reflecting the degree of difference between $P$ and $Q$. Consequently, the optimal solution sought for point cloud registration corresponds to the minimum value of $\mathrm{E}\left(R,t\right)$ and its respective $\left(R,t\right)$.

The ICP algorithm is iteratively calculated based on the least squares method, starting with computing the closest corresponding point in $Q$ for each point in the source point set $P$, then solving for the optimal $R$ and $t$ corresponding to the minimum value of $\mathrm{E}\left(R,t\right)$, using $R$ and $t$ to obtain a new transformed point set $P^{\prime }$, repeating the iteration until the set $P^{\prime }$ and $Q$ meets the requirements of the objective function to stop the calculation. Assuming any point $P_j$ is selected from $P$, and there exists a point $Q_j$ in $Q$ with the shortest Euclidean distance from $P_j$, the optimization objective function can be expressed as:

$$\mathrm{E}\left(R,t\right)=\underset{R,t}{\mathrm{a}\mathrm{r}\mathrm{g}\mathrm{m}\mathrm{i}\mathrm{n}}j={\displaystyle \frac{1}{N}}{\sum }_{j=1}^N{‖\left(R{P}_j+t\right)-Q_j‖}^2$$

(1)

Iterative computation is used to determine the optimal transformation matrix $R$, $t$ to achieve alignment between two point clouds. The solution for $R$, $t$ can be obtained through the singular value decomposition (SVD) decomposition, constructing the identity matrix $A$.

$$A={\sum }_j^N\left(P_j-{\displaystyle \frac{1}{N}}{\sum }_{j=1}^N{P}_j\right){\left(Q_j-{\displaystyle \frac{1}{N}}{\sum }_{j=1}^N{Q}_j\right)}^T$$

(2)

Next, perform the SVD on matrix $A$.

$$A=U\mathsf{\Sigma }{V}^T$$

(3)

Let $A$ be $3\times 3$ matrix, $\mathsf{\Sigma }$ be a diagonal matrix composed of singular values, and $U$ and $U^T$ be orthogonal matrices.

Define $M=V^{T}RU$. According to the definition of SVD, $M$ remains an orthogonal matrix. Since the elements of an orthogonal matrix are constrained to values less than or equal to 1, we can derive the following $R$ when $M$ is the identity matrix:

$$R=V{U}^T$$

(4)

$$t={\displaystyle \frac{1}{N}}{\sum }_{j=1}^N{P}_j-R{\displaystyle \frac{1}{N}}{\sum }_{j=1}^N{Q}_j$$

(5)

where $\mathrm{E}\left(R,t\right)$ is an objective function of Euclidean distance; $P$ and $Q$ are source points and target points.

2.2. Point-to-Line ICP and Point-to-Plane ICP Point Cloud Registration Algorithm

Point-to-line ICP and point-to-plane ICP are two direct enhancements of the basic point-to-point ICP method. Building on the definition of point-to-point ICP, both approaches seek to determine the transformation $R$ and $t$ between two point clouds. However, instead of identifying a point $Q_j$ that minimizes the Euclidean distance from a specific point $P_j$, these methods look for nearby points to fit into a line or plane.

In point-to-line ICP, the algorithm identifies the two nearest points and connects them, using the distance from a point to the line as the error metric. The optimization of the error function aims to achieve the best convergence toward an optimal solution. For point-to-line ICP, it is essential to locate the two nearest points in the target point cloud $Q$ that correspond to the line ($Q_{j2}-Q_{j1}$) and to derive the normal vector $N_j$ of that line. Consequently, the error function for point-to-line ICP can be articulated as follows:

$$\mathrm{E}\left(R,t\right)={\sum }_{j=1}^N{{{(N}_j}^T[R{P}_j+t-Q_j])}^2$$

(6)

Point-to-plane ICP fits planes to local points. The point-to-plane ICP minimizes the error between points and planes during each iteration. This optimization employs a nonlinear least squares method. The objective is to minimize the sum of the squared distances from each source point to its corresponding target point’s tangent plane. Thus, the error function for point-to-plane ICP can be expressed as follows:

$$\mathrm{E}\left(R,t\right)={\sum }_{j=1}^N{\left({{\left(R{P}_j+t-Q_j\right)}^{T}n}_s\right)}^2={‖Ax-b‖}^2$$

(7)

In this context, $P_j$ represents the source point, $Q_j$ denotes the target point, and $n_s$ signifies the unit normal vector at $Q_j$.

2.3. NDT Point Cloud Registration Algorithm

The NDT algorithm offers an alternative approach to point cloud registration by representing the point clouds as normal distributions rather than discrete points [23]. This method involves:

Step 1. Grid creation: Dividing the target point cloud into a grid of cells, each containing a normal distribution representing the points within the cell.

Step 2. Probability density function: For each cell, compute a probability density function based on the normal distribution.

Step 3. Transformation estimation: Estimating the transformation that maximizes the likelihood of the source point cloud fitting the normal distributions of the target point cloud.

Step 4. Optimization: Using iterative optimization techniques to refine the transformation parameters.

The flowchart of the NDT algorithm is shown in Figure 3.

NDT is known for its robustness, efficiency, and broad convergence range, making it less sensitive to initial conditions and more effective in handling large-scale environments.

NDT is achieved by dividing the target point cloud into several cubes, solving the multi-dimensional normal distribution for each cube and calculating its probability distribution model. As the source point clouds at the same coordinates enter, the probability of each transformed point in the corresponding cube is calculated based on the normal distribution parameters [24]. The accumulated probabilities of all grids are obtained, and when the cumulative probability $p$ reaches the maximum, the optimal matching relationship is found.

First, calculate the mean $q$ and covariance matrix $\mathsf{\Sigma }$ for each cube.

$$q={\displaystyle \frac{1}{n}}{\sum }_{j=1}^n{x}_j$$

(8)

$$\mathsf{\Sigma }={\displaystyle \frac{1}{n}}{\sum }_{j=1}^n\left(x_j-q\right){\left(x_j-q\right)}^T$$

(9)

The $x_j\left(j=\mathrm{1,2},3,...,n\right)$ represent individual grid points, with $n$ being the total number of point clouds.

Next, calculate the probability density function for each mapping point in the cubic grid.

$$p\left(x\right)~exp\left[-{\displaystyle \frac{{\left(x_j-q\right)}^T{\mathsf{\Sigma }}^{-1}\left(x_j-q\right)}{2}}\right]$$

(10)

The point cloud $x_j$ to be registered is transformed to the coordinates in the reference coordinate system as ${x_j}^{\prime }$ through the rotation matrix $R$ and the translation matrix $t$. The probability distribution of each grid mapping point is evaluated and accumulated to calculate the score.

$$score\left(p\right)={\sum }_{j}exp\left[-{\displaystyle \frac{{\left({x_j}^{\prime }-q\right)}^T{\mathsf{\Sigma }}^{-1}\left({x_j}^{\prime }-q\right)}{2}}\right]$$

(11)

Finally, optimize the objective function using Newton’s optimization method to find the optimal solution of the $score\left(p\right)$ function until the convergence point is reached.

LOAM (LiDAR odometry and mapping) and its optimized variant, Lego-LOAM, are significant advancements in SLAM technology. LOAM splits the SLAM process into high-frequency odometry and low-frequency mapping tasks, achieving a balance between real-time performances and mapping accuracy. However, it lacks loop closure detection, which can lead to cumulative errors over time. Lego-LOAM addresses some of LOAM’s limitations by optimizing computation efficiency, making it faster at pose calculations.

Despite this improvement, Lego-LOAM experiences a slight decrease in accuracy and retains a dependency on initial value precision for effective point cloud registration. The NI-LIO algorithm leverages the strengths of both ICP and NDT to overcome their individual limitations. By performing coarse registration using NDT in the front-end odometry phase, NI-LIO ensures robust initial alignment. Subsequently, precise registration using ICP refines the alignment, significantly improving localization accuracy [25]. This dual-stage registration process mitigates the drawbacks associated with single-method approaches, enhancing computational efficiency and reducing sensitivity to initial conditions. The integration of these methods in NI-LIO represents a significant step forward in achieving high-performance SLAM, particularly in complex and dynamic environments.

3. SLAM Mapping Algorithm for NI-LIO Based on the Fusion of ICP and NDT

This section presents a SLAM mapping approach based on the fusion of iterative closest point (ICP) [26] and normal distributions transform (NDT) [27]. The discussion is divided into the following subsections: Data preprocessing, SLAM mapping based on the fusion of ICP and NDT, and its algorithm flowchart.

3.1. Data Preprocessing

Point cloud data are fundamental for mapping in SLAM. The quality of the initial pose estimation of point cloud data determines the effectiveness of point cloud registration. External and internal factors, such as environmental conditions and vehicle speed, affect data acquisition by LiDAR, resulting in extensive data that contain numerous noisy and sparse points. Therefore, preprocessing raw sensor data is crucial for the subsequent point cloud registration process. Distortion compensation and voxel grid filtering are applied to LiDAR data to remove noise points, while IMU data are pre-integrated to reduce noise for further point cloud registration.

3.2. SLAM Mapping Based on the Fusion of ICP and NDT

The ICP algorithm exhibits high computational accuracy and good convergence without the need for point cloud data segmentation and feature extraction. However, its computational cost is significantly high, leading to long processing times. Moreover, it is sensitive to initial transformations and may easily converge to local optima when the initial values are not precise enough. The fusion of ICP and NDT in the SLAM mapping process leverages the strengths of both algorithms to achieve superior localization accuracy and robustness [28]. Figure 4 illustrates the local optimal solution in the ICP algorithm.

The NDT algorithm presents a more streamlined approach compared to the ICP algorithm, characterized by a simpler and more practical methodology. Principally, the distinction lies in the fact that NDT does not align points to other points, lines, or surfaces. Instead, it contrasts them against a statistical measure within voxel representations. It requires merely the local statistical information of the processed point cloud, leveraging this data for effective point cloud matching. Since the direct odometry method excludes the feature extraction step, it is particularly advantageous for handling large volumes of point cloud data that have not undergone feature extraction or segmentation. NDT, through its statistical model, is inherently better suited for managing extensive datasets, making it more appropriate for initial coarse matching compared to ICP. Additionally, the indirect odometry registration method typically results in some loss of subtle point cloud details due to the feature extraction processes, which can slightly reduce accuracy. Meanwhile, employing extensive point-to-surface ICP registration using direct methods may lead to local optimality issues, as the unprocessed point cloud data tends to be more chaotic, yielding suboptimal outcomes. Although the NDT algorithm also faces challenges related to voxel dependencies, its primary function of coarse alignment effectively addresses significant discrepancies in point cloud data, facilitating subsequent point-to-surface ICP registration.

3.3. Algorithm Flowchart

For this purpose, this subsection introduces a new algorithm NI-LIO, which employs NDT-ICP fusion for point cloud registration in the front end of the Lego-LOAM algorithm. The SLAM algorithm front end first performs an initial positional transformation on the surrounding point cloud data using NDT for rough registration. The registered point cloud data serves as the initial value for ICP, resulting in more accurate registration and mitigating the issue of local optima caused by inaccurate initial ICP alignment.

This subsection proposes a novel algorithm, NI-LIO. The original Lego-LOAM algorithm’s odometry module (scan-to-scan) utilized a dual L-M optimization method with ICP for registration. Initially, it estimates the planar transformation parameters from ground point cloud data. Then, it matches edge points and planar points from the segmented point cloud to obtain the final six degrees of freedom pose data. However, ICP’s inherent dependency on initial values poses challenges. Therefore, this study integrates NDT-ICP into the scan-to-scan odometry of the Lego-LOAM algorithm’s front-end. Initially, the SLAM algorithm’s odometry employs NDT for an approximate position transformation, performing a coarse registration of surrounding point cloud data. Although its accuracy may not be optimal, this method allows for faster registration speeds and iterations. After coarse registration, precise registration is necessary to enhance accuracy. For this, point-to-plane ICP, known for its precision and convergence, aligns odometry frames effectively. This approach yields high-precision initial values for point-to-plane ICP, effectively addressing the ICP initial value problem and minimizing the risk of converging on local optima, thus improving the algorithm’s runtime. The specific algorithm flowchart is illustrated in Figure 5. The combined approach can be summarized as follows:

Task 1. Coarse registration with NDT:

Initial alignment: The NDT algorithm is employed for the initial coarse registration of point clouds. NDT converts the point cloud into a grid of normal distributions, providing a probabilistic framework for alignment. This step ensures robust initial alignment, reducing sensitivity to the initial guess and improving convergence.

Task 2. Fine registration with ICP:

Refinement: After the coarse alignment, the ICP algorithm is applied to refine the registration. ICP minimizes the point-to-point distances between the source and target point clouds, providing high-precision alignment. This fine-tuning step enhances the overall accuracy of the SLAM system.

Task 3. Iterative optimization:

Loop: The combined registration process is iterated to ensure optimal alignment. The coarse and fine registration steps are repeated until convergence criteria are met, resulting in a globally consistent map.

This approach also enhances the algorithm’s runtime efficiency. The specific algorithm flowchart depicted in Figure 5 illustrates the SLAM mapping process that integrates ICP and NDT fusion. The flowchart outlines the sequential steps involved in the algorithm:

Step 1. Input point cloud data: Obtain raw point cloud data from LiDAR sensors.

Step 2. Data preprocessing: Apply filtering, noise reduction, and motion compensation using IMU data.

Step 3. Initial alignment (NDT): Perform coarse registration using the NDT algorithm.

Step 4. Point-to-plane (ICP): Apply the ICP algorithm for fine registration.

Step 5. Iterative optimization: Repeat the registration process iteratively until convergence.

Step 6. Map update: Update the SLAM map with the aligned point clouds.

Step 7. Output: Generate the final, globally consistent map.

Figure 5 illustrates the detailed flow of the SLAM mapping algorithm, highlighting the integration of ICP and NDT in the registration process. This combined approach ensures robust and accurate SLAM performance, capable of handling complex and dynamic environments effectively.

In the original Lego-LOAM algorithm, the preprocessing of front-end point cloud data relies on constructing a feature-based LiDAR odometry method. This process necessitates point cloud segmentation and feature extraction, followed by registration between frames. Such steps can slow down the execution speed. In contrast, the proposed NI-LIO algorithm employs a direct LiDAR odometry approach. It performs only basic filtering on the initial point cloud data before proceeding directly to frame registration. By eliminating the need for segmentation and feature extraction, and leveraging the inherent efficiency of the NDT algorithm, this method significantly reduces computational time. Since the NI-LIO algorithm is an enhancement of the Lego-LOAM framework, it also utilizes a loosely coupled framework structure. The inertial measurement unit (IMU) plays a critical role in predicting poses for motion compensation. As the vehicle moves, varying speeds and sharp turns can distort LiDAR scan data. Consequently, the IMU helps predict poses, thereby compensating for motion-induced distortions in the scan data. The overall workflow of the loosely coupled LIO approach is illustrated in Figure 6.

4. Experimental Results and Analysis

To validate the efficacy and reliability of the algorithm proposed in this paper, algorithm testing was conducted on the publicly available KITTI dataset. The algorithm was executed on a laptop system with an AMD R7 (Santa Clara, CA, USA) processor and 16 GB of memory, running the Linux (Ubuntu18.04 desktop) operating system. Experimental testing was carried out on the 04 and 07 datasets from the KITTI dataset, and comparative analysis was performed with the popular Lego-LOAM, FAST_LIO2, and ALOAM [29] algorithms. Due to the overall loose coupling structure in this paper, the comparative analysis exclusively employed the loosely coupled algorithm Lego-LOAM and the more novel and superior mapping algorithm FAST_LIO2 for the experimental comparison. In the study, the point cloud data for the Lego-LOAM, FAST_LIO2, ALOAM, and NI-LIO algorithms were initially preprocessed using voxel grid filtering techniques.

The experiment utilized the publicly available KITTI dataset, which was recorded using the Velodyne HDL-64E (Silicon Valley, CA, USA) model of the LiDAR sensor in conjunction with the OXTS RT 3003 (Middleton Stoney, OX25 4AL, UK) mid-range inertial sensor. Data collection was conducted by Volkswagen across various environments, including rural areas, urban settings, and highways.

4.1. Public Dataset Preprocessing

Preprocessing of the KITTI dataset was essential to ensure accurate and meaningful results. The preprocessing steps included:

Step 1. Data cleaning: Removing redundant and noisy data points to enhance the quality of the point clouds.

Step 2. Down sampling: Applying voxel grid filtering to reduce the number of points while preserving the overall structure, thus improving computational efficiency.

Step 3. Synchronization: Aligning the LiDAR and IMU data timestamps to ensure accurate pose estimation and point cloud registration.

As shown in

Table 1. Preprocessing experimental results.

Parameters	04 Dataset	05 Dataset	07 Dataset
Total number of frames	271	2761	1101
Number of original point clouds	1,412,696	677,856	4,130,997
After preprocessing, the point cloud quantity	155,396	180,229	503,391
Data reduction/%	89%	73%	87%

, the point cloud comparison table of the KITTI dataset before and after the preprocessing of the above steps shows that the point cloud volume is significantly reduced after preprocessing, thereby reducing the operating pressure of the system.

To reduce noise and redundant points in the point cloud data, preprocessing through voxel grid filtering was conducted before point cloud registration. As shown in Table 1, for dataset 04, with a total of 271 frames, the original point cloud count was 1,412,696, and after filtering, the count was reduced to 155,396, which is approximately an 89% reduction. For dataset 05, with a total of 2761 frames, the original point cloud count was 677,856, and after filtering, the count was reduced to 180,229, a 73% reduction. For dataset 07, with a total of 1101 frames, the original point cloud count was 4,130,997, and after filtering, the count was reduced to 503,391, an 87% reduction.

The algorithm presented in this paper employed a direct approach to LiDAR odometry design, eliminating the need for point cloud segmentation and feature extraction during the preprocessing of the front-end point cloud. In contrast, the original algorithm utilized an indirect LiDAR odometry method, wherein point cloud segmentation and feature extraction were performed to facilitate L-M optimization for ICP matching. The current approach directly utilized filtered raw data devoid of noise for NDT registration, thus obviating the necessity for segmentation and feature cluster extraction within the NDT algorithm.

Experimental setup: The experiments were conducted on a laptop with the following specifications: Processor: AMD R7, Memory: 16 GB, Operating System: Linux. Through testing experiments on six datasets in the public KITTI dataset, we selected the more effective datasets 04, 05, and 07 for demonstration. These sequences were chosen due to their varying environmental conditions and complexity, providing a robust test for the algorithm.

The performance of the NI-LIO algorithm was compared against the Lego-LOAM [30], FAST_LIO2, and ALOAM [29] algorithms. The comparison criteria included localization accuracy, computational efficiency, and robustness.

• Localization Accuracy: The accuracy of the generated maps was evaluated by comparing them to ground truth data. Metrics such as root mean square error (RMSE) were used to quantify the differences;
• Computational Efficiency: The runtime of the algorithms was measured to assess the computational load and efficiency improvements;
• Robustness: The ability of the algorithms to handle various environmental conditions and maintain accuracy over long sequences was analyzed.

Figure 7 depicts a comprehensive map of cloud data processing. Figure 7(a1) shows the overall point cloud data of the 04 dataset without preprocessing, while Figure 7(a2) displays the overall point cloud data of the 04 dataset after preprocessing. Figure 7(b1) illustrates the point cloud data of the 05 dataset without preprocessing, and Figure 7(b2) depicts the point cloud data of the 05 dataset after preprocessing. Figure 7(c1) illustrates the point cloud data of the 07 dataset without preprocessing, and Figure 7(c2) depicts the point cloud data of the 07 dataset after preprocessing. As shown in Figure 7, it is evident that after applying the voxel grid filtering algorithm to remove noise points from the original data, the number of points in the point cloud was significantly reduced while preserving the original shape of the map. This preprocessing step greatly improved the overall efficiency of the algorithm presented in this article.

4.2. SLAM Mapping Experiment Analysis

In this subsection, we analyzed the performance of the NI-LIO algorithm by comparing it to the Lego-LOAM [30], FAST_LIO2, and ALOAM algorithms using the public KITTI dataset. The comparison focused on trajectory and pose error.

The KITTI dataset sequences 04, 05, and 07 were specifically chosen for this analysis [31]. Sequence 04 depicts a long highway scene with numerous trees, long straight road segments, and no loops. Sequence 07 represents a structured residential area with a long driving trajectory, various scene changes, and a single loop.

• Trajectory Comparison:

• Sequence 04: This sequence’s long straight roads and consistent environmental features provided a challenging test for maintaining trajectory accuracy over extended distances. The NI-LIO algorithm demonstrated superior performance, maintaining closer alignment to the actual trajectory compared to Lego-LOAM [30], FAST_LIO2, and ALOAM;
• Sequence 05: With its more complex structured residential layout and long straight roads, various scene changes, and a loop, this sequence tested the algorithm’s ability to adapt to different environments and recognize revisited areas. The NI-LIO algorithm effectively handled these challenges, producing a trajectory that accurately followed the actual path;
• Sequence 07: With its structured residential layout, various scene changes, and a loop, this sequence tested the algorithm’s ability to adapt to different environments and recognize revisited areas. The NI-LIO algorithm effectively handled these challenges, producing a trajectory that accurately followed the actual path.

• Pose Error Analysis:

• Pose error is a critical metric for evaluating SLAM performance. It measures the difference between the estimated positions and orientations of the vehicle and the ground truth. In our experiments, NI-LIO consistently showed lower pose error compared to Lego-LOAM, FAST_LIO2, and ALOAM, demonstrating its robustness and accuracy in various scenarios.

• Mapping Accuracy:

• The effectiveness of mapping is determined by comparing the algorithm-generated map path length with the actual map path length. Table 2 provides a detailed comparison.

⋅

Sequence 04: The actual map path length was closely matched by the NI-LIO algorithm, while Lego-LOAM, FAST_LIO2, and ALOAM showed larger discrepancies;

⋅

Sequence 05: The end drift was smaller and more accurate by the NI-LIO algorithm, while Lego-LOAM, FAST_LIO2, and ALOAM showed larger discrepancies;

⋅

Sequence 07: Similar results were observed, with NI-LIO producing a map path length that closely aligned with the actual path, outperforming the other algorithms.

To assess the mapping accuracy of the algorithms, we compared the lengths of the paths generated by the three algorithms to the actual map path length. This comparison helps determine the effectiveness of each algorithm in accurately mapping the environment. As shown in Table 2, the map path lengths generated by the NI-LIO algorithm closely resembled the actual map path lengths, indicating high mapping accuracy.

Table 2. Comparison of actual map path length and algorithm-generated map path length.

Sequence	Actual Path Length (m)	NI-LIO Path Length (m)	Lego-LOAM Path Length (m)	ALOAM Path Length (m)	FAST_LIO2 Path Length (m)
04	393.645	393.665	393.100	392.303	417.112
05	2205.576	2202.121	2170.236	2201.596	2200.661
07	694.697	695.342	699.066	697.431	696.498

Table 2. Comparison of actual map path length and algorithm-generated map path length.

Sequence	Actual Path Length (m)	NI-LIO Path Length (m)	Lego-LOAM Path Length (m)	ALOAM Path Length (m)	FAST_LIO2 Path Length (m)
04	393.645	393.665	393.100	392.303	417.112
05	2205.576	2202.121	2170.236	2201.596	2200.661
07	694.697	695.342	699.066	697.431	696.498

An analysis of Table 2 indicates that NI-LIO demonstrated an average improvement of over 30% in the original trajectory length error compared to other algorithms. The results, summarized in Table 2, indicated that the NI-LIO algorithm not only achieves high computational efficiency but also maintains high accuracy in mapping and trajectory estimation. The substantial improvements in trajectory alignment and reduced pose error highlight the effectiveness of the NDT-ICP fusion approach implemented in NI-LIO.

4.2.1. Trajectory Comparison Experimental Analysis

To further validate the feasibility of the NI-LIO algorithm, which combines ICP and NDT for SLAM mapping, we conducted a detailed analysis of trajectory comparison, relative pose error, and absolute pose error [32]. These analyses were performed in comparison with existing algorithms, Lego-LOAM, FAST_LIO2, and ALOAM, using the KITTI dataset. Trajectory Comparison:

Overview: The comparison focused on how well each algorithm aligned with the ground truth trajectory. Accurate trajectory estimation is crucial for reliable SLAM performance. Results: Figure 8 shows the trajectory comparison between our NI-LIO algorithm and the Lego-LOAM, FAST_LIO2, and ALOAM algorithms using sequences from the KITTI dataset. The overall trajectories are plotted to illustrate how closely each algorithm followed the ground truth. Local zoom analysis: Detailed views in Figure 8a,b present zoomed-in segments of the trajectories. These local zooms highlight the areas where our algorithm aligned more accurately with the ground truth compared to Lego-LOAM, FAST_LIO2, and ALOAM. Whether or not loop closures were present, NI-LIO consistently showed better alignment with the true trajectory.

Relative pose error: Definition: Relative pose error measures the error in the estimated position and orientation of the vehicle over a short distance, reflecting the algorithm’s ability to accurately track motion between successive frames. Comparison: Our experiments indicated that NI-LIO had a lower relative pose error compared to Lego-LOAM, FAST_LIO2, and ALOAM. This demonstrates that the hybrid NDT-ICP approach improves the precision of motion estimation, especially in environments with significant changes.

Absolute pose error: Definition: Absolute pose error measures the discrepancy between the estimated and actual positions and orientations over the entire trajectory. It provides an overall assessment of the SLAM system’s accuracy. Results: The absolute pose error analysis further supports the superior performance of NI-LIO. Our algorithm consistently resulted in lower absolute pose errors across different sequences, confirming its robustness and reliability for long-term navigation.

The trajectory comparison, along with relative and absolute pose error analyses, clearly demonstrated the advantages of the NI-LIO algorithm. By effectively integrating NDT and ICP for point cloud registration, NI-LIO achieved higher accuracy and robustness in SLAM mapping compared to traditional methods. These improvements were particularly evident in the detailed trajectory alignments and error metrics, showcasing the algorithm’s potential for practical applications in autonomous navigation.

4.2.2. Error Analysis of Pose for Comparative Experimental Study

To assess the superiority of the NI-LIO algorithm, we evaluated the mapping results of datasets 04 and 07 using the NI-LIO, Lego-LOAM, FAST_LIO2, and ALOAM algorithms for absolute pose error (APE) and relative pose error (RPE). The evaluations were conducted using the EVO tool, with ground truth data provided by the KITTI dataset official website [31]. Absolute pose error (APE) analysis: APE measures the error in the estimated position and orientation of the vehicle relative to the ground truth over the entire trajectory. It is a critical metric for assessing the overall accuracy and stability of SLAM algorithms. Results: Figure 9 shows the comparison of APE in mapping experiments using the NI-LIO, Lego-LOAM, FAST_LIO2, and ALOAM algorithms on datasets 04, 05, and 07.

Dataset 04: The APE curve for the NI-LIO algorithm demonstrated smoother overall linear characteristics without significant positional variations over continuous time intervals, highlighting its superior stability. The Lego-LOAM algorithm exhibited notable positional variations during early and late periods, indicating weaker stability. Dataset 05: Similar trends were observed. The Lego-LOAM, FAST_LIO2, and ALOAM fluctuations were more pronounced compared to NI-LIO, and the mapping accuracy was lower.

Dataset 07: Similar trends were observed, with the NI-LIO algorithm maintaining stable performance, while Lego-LOAM and ALOAM showed greater fluctuations.

Root mean square error (RMSE) comparison:

• Analysis: The bar chart in Figure 9 compares the RMSE values of the APE for the three algorithms across datasets 04, 05, and 07. The NI-LIO algorithm showed lower RMSE values than both Lego-LOAM, FAST_LIO2, and ALOAM, indicating higher mapping accuracy.

○

Dataset 04: NI-LIO achieved the lowest RMSE, followed by ALOAM and then Lego-LOAM;

○

Dataset 05: NI-LIO still kept the lowest RMSE. FAST_LIO2 was the worst performer;

○

Dataset 07: The trend remained consistent, with NI-LIO exhibiting the best performance in terms of RMSE.

The comparison of bar charts revealed that the root mean square error (RMSE) of the NI-LIO algorithm was smaller than that of both the Lego-LOAM and ALOAM algorithms across two distinct datasets. Specifically, the absolute pose error (APE) RMSE of NI-LIO demonstrated an estimated average reduction of approximately 34% compared to the ALOAM algorithm in the no-loop dataset 04. In the datasets with loops 05 and 07, reductions of about 42% and 24%, respectively, were observed. The error analysis of posture for comparative experimental study underscores the advantages of the NI-LIO algorithm. The smoother APE curves and lower RMSE values highlight its superior stability and accuracy in SLAM mapping. By effectively integrating NDT for coarse alignment and ICP for fine registration, NI-LIO consistently outperforms traditional algorithms, demonstrating its potential for real-world autonomous navigation applications.

To further validate the feasibility of the proposed algorithm, we conducted extensive tests on the absolute pose error (APE) using the publicly available KITTI dataset and other datasets. The root mean square error (RMSE) results for the absolute pose error (APE) are presented in

Table 3. Analysis of the APE accuracy of the dataset.

Dataset	Scene	Loop	Algorithm	APE/m
00	City	Yes	NI-LIO	2.24
			Lego-LOAM	6.61
			ALOAM	3.30
			FAST-LIO2	12.58
04	Highways	No	NI-LIO	0.25
			Lego-LOAM	0.41
			ALOAM	0.38
			FAST-LIO2	3.77
05	City	Yes	NI-LIO	1.43
			Lego-LOAM	4.58
			ALOAM	2.47
			FAST-LIO2	7.09
06	Highways	Yes	NI-LIO	1.07
			Lego-LOAM	2.85
			ALOAM	2.18
			FAST-LIO2	14.99
07	City	Yes	NI-LIO	0.37
			Lego-LOAM	0.87
			ALOAM	0.49
			FAST-LIO2	1.97
09	City	Yes	NI-LIO	3.48
			Lego-LOAM	14.30
			ALOAM	9.41
			FAST-LIO2	5.84

. Table 3 indicates that the NI-LIO algorithm showed varying degrees of improvement in RMSE across both loop closure scenarios and in complex and simple maps. This demonstrates a significant enhancement in both algorithm accuracy and trajectory consistency of the NI-LIO method across diverse environments.

Figure 10 shows the comparison of the relative pose error (RPE) in mapping experiments using the NI-LIO, Lego-LOAM, and ALOAM algorithms with the 04, 05, and 07 datasets. Analysis of the RPE line chart indicated that the line chart of the NI-LIO algorithm was smoother, demonstrating its good stability and smaller relative motion differences. Furthermore, the bar chart revealed that the standard deviation of NI-LIO was smaller compared to the other two algorithms, indicating lower dispersion and, hence, greater stability. The root mean square error (RMSE) of the relative pose error (RPE) for NI-LIO estimated an average reduction of approximately 35% compared to the ALOAM algorithm in the loop-free dataset 04. In the datasets with loops, 05 and 07, the reductions were roughly 75% and 24%, respectively. Additionally, the root mean square error (RMSE) of NI-LIO was smaller than that of the other three algorithms, and this trend was consistent across both datasets, proving the superior stability of the NI-LIO algorithm over the other two.

In the KITTI dataset utilized for testing, there is a significant presence of loop environments, which are characterized by their high complexity. The phenomenon of drift in LiDAR systems during high-speed operations becomes particularly pronounced at turns, leading to substantial cumulative errors due to the occurrence of consecutive turns in various loop scenarios. This, in turn, adversely affects the accuracy of map construction. The algorithm presented in this paper demonstrated an improvement in registration performance, which subsequently enhanced the accuracy within loop environments. As evidenced by the comparative analysis of the absolute pose error (APE) presented in Table 3, there was a notable enhancement in pose accuracy across the five different loop environments. Therefore, the NI-LIO algorithm exhibits commendable adaptability in loop environments, yielding effective results across a range of loop scenarios of varying sizes.

Relative pose error (RPE) analysis:

• Overview: RPE measures the error in the estimated position and orientation over short distances between successive frames. It provides insight into the algorithm’s ability to accurately track motion over time;
• Results: The NI-LIO algorithm consistently outperformed Lego-LOAM, FAST_LIO2, and ALOAM, exhibiting lower RPE values and demonstrating better motion tracking accuracy.

As illustrated in Figure 10, a comparative analysis of the relative pose error (RPE) for the datasets 04, 05, and 07 was conducted using the NI-LIO, Lego-LOAM, and ALOAM algorithms. An analysis of the relative pose error line graph indicated that the NI-LIO algorithm exhibited a smoother curve, demonstrating superior stability and minimal relative motion discrepancies. Furthermore, the bar graph revealed that the standard deviation of the NI-LIO algorithm was lower than that of the other two algorithms, indicating a reduced degree of dispersion and thus greater stability of the algorithm.

To further validate the feasibility of the algorithm in this paper, we conducted comprehensive tests on relative pose error (RPE) using the public KITTI dataset and other datasets. The results of the root mean square error (RMSE) for the relative pose error (RPE) are presented in

Table 4. Analysis of the RPE accuracy of the dataset.

Dataset	Scene	Loop	Algorithm	RPE/m
00	City	Yes	NI-LIO	1.003
			Lego-LOAM	6.919
			ALOAM	1.213
			FAST-LIO2	1.376
04	Highways	No	NI-LIO	0.027
			Lego-LOAM	0.084
			ALOAM	0.042
			FAST-LIO2	2.415
05	City	Yes	NI-LIO	0.290
			Lego-LOAM	7.272
			ALOAM	1.195
			FAST-LIO2	1.253
06	Highways	Yes	NI-LIO	1.630
			Lego-LOAM	9.246
			ALOAM	3.588
			FAST-LIO2	1.651
07	City	Yes	NI-LIO	0.013
			Lego-LOAM	0.057
			ALOAM	0.015
			FAST-LIO2	1.223
09	City	Yes	NI-LIO	0.687
			Lego-LOAM	8.865
			ALOAM	1.550
			FAST-LIO2	1.570

. As shown in Table 4, the proposed NI-LIO algorithm demonstrated varying degrees of improvement in average RMSE in scenarios with and without loop closures, as well as in complex and simple maps. This confirms that the NI-LIO algorithm maintains a relatively low systemic drift in estimation across different environments.

Based on the experimental results, the NI-LIO algorithm constructs maps that are closer to the ground truth compared to the Lego-LOAM and ALOAM algorithms in trajectory comparison experiments. In terms of APE, the NI-LIO algorithm reduces the average error by about 34% compared to the ALOAM algorithm. For RPE, the NI-LIO algorithm reduces the average error by approximately 35% compared to the ALOAM algorithm. Both error metrics indicate that the NI-LIO algorithm exhibits smoother linearity. The experimental results suggest that the NI-LIO algorithm demonstrates higher precision and stability, facilitating better outdoor localization and mapping.

Since this dataset utilizes the odometry correction dataset from the KITTI collection, it does not place significant emphasis on processing the IMU data. Consequently, the performance of systems employing tightly coupled LIO for SLAM mapping is suboptimal. As a result, the error observed in the testing results for FSAT-LIO2 showed a considerable offset when compared to loosely coupled LIO systems that do not rely on IMU data for SLAM.

Simultaneously, the KITTI dataset, particularly in scenarios such as sequence 00, was characterized by high drift and significant noise. The NI-LIO algorithm demonstrated commendable adaptability and robustness in such environments, achieving superior localization accuracy compared to other algorithms. In conclusion, the experimental analysis demonstrates that the NI-LIO algorithm outperforms the popular Lego-LOAM, FAST_LIO2, and ALOAM algorithms in terms of trajectory accuracy, pose error, and mapping accuracy. The integration of NDT for coarse alignment and ICP for fine registration in NI-LIO significantly enhances SLAM performance, making it a robust and efficient choice for real-world applications.

5. Conclusions

In laser-based SLAM algorithms, the accuracy of point cloud registration in the front end significantly influences both mapping and localization performance. Inaccurate registration can lead to shifts in the generated maps, reducing localization precision and causing discrepancies between the estimated and actual paths. Therefore, robust registration algorithms are essential to achieving reliable SLAM outcomes. This paper proposed a fusion-based approach to address the challenges associated with traditional point cloud registration methods. We introduced NI-LIO, a novel SLAM algorithm that integrates Iterative Closest Point (ICP) and Normal Distributions Transform (NDT), to enhance point cloud registration. Our approach mitigates the limitations of ICP and NDT—such as high computational costs, sensitivity to initial estimates, and vulnerability to local optima—by combining their strengths to improve localization accuracy, computational efficiency, and robustness. Using the publicly available KITTI dataset (sequences 04 and 07), we evaluated the performance of NI-LIO. Comparative analysis against widely used algorithms, Lego-LOAM and ALOAM, showed that NI-LIO achieved significant improvements in both absolute pose error (APE) and relative pose error (RPE). Specifically, NI-LIO reduced the average APE by approximately 34% and the average RPE by 35% in loop-free environments, while also offering better stability and accuracy in trajectory alignment.

The results demonstrated that NI-LIO generates maps that are more consistent with the ground truth and enhances system stability, making it particularly effective for outdoor localization and mapping. Although the improvements in loop-containing environments were less pronounced, NI-LIO still achieved notable error reductions, highlighting its robustness. Future work will focus on optimizing the algorithm for better performance in loop-containing environments and exploring its application in other challenging SLAM scenarios. The promising results of NI-LIO suggest its strong potential for real-world autonomous navigation applications where high precision and stability are critical.

Front-end point cloud registration is a critical component of laser SLAM algorithms. However, due to the continuous nature of mapping in SLAM, it is inevitable that cumulative errors can lead to significant drift in the constructed point cloud map, resulting in substantial discrepancies with the actual environment. To significantly enhance the accuracy of SLAM localization and the quality of mapping, merely improving front-end point cloud registration is insufficient. This limitation highlights the constraints of focusing solely on registration methods. Consequently, subsequent research will prioritize advancements in loop closure detection and back-end graph optimization to achieve more pronounced improvements in algorithm performance.