Online Mapping from Weight Matching Odometry and Highly Dynamic Point Cloud Filtering via Pseudo-Occupancy Grid

Abstract

Efficient locomotion in autonomous driving and robotics requires clearer visualization and more precise map. This paper presents a high accuracy online mapping including weight matching LiDAR-IMU-GNSS odometry and an object-level highly dynamic point cloud filtering method based on a pseudo-occupancy grid. The odometry integrates IMU pre-integration, ground point segmentation through progressive morphological filtering (PMF), motion compensation, and weight feature point matching. Weight feature point matching enhances alignment accuracy by combining geometric and reflectance intensity similarities. By computing the pseudo-occupancy ratio between the current frame and prior local submaps, the grid probability values are updated to identify the distribution of dynamic grids. Object-level point cloud cluster segmentation is obtained using the curved voxel clustering method, eventually leading to filtering out the object-level highly dynamic point clouds during the online mapping process. Compared to the LIO-SAM and FAST-LIO2 frameworks, the proposed odometry demonstrates superior accuracy in the KITTI, UrbanLoco, and Newer College (NCD) datasets. Meantime, the proposed highly dynamic point cloud filtering algorithm exhibits better detection precision than the performance of Removert and ERASOR. Furthermore, the high-accuracy online mapping is built from a real-time dataset with the comprehensive filtering of driving vehicles, cyclists, and pedestrians. This research contributes to the field of high-accuracy online mapping, especially in filtering highly dynamic objects in an advanced way.

1. Introduction

LiDAR mapping technology has made remarkable progress and is widely applied in various fields, including autonomous driving [1,2,3], SLAM systems [4,5,6] and robotic navigation [7,8,9,10]. Particularly in robotics [9,10,11], the synergistic integration of multi-sensors and computer vision has revolutionized the environmental perception capabilities of intelligent agents.

Recent research demonstrates that information fusion from a Global Navigation Satellite System (GNSS), an Inertial Measurement Unit (IMU), and a LiDAR sensor achieves high-precision mapping and localization in complex environments [12,13]. However, in reality, the presence of dynamic objects poses significant challenges to the accuracy of LiDAR mapping. Conventional point cloud matching algorithms typically assume a static environment. This assumption holds in most cases due to the short single-frame scanning time, and thus the impact of dynamic objects on odometry accuracy can be negligible [14]. Nevertheless, when objects move at a high speed, the resulting point cloud distortion severely disrupts matching precision, leading to accumulated localization errors. In some cases, moving objects introduced as “ghost artifacts” [15,16] appear into the generated map. The resulting map redundancy can severely mislead localization and path planning, decreasing accuracy and safety in navigation decision-making. Therefore, effectively suppressing and eliminating the side effects of highly dynamic point clouds on maps is of great significant in enhancing the reliability and practicality of LiDAR mapping systems.

According to the real-time performance of processing sensor data in dynamic environments, dynamic point cloud filtering algorithms can generally be categorized into two classes: online filtering and post-processing-based filtering. Online filtering methods typically select several consecutive point cloud frames as a reference and then compare them with the target frame in order to achieve superior real-time performance. For instance, RF-LIO [17], an extension of LIO-SAM [18], leverages the information from the initial pose of frontend odometry and local submaps to filter dynamic points on the current frame adaptively and iteratively, ultimately generating a static global map. ETH-Zurich’s ASL Lab proposed an end-to-end dynamic object detection framework [19], which is based on occupancy grids. This framework automatically labels dynamic objects and then trains them using 3D-MiniNet, enabling online detection and filtering of dynamic point clouds. Post-processing methods utilize all frames during the entire SLAM cycle as a reference, thus achieving higher accuracy and precision. The Peopleremover algorithm [20] constructs a regular voxel occupancy grid and then determines free voxels by traversing the lines of sight from the sensor to the measured points through the voxel grid. ERASOR [21], proposed by Hyungtae Lim et al., detects dynamic points by comparing region-wise occupancy ratios between the current scan and local submaps. The ratios exceeding a threshold are marked as dynamic. Removert [22], presented by Giseop Kim et al., built a novel 2D depth map by projecting a 3D point cloud with its neighboring submaps. By comparing the pixel depths at the same positions on the two maps, the shallower one is considered a dynamic point. This method outperforms manual annotations on the SemanticKITTI dataset [23], demonstrating an excellent filtering efficacy.

In real-world scenarios, to ensure accurate decision-making at high speeds requires prioritizing the real-time performance of maps. Simultaneously, at the same time, to optimize the effectiveness of decision-making and the robustness of motion behaviors, the map must be built with sufficiently high accuracy. In order to address it, therefore, this paper improves the precision of LiDAR-IMU-GNSS multi-sensor mapping by analyzing the geometric and reflective similarities of point cloud matching points to determine weight coefficients. It then employs a pseudo-occupancy grid-based method to filter object-level highly dynamic point clouds in real time, ultimately effectively enhancing accuracy and efficiency in mapping. Simulation experiments demonstrate that the accuracy of this odometry system surpasses that of LIO-SAM and FAST-LIO2 based on KITTI, UrbanLoco [24], and Newer College [25] (NCD) datasets. Furthermore, compared to the Removert and ERASOR algorithms, the proposed filtering method more effectively removes highly dynamic point clouds. The proposed algorithm is applied to a real-time dataset to build the map online with successful filtering of driving vehicles, cyclists, and pedestrians.

2. Algorithm Framework

This paper proposes an online mapping algorithm for the high accuracy multi-sensor fusion of LiDAR-IMU-GNSS and object-level highly dynamic point filtering. The framework comprises data preprocessing, weight feature point matching, and dynamic point filtering, as illustrated in Figure 1.

(i)

Data preprocessing: Raw point cloud data from LiDAR, IMU, and GNSS are processed to generate synchronized and fused multi-sensor measurements. This includes IMU pre-integration, ground point segmentation using the Progressive Morphological Filter (PMF), and motion compensation of LiDAR point clouds.

(ii)

Weight feature point matching: the weight coefficient of the Mahalanobis distance is determined by geometric and reflectance intensity similarities to obtain the optimized pose.

(iii)

Dynamic point filtering: Dynamic objects are removed from the point cloud, and the map is built in real time using a pseudo-occupancy grid filtering algorithm.

3. Data Fusion for Weight Matching LiDAR-IMU-GNSS Odometry

3.1. IMU Pre-Integration

IMU pre-integration constructs constraints between LiDAR keyframes to suppress odometry drift and establish a pose graph that includes attitude, velocity, and position information. After acquiring IMU data and accounting for bias and noise, the predicted pre-integration values for attitude $R_i$, velocity $v_i$ and position $p_i$ between frames $i$ and $j$ are derived using a discrete-time integration method. Furthermore, their relative differences $\Delta R_{i,j}$, $\Delta v_{i,j}$, and $\Delta p_{i,j}$ are expressed in Equation (1),

$$\left\{\begin{array}{c}\Delta R_{i,j}=R_i^T{R}_j={\prod }_{k=i}^{j-1}Exp\left(\left({\tilde{\omega }}_k-b_k^{\omega }-n_k^{\omega }\right)\Delta t_k\right)\\ \Delta v_{i,j}=v_j-v_i={\sum }_{k=i}^{j-1}\left(\left(R_k\left({\tilde{a}}_k-b_k^a-n_k^a\right)-g\right)\Delta t_k\right)\\ \Delta p_{i,j}=p_j-p_i={\sum }_{k=i}^{j-1}\left(v_k\Delta t_k+{\displaystyle \frac{1}{2}}\left(R_k\left({\tilde{a}}_k-b_k^a-n_k^a\right)-g\right)\Delta t_k^2\right)\end{array}\right..$$

(1)

where ${\tilde{\omega }}_k$ and ${\tilde{a}}_k$ represent the angular velocity measured by the gyroscope and the linear acceleration by the accelerometer at the kth frame, respectively. $n_k^{\omega }$ and $n_k^a$ denote the bias noise of the angular velocity and acceleration at the kth frame. $b_k^{\omega }$ and $b_k^a$ are the measurement biases for the angular velocity and acceleration at the kth frame. $\Delta t_k$ is the time interval between the kth frame and the (k + 1)^th frame in the IMU data. $g$ represents gravity. $Exp\left(.\right)$ denotes the mapping from a rotation vector to a rotation matrix. Therefore, the state at the (k + 1)^th frame can be predicted by integrating the state at the kth frame.

Assuming that the IMU biases $b_k^{\omega }=const$ and $b_k^a=const$ over a short period, the noise terms can be further simplified. Using the linear approximation from Lie group theory, we establish the relationship between the predicted $\Delta R_{i,j}$, $\Delta v_{i,j}$, and $\Delta p_{i,j}$ and the measured $\Delta {\tilde{R}}_{i,j}$, $\Delta {\tilde{v}}_{i,j}$, and $\Delta {\tilde{p}}_{i,j}$. The measured values are written as follows:

$$\left\{\begin{array}{c}\Delta R_{i,j}=\Delta {\tilde{R}}_{i,j}Exp\left(-\delta {\varphi }_{i,j}\right)\\ \Delta v_{i,j}=\Delta {\tilde{v}}_{i,j}-\delta v_{i,j}\\ \Delta p_{i,j}=\Delta {\tilde{p}}_{i,j}-\delta p_{i,j}\end{array}\right..$$

(2)

On the left-hand side of Equation (2) are the predicted values derived from the state variables, and on the right-hand side are the measured values obtained through IMU integration and a random noise term. To explicitly represent the measured values in Equation (2), it is necessary to derive the state transition equations for the random noise terms.

Equation (3) reveals that $\Delta R_{i,j}$ differs from $\Delta {\tilde{R}}_{i,j}$ by a noise term $Exp\left(-\delta {\varphi }_{i,j}\right)$. By applying the operator $Log\left(.\right)$, which denotes the mapping from a rotation matrix to a rotation vector, the noise term can be linearly approximated. Retaining only the first-order term, the $\delta {\varphi }_{i,j}$ is expressed as

$$\delta {\varphi }_{i,j}=-Log\left(\prod ^{\underset{j-1}{k=i}}Exp\left(-\Delta {\tilde{R}}_{k+1,j}^T{J}_{r,k}{n}_k^{\omega }\Delta t_k\right)\right)\approx {\sum }_{k=i}^{j-1}\Delta {\tilde{R}}_{k+1,j}^T{J}_{r,k}{n}_k^{\omega }\Delta t_k=\Delta {\tilde{R}}_{k+1,j}^T\delta {\varphi }_{i,j-1}+J_{r,j-1}{n}_{j-1}^{\omega }\Delta t_{j-1},$$

(3)

where $J_{r,k}$ is the Jacobian matrix between the rth frame and the kth frame, and similarly explanation to $J_{r,j-1}$. $n_{j-1}^{\omega }$ represents the (j − 1)^th bias noise of the angular velocity. $\Delta {\tilde{R}}_{k+1,j}^T$ represents the measured attitude difference between the (k + 1)^th frames and the $j$^th frame, and $T$ is transpose. $\Delta t_{j-1}$ is the time interval between the (j − 1)^th frame and the jth frame in the IMU data.

Assuming that the noise terms of $\delta {\varphi }_{i,j}$, $\delta v_{i,j}$, and $\delta p_{i,j}$ as well as the bias noise of the IMU, all follow zero-mean Gaussian distributions, we derive the following expression:

$${\eta }_{i,j}=\left[\begin{array}{c}\delta {\varphi }_{i,j}\\ \delta v_{i,j}\\ \delta p_{i,j}\end{array}\right]\sim N\left(0_{9\times 1},{\Sigma }_{{\eta }_{i,j}}\right) \mathrm{a}\mathrm{n}\mathrm{d} {\eta }_j^n=\left[\begin{array}{c}{n}_j^{\omega }\\ n_j^a\end{array}\right]\sim N\left(0_{6\times 1},{\Sigma }_{{\eta }_j^n}\right).$$

(4)

Similar procedures are applied in $\delta v_{i,j}$ and $\delta p_{i,j}$ terms. The state transition equation of ${\eta }_{i,j}$ is given by

$${\eta }_{i,j}=A_j{\eta }_{i,j-1}+B_j{\eta }_{j-1}^n{A}_j=\left[\begin{array}{ccc}\Delta {\tilde{R}}_{j-1,j}^T& 0& 0\\ -\Delta {\tilde{R}}_{i,j-1}{\left({\tilde{a}}_{j-1}-b_i^a\right)}^{\wedge }\Delta t_{j-1}& I& 0\\ -{\displaystyle \frac{1}{2}}\Delta {\tilde{R}}_{i,j-1}{\left({\tilde{a}}_{j-1}-b_i^a\right)}^{\wedge }\Delta t_{j-1}^2& \Delta t_{j-1}& I\end{array}\right] \mathrm{a}\mathrm{n}\mathrm{d} B_j=\left[\begin{array}{cc}{J}_{r,j-1}\Delta t_{j-1}& 0\\ 0& \Delta {\tilde{R}}_{i,j-1}\Delta t_{j-1}\\ 0& {\displaystyle \frac{1}{2}}\Delta {\tilde{R}}_{i,j-1}\Delta t_{j-1}^2\end{array}\right],$$

(5)

where $I$ denotes the identity matrix. $\wedge$ is the symbol for antisymmetric matrix. Consequently, the state transition equation for the covariance matrix of the noise term is derived as

$${\Sigma }_{{\eta }_{i,j}}=A_j{\Sigma }_{{\eta }_{i,j-1}}{A}_j^T+B_j{\Sigma }_{{\eta }_j^n}{B}_k^T.$$

(6)

The variation of IMU biases over time significantly affects the accuracy of measurements. By assuming that the pre-integrated measurements vary linearly with the IMU biases, we retain the first-order partial derivatives of the pre-integrated measurements with respect to the biases, so as to compensate for bias-induced errors in pre-integration.

$$\left\{\begin{array}{c}\Delta {\tilde{R}}_{i,j}\left(b_i^{\omega }+\delta b_i^{\omega }\right)=\Delta {\tilde{R}}_{i,j}\left(b_i^{\omega }\right)Exp\left({\displaystyle \frac{\partial \Delta {\tilde{R}}_{i,j}}{\partial b_i^{\omega }}}\partial b_i^{\omega }\right)\\ \Delta {\tilde{v}}_{i,j}\left(b_i^{\omega }+\delta b_i^{\omega },b_i^a+\delta b_i^a\right)=\Delta {\tilde{v}}_{i,j}\left(b_i^{\omega },b_i^a\right)+{\displaystyle \frac{\partial \Delta {\tilde{v}}_{i,j}}{\partial b_i^{\omega }}}\delta b_i^{\omega }+{\displaystyle \frac{\partial \Delta {\tilde{v}}_{i,j}}{\partial b_i^a}}\delta b_i^a\\ \Delta {\tilde{p}}_{i,j}\left(b_i^{\omega }+\delta b_i^{\omega },b_i^a+\delta b_i^a\right)=\Delta {\tilde{p}}_{i,j}\left(b_i^{\omega },b_i^a\right)+{\displaystyle \frac{\partial \Delta {\tilde{p}}_{i,j}}{\partial b_i^{\omega }}}\delta b_i^{\omega }+{\displaystyle \frac{\partial {\tilde{p}}_{i,j}}{\partial b_i^a}}\delta b_i^a\end{array}\right..$$

(7)

3.2. Ground Point Segmentation

The point cloud of the keyframe is extracted to reduce storage. In order to achieve the adaptive adjustment of the keyframe threshold, the parameter $d_k$ is determined based on the surrounding environment. Therefore, the adaptive keyframe threshold $d{i}_k$ is expressed in Equation (8):

$$d{i}_k=\left\{\begin{array}{c}5.0, d_k>20\\ 3.0, 8<d_k\le 20\\ 1.0,{ d}_k\le 8\end{array}\right.,$$

(8)

where $d_k=\alpha d_{k-1}+\beta \bar{r}$, $\bar{r}$ is the median of the Euclidean distances from the point cloud of the current frame to the origin of the LiDAR. $d_{k-1}$ represents the openness of the scanning environment in the previous frame. $\alpha$ and $\beta$ are constants, and $\alpha =0.9$ and $\beta =0.1$ are chosen empirically in the experimental section.

During online mapping, the Progressive Morphological Filtering (PMF) algorithm is employed to segment ground points and non-ground points in each keyframe point cloud in real time.

PMF optimizes the window dimensions by gradually increasing the size of the filtering window. The update principles for the filtering window size $w_k$ and the height difference threshold $d{h}_k$ are presented in Equation (9).

$$w_k=2b^k+1,d{h}_k=\left\{\begin{array}{c}d{h}_0,w_k\le 3\\ s\left(w_k-w_{k-1}\right)c+d{h}_0,w_k>3\\ d{h}_{max},d{h}_k>d{h}_{max}\end{array}\right.,$$

(9)

where $w_k$ denotes the window size at the kth iteration. $b$ represents the initial window size. $d{h}_0$ and $d{h}_{max}$ refer to the initial and maximum height difference thresholds, respectively. $s$ indicates the slope, and $s=2d{h\left(w_k-w_{k-1}\right)}_{max}$. $c$ stands for the grid size.

3.3. Motion Compensation of LiDAR Point Clouds

At high vehicle speeds, motion distortion compensation for point clouds becomes essential. When IMU data is available, IMU-based motion compensation in continuous time is used. Otherwise, in the case of IMU data dropout, the motion compensation method based on a constant velocity model is used. Assuming that the current frame maintains the same motion state as the previous LiDAR frame, the LiDAR pose at each timestamp within the current frame can be calculated through interpolation, as expressed below,

$$\left\{\begin{array}{c}{R}^{\mathrm{*}}=R_{i-1}\cdot Exp\left(\mu \Delta \varphi \right)\\ \Delta \varphi =Log\left(R_{i-2}^T\cdot R_{i-1}^T\right)\\ p^{\mathrm{*}}=p_{i-1}+\mu \left(p_{i-1}-p_{i-2}\right)\\ \mu ={\displaystyle \frac{t^{\mathrm{*}}-t_{i-1}}{t_{i-1}-t_{i-2}}}\end{array}\right.,$$

(10)

where $R^{*}$ and $p^{*}$ denote the query point’s attitude and position. $t^{*}$ represents the current timestamp.

3.4. Weight Feature Point Matching Method Based on Geometric-Reflectance Intensity Similarity

The Mahalanobis distance between points is commonly used as the criterion for feature point matching and for obtaining the optimized pose. However, assigning the same weight to each matched point fails to accurately reflect the varying influence of uncertainty across different matched point pairs. During the point matching process, the weight of false matches should be significantly reduced. In addition, the positional uncertainty of the matched points introduces noise, and thus the weight of such points should decrease as their uncertainty increases.

To enhance both point matching accuracy and calculational efficiency, an eigenvalue-based approach is employed to extract planar feature points. Accordingly, the centroid and the covariance matrix of the point distribution around the point $i$ is given in Equation (11) based on the KD-Tree method.

$$\left\{\begin{array}{c}{p}_{center}={\displaystyle \frac{1}{N}}{\sum }_{j=1}^N{p}_j\\ C_i={\displaystyle \frac{1}{N-1}}{\sum }_{i=1}^N\left(\left(p_i-p_{center}\right)\cdot {\left(p_i-p_{center}\right)}^T\right)\end{array}\mathrm{a}\mathrm{n}\mathrm{d}\left\{\begin{array}{c}{C}_i=U_i\cdot {\sum }_i\cdot V_i^T\\ {\sum }_i=\left[\begin{array}{ccc}{\lambda }_0& 0& 0\\ 0& {\lambda }_1& 0\\ 0& 0& {\lambda }_2\end{array}\right]\\ {\lambda }_0\ge {\lambda }_1\ge {\lambda }_2\ge 0\end{array}\right.,\right.$$

(11)

where ${\lambda }_0$, ${\lambda }_1$, and ${\lambda }_2$ are the three eigenvalues of the covariance matrix.

If eigenvalues satisfy Equation (12), the point $i$ is a planar feature point.

$$\left\{\begin{array}{c}{\displaystyle \frac{{\lambda }_2}{{\lambda }_0+{\lambda }_1}}<{\rho }_0\\ {\displaystyle \frac{{\lambda }_0-{\lambda }_1}{{\lambda }_0+{\lambda }_1}}<{\rho }_1\end{array}\right.,$$

(12)

where ${\rho }_0$ and ${\rho }_1$ are thresholds.

In order to reduce incorrect matching, the similarity between two feature points should be taken into account. By assigning different weights to each point, the residual function for point cloud matching can be formulated. In this section, a weight feature point matching method based on the geometric-reflectance intensity similarity of point clouds is proposed.

The geometric similarity is defined by the positional relationship between the normal vectors $\mathbf{n}$ of the planar feature point $i$ in one point cloud and $j$ in the target point cloud, separately. Note that ${\lambda }_2$ is the smallest eigenvalue; its eigenvector ${\mathbf{v}}_2$ is thus approximately equivalent to the normal vector of the corresponding plane. The subscript $i$ and $j$ correspond to the points $i$ and $j$. Therefore, the geometric similarity $S_{G,i}$ can be expressed as

$$S_{G,i}={\displaystyle \frac{{\mathbf{n}}_i^T{\mathbf{n}}_j}{‖{\mathbf{n}}_i‖‖{\mathbf{n}}_j‖}}\approx {\displaystyle \frac{{\mathbf{v}}_{2,i}^T{\mathbf{v}}_{2,j}}{‖{\mathbf{v}}_{2,i}‖‖{\mathbf{v}}_{2,j}‖}}.$$

(13)

The reflection intensity of matched points exhibits similar distributions and is thus chosen as the second similarity. We assume that the reflection intensity around points $i$ and $j$ follows normal distributions $N_i\left({\mu }_i,{\sigma }_i^2\right)$ and $N_j\left({\mu }_j,{\sigma }_j^2\right)$, in which ${\mu }_i$, ${\sigma }_i$ and ${\mu }_j$, ${\sigma }_j$ represent the mean value and standard deviation for points $i$ and $j$, respectively. The distances from the two points to the sensor center are $r_i$ and $r_j$, with $r_j=\kappa r_i$ (where $\kappa$ is the scale factor), with their reflection intensity satisfies scaling law as well. Therefore, by applying Kullback–Leibler (KL) divergence theory to the reflection intensity of matched points [26], a modified KL divergence model is yielded based on Lambertian theory:

$$\left\{\begin{array}{c}{\mu }_j={\kappa }^2{\mu }_i\\ {\sigma }_j^2={\kappa }^4{\sigma }_i^2\\ K{L}_{sym}={\displaystyle \frac{{\mu }_i^2}{4{\sigma }_i^2}}\left({\displaystyle \frac{1}{{\kappa }^4}}+1\right){\left(1-{\kappa }^2\right)}^2+{\displaystyle \frac{1}{4{\kappa }^4}}+{\displaystyle \frac{{\kappa }^4}{4}}-{\displaystyle \frac{1}{2}}\end{array}\right..$$

(14)

Since the weight coefficient is not larger than 1, a Gaussian function is used to normalize the Kullback–Leibler (KL) divergence, thereby obtaining the reflection intensity similarity $S_I$ in Equation (15).

$$S_I=\mathit{exp}\left(-{\displaystyle \frac{{\left(K{L}_{sym}^{norm}\right)}^2}{2{\eta }^2}}\right),$$

(15)

where $\eta$ is the Gaussian scale factor.

To avoid the sparsity error, the average planarity $S_p$ of the points $i$ and $j$ is introduced as the third weight coefficient.

$$S_p={\displaystyle \frac{1}{2}}\left({\displaystyle \frac{{\lambda }_1^i-{\lambda }_2^i}{{\lambda }_0^i}}+{\displaystyle \frac{{\lambda }_1^j-{\lambda }_2^j}{{\lambda }_0^j}}\right).$$

(16)

Eventually, the weight $w$ of the points $i$ and $j$ is defined as

$$w=S_G\cdot S_I\cdot S_p.$$

(17)

To ensure the real-time performance of the odometer, the planar feature point cloud of the current frame is matched with that of the local submap in the keyframe. The transformation matrix is obtained by minimizing the weighted Mahalanobis distance derived from the local geometric characteristics, as shown in Equation (18).

$$\left\{\begin{array}{c}R,t=\underset{R,t}{argmin}{\sum }_{i=0}^N{w}_i{r}_{L^i}\\ r_{L^i}=d_i^T{\left({\tilde{C}}_i^M+T{\tilde{C}}_i^P{T}^T\right)}^{-1}{d}_i\\ T=\left[\begin{array}{cc}R& t\\ 0_{1\times 3}& 1\end{array}\right],{\tilde{C}}_i=\left[\begin{array}{cc}{C}_i& 0_{3\times 1}\\ 0_{1\times 3}& 1\end{array}\right]\end{array}\right.,$$

(18)

where $r_{L^i}$ represents the Mahalanobis distance calculated by the GICP algorithm. $d_i$ represents the transformation error term between the current frame and the local submap of the keyframe. ${\tilde{C}}_i$ represents the covariance matrix of the points. The superscripts $P$ and $M$ represent the covariance matrices of the current frame and the submap, respectively. For the covariance matrix $C_i=\mathit{V}\mathit{\Lambda }{\mathit{V}}^T$, since all the feature points lie on a plane, the diagonal elements of $\mathit{\Lambda }$ are replaced with (1,1,ϵ), where ϵ is a very small constant, to regularize the covariance matrix $\mathit{\Lambda }$.

4. Online Filtering Method for Highly Dynamic Point Clouds

If the set of keyframe point clouds is denoted as $F$, after extracting the ground points, the points are divided into ground points $G$ and non-ground points $P$. Furthermore, the point cloud set obtained by transforming each keyframe point cloud into the global coordinate system and then splicing them together is the map $M$. If the highly dynamic point cloud map is represented as $M_d$, then the static point cloud map can be expressed as $M_s=M-M_d$. Removing point clouds from a map directly may result in point cloud loss and over-filtering, but removing only the point clouds from one or several frames allows the point clouds from other frames to compensate for the loss in the map. Therefore, we obtain the static map by filtering the object-level highly dynamic point clouds at each keyframe, as shown in Equation (19).

$$M_s={\bigcup }_{k=1}^N\left(G_k+P_k-{\bigcup }_{I_k^d\in I_k}{I}_k^d\right),$$

(19)

where $G_k$ is the ground point cloud of the kth keyframe, which is regarded as the static point cloud. $I_k^d$ represents the object-level highly dynamic point cloud set of the kth keyframe. $P_k$ denotes all object-level point clouds in the kth keyframe.

Objects that generate highly dynamic points, such as pedestrians, vehicles, and so forth, temporarily occupy a certain area in the map for a short time, while static points remain persistently fixed in place. Therefore, by comparing the grid occupancy between the current frame’s point cloud and the map, we can identify regions occupied by dynamic objects. Consequently, non-ground points within these dynamic regions can be classified as highly dynamic points.

First, to eliminate the influence of outliers in the point cloud, a pass-through filter is applied to extract the region of interest. The filtering range is set from $h_{min}$ to $h_{max}$. Then, the point cloud of the region of interest in the current keyframe is represented as

$$P_k^{int}=\left\{\left.p\in P_k\right|h_{max}{\ge h}_p\ge h_{min}\right\}.$$

(20)

The grid obtained from the point cloud of the region of interest $P_k^{int}$ in the global coordinate system is $G_k$. Thus the local submap can be represented by Equation (21).

$$M_k^{sub}=\left\{\left.p\in M\right|\Gamma \left(p\right)\in G_k\right\},$$

(21)

where the grid projection function $\Gamma \left(p\right)$ is defined as $\Gamma \left(p\right)=\left(x,y\right)=(floor\left(x\cdot re{s}_x\right),floor\left(y\cdot re{s}_y\right))$, $re{s}_x$ and $re{s}_y$ are the grid resolutions in the X and Y dimensions, respectively. $\left(x,y\right)$ is the index of the grid corresponding to the point $p$.

The pseudo-occupancy rate is defined as the difference between the maximum and minimum heights of the point cloud within the grid. Therefore, the rates $\Delta h_{i,j}^k$ and $\Delta h_{i,j}^{M_k^{sub}}$ of the current keyframe $k$ and the local submap are expressed as follows:

$$\begin{array}{c}\Delta h_{i,j}^k=\mathit{max}\left(z_{G_{i,j}}\right)-z_{k,i,j}^{low}\\ \Delta h_{i,j}^{M_k^{sub}}=\mathit{max}\left(z_{G_{i,j}}\right)-z_{i,j}^{M_k^{sub},low},\end{array}$$

(22)

where $G_{i,j}$ represents the grid with index $\left(i,j\right)$; $z_{i,j}^{M_k^{sub},low}$ and $z_{k,i,j}^{low}$ represent the minimum values of the point cloud heights of the grid with index $\left(i,j\right)$ in the local submap and the current keyframe $k$, respectively.

The pseudo-occupancy ratio determines the occupancy status of dynamic point clouds in the current frame’s grid, and it is defined as the ratio between the pseudo-occupancy rate of the current frame and the submap grid, as shown in Equation (23).

$${\displaystyle \frac{\Delta h_{i,j}^k}{\Delta h_{i,j}^{M_k^{sub}}}}>{\tau }_h,$$

(23)

where ${\tau }_h$ is the threshold for the pseudo-occupancy ratio. If the pseudo-occupancy ratio exceeds ${\tau }_h$, the grid $\left(i,j\right)$ in the current frame is considered to contain dynamic point clouds.

The joint distribution of the binary states $\mathit{m}$ for pseudo-occupancy grids is expressed as follows:

$$p\left(\mathit{m}⌋{\mathit{z}}_{1:\mathit{t}}\right)={\prod }_{i}p\left({\mathit{m}}_{\mathit{i}}⌋{\mathit{z}}_{1:\mathit{t}}\right),$$

(24)

where ${\mathit{m}}_{\mathit{i}}$ represents the occupancy state of the ith grid, and ${\mathit{z}}_{1:\mathit{t}}$ denotes all observations up to frame $t$, where each observation corresponds to the difference between the scan at frame $i$ and the map.

We update the probability information in the map grid according to Bayes’ rule; that is,

$$\mathit{log}it\left(p\left(\left.m_i\right|z_{1:t}\right)\right)=\mathit{log}it\left(p\left(\left.m_i\right|z_{1:t-1}\right)\right)+\mathit{log}it\left(p\left(\left.m_i\right|z_t\right)\right)-\mathit{log}it\left(p\left(m_i\right)\right),$$

(25)

where $\mathit{log}it\left(p\left(x\right)\right)$ defines the occupancy and non-occupancy of the grid, and $\mathit{log}it\left(p\left(x\right)\right)=\mathit{log}\left(p\left(x\right)/\left(1-p\left(x\right)\right)\right)$.

The update rule for $\mathit{log}it\left(p\left(x\right)\right)$ is as follows:

$$\mathit{log}it\left(p\left(\left.m_{i,j}\right|z_k\right)\right)=\left\{\begin{array}{c}\kappa \mathit{log}it\left(p\left(\left.m_{i,j}\right|z_k\right)\right),{\displaystyle \frac{\Delta h_{i,j}^k}{\Delta h_{i,j}^{M_k^{sub}}}}>{\tau }_h\\ \mathit{log}it\left(p\left(\left.m_{i,j}\right|z_k\right)\right),{\displaystyle \frac{\Delta h_{i,j}^k}{\Delta h_{i,j}^{M_k^{sub}}}}<{\tau }_h,{\displaystyle \frac{G_{i,j}^k}{{P_{k,i,j}^{int}}_{gd}}}>{\tau }_{gd}\\ \mathit{log}it\left(p_0\right)\end{array}\right.,$$

(26)

In Equation (26), for ${\displaystyle \frac{\Delta h_{i,j}^k}{\Delta h_{i,j}^{M_k^{sub}}}}>{\tau }_h$, $\mathit{log}it\left(p\left(\left.m_{i,j}\right|z_k\right)\right)$ is increased by a gain factor $\kappa$ ($\kappa >1$). Therefore, the updated probability value $\mathit{log}it\left(p\left(\left.m_{i,j}\right|z_k\right)\right)$ increases, and $\mathit{log}it\left(p\left(\left.m_{i,j}\right|z_k\right)\right)>\mathit{log}it\left(p_0\right)$. For ${\displaystyle \frac{\Delta h_{i,j}^k}{\Delta h_{i,j}^{M_k^{sub}}}}<{\tau }_h$ and ${\displaystyle \frac{G_{i,j}^k}{P_{k,i,j}^{int}}}>{\tau }_{gd}$, the grid is still likely occupied by dynamic points and is updated using $\mathit{log}it\left(p\left(\left.m_{i,j}\right|z_k\right)\right)$. Otherwise, $\mathit{log}it\left(p\left(x\right)\right)$ retain its initial value $\mathit{log}it\left(p_0\right)$. $p_0$ represents an initial prior probability.

In order to avoid deleting objects with ambiguous boundaries by mistake, a curved voxel point cloud clustering algorithm is employed [27]. This algorithm detects the point cloud corresponding to the object in the map, enabling subsequent object-level segmentation of the highly dynamic point cloud in the map. If all objects containing dynamic grids are filtered out without considering their dynamic attributes, static objects may be inadvertently removed. Therefore, an object dynamic score is defined for each object point cloud, as shown in Equation (27), serving as a dynamic evaluation criterion to accurately distinguish and separate the highly dynamic object point cloud completely.

$$Scor{e}_i={\displaystyle \frac{1}{Num\left({Cl}_i\right)}}{\sum }_{k=1}^{Num\left({Cl}_i\right)}\mathit{log}it\left(f\left(G\left(p_k^{{Cl}_i}\right)\right)\right),$$

(27)

where $Num\left({Cl}_i\right)$ represents the number of point clouds of the ith object. $G\left(p_k^{{Cl}_i}\right)$ represents the grid corresponding to each point in the object ${Cl}_i$. The function $f\left(.\right)$ returns the corresponding value based on the probability value of the grid. The definition is as follows:

$$f\left(G\left(p\right)\right)=\left\{\begin{array}{c}p\left(\left.m_i\right|z_{1:t}\right),p\left(\left.m_i\right|z_{1:t}\right)\ge p_0\\ p_{-}\end{array}\right.,$$

(28)

where the true probability of the grid is returned when the grid probability exceeds the initial value $p_0$. Otherwise, a constant $p_{-}$ ($p_{-}<1$) is returned. It should be noted that when the grid is always occupied by a static object, the probability of the grid remains $p_0$, and its log odds value of the grid remains $\mathit{log}it\left(p_0\right)$. When both static and dynamic objects occupy the grid, the presence of $p_{-}$ reduced the object dynamic score, thereby decreasing the probability of mistakenly removing the static object. The final result is dynamic object point cloud.

$$D=\left\{\left.{Cl}_i\right|\mathit{log}i{t}^{-1}\left(Scor{e}_i\right)>{\tau }_{score}\right\},$$

(29)

where $\mathit{log}i{t}^{-1}$ is the inverse of the log odds function, converting the log odds back to a probability value. ${\tau }_{score}$ is the adaptive probability threshold for dynamic objects. ${Cl}_i$ is the point cloud cluster.

When performing point cloud clustering, two objects located close to each other may be mistakenly identified as a single cluster. As a result, static objects within may be removed incorrectly. Since such point clouds generally occupy a large number of grids but contain a small proportion of highly dynamic grids, they can be distinguished based on the total number of occupied grids and the proportion of dynamic grids, as shown in Equation (30).

$$\left\{\begin{array}{c}Num\left(G\left({Cl}_i\right)\right)>{\tau }_{grid}\\ 0<{\displaystyle \frac{Num\left(G_{dynmic}\left({Cl}_i\right)\right)}{Num\left(G\left({Cl}_i\right)\right)}}<{\mu }_{dynamic}\end{array}\right..$$

(30)

The weights of point cloud features are considered by LiDAR-IMU-GNSS odometry when calculating the Mahalanobis distance between the matching point clouds.

$Num\left(G\left({Cl}_i\right)\right)$ represents the number of grids occupied by the point cloud cluster ${Cl}_i$. ${\tau }_{grid}$ represents the minimum threshold of occupied grids. $Num\left(G_{dynmic}\left({Cl}_i\right)\right)$ represents the number of dynamic grids in the point cloud cluster ${Cl}_i$. ${\mu }_{dynamic}$ represents the maximum proportion threshold of dynamic grids.

After removing highly dynamic grids from the under-segmented point cloud, the static map is ultimately established.

5. Algorithm Validation

5.1. Accuracy of Proposed Odometry Systems

For the sake of accurately validating the proposed weight matching LiDAR-IMU-GNSS odometry, our method is tested on the KITTI, UrbanLoco [24], and Newer College [25] (NCD) datasets, and compared with the LIO-SAM and FAST-LIO2 methods.

We present the trajectory comparison between the proposed algorithm and the LIO-SAM algorithm on 00, 05, 09, and 10 sequences of the KITTI dataset. Figure 2 illustrates the typical results from 00 and 05 sequences. The ground truth trajectory is shown in green, the LIO-SAM trajectory in blue, and the trajectory of the proposed algorithm in red. Clearly, the trajectory produced by the proposed algorithm is significantly closer to the ground truth, indicating that it achieves higher accuracy than the LIO-SAM algorithm.

The root-mean-squared error (RMSE) of the absolute trajectory error (ATE) was used to evaluate the error across the entire trajectory. The RMSE is defined as follows:

$$RMS{E}_{ATE}=\sqrt{{\displaystyle \frac{1}{N}}{\sum }_{i=1}^N{‖trans\left(T_{gt}^{-1}{T}_{est,i}\right)‖}^2},$$

(31)

where $N$ represents the number of frames in the dataset. $trans(.)$ denotes the extraction of the translational component of the pose, and the Euclidean norm is used to compute the vector magnitude in the RMSE calculation.

Table 1. The RMSE of three algorithm frameworks on datasets (RMSE/m).

Dataset	LIO-SAM	FAST-LIO2	Our Method
00	5.8	3.7	1.1
01	11.3	10.8	10.9
02	11.8	13.2	12.9
03	-	-	-
04	1.2	1.0	0.9
05	3.0	2.8	2.5
06	1.0	1.3	1.1
07	1.2	1.1	1.1
08	4.4	3.9	3.9
09	4.3	4.8	2.1
10 UrbanLoCo-CA-1 UrbanLoCo-CA-2 UrbanLoCo-HK-1 UrbanLoCo-HK-2 NCD-long-13	2.4 5.295 11.635 1.342 1.782 0.187	1.7 10.943 7.901 1.196 1.802 0.194	1.5 4.615 7.189 1.159 1.768 0.163
NCD-long-14	0.195	0.212	0.185
NCD-long-15	0.162	0.173	0.169

summarizes the RMSE values for three algorithms across subsets of the KITTI, UrbanLoco, and NCD datasets. The smaller the error is, the better the result is. The results indicate that the proposed algorithm achieves higher accuracy than the other two open-source algorithms in most datasets, demonstrating superior performance.

5.2. Comparison of Highly Dynamic Point Cloud Filtering Algorithm

Figure 3 and Figure 4 show the visualization results of the proposed highly dynamic point cloud filtering algorithm, the Removert and ERASOR algorithms based on sequence 00 and 05 of the SemanticKITTI dataset, meantime, the truth is given as well. In the figures, the red point clouds represent the highly dynamic object point clouds detected by each algorithm, while the blue ones represent the static point clouds.

In Figure 3a,b, compared with the ground truth, both the Removert and ERASOR algorithms effectively filter out dynamic points, but they incorrectly remove a large number of static points. This results in an excessive number of false-positive detections, ultimately producing a relatively sparse map. In contrast, the method proposed in this paper ensures high detection precision while generating fewer false positive points. In Figure 4, using the sequence 05 dataset, the Removert algorithm achieves a relatively low detection precision, and the ERASOR algorithm has a relatively high proportion of false positives, while the proposed algorithm outperforms both on this dataset.

Furthermore, using sequences 00, 01, 05, and 07 from the SemanticKITTI dataset, which contain numerous dynamic objects, the accuracy of the proposed highly dynamic point cloud filtering algorithm is compared with the Removert and ERASOR algorithms.

Table 2. Results of SemanticKITTI datasets.

Dataset	Method	PR	RR	F1
00	Removert	86.8	90.6	0.88
	ERASOR	93.9	97.0	0.95
	Our Algorithm	98.7	98.5	0.98
01	Removert	95.8	57.0	0.71
	ERASOR	91.8	94.3	0.93
	Our Algorithm	96.8	94.6	0.95
05	Removert	86.9	87.8	0.87
	ERASOR	88.7	98.2	0.93
	Our Algorithm	97.5	96.3	0.96
07	Removert	80.6	98.8	0.88
	ERASOR	90.6	99.2	0.948
	Our Algorithm	96.6	98.9	0.977

presents the Preservation Rate (PR), Recall Rate (RR), and F₁ score of the proposed highly dynamic point cloud filtering module in comparison with the Removert and ERASOR algorithms. The three metrics are defined as follows:

$$\left\{\begin{array}{c}PR={\displaystyle \frac{TP}{TP+FP}}\\ RR={\displaystyle \frac{TP}{TP+FN}}\\ F_1=2\times {\displaystyle \frac{PR\times RR}{PR+RR}}\end{array}\right.$$

(32)

where TP (True Positive) represents the number of samples that are correctly detected as dynamic object categories. FP (False Positive) represents the number of samples that are actually static but incorrectly detected as dynamic categories. FN (False Negative) represents the number of samples that are actually dynamic but incorrectly detected as static categories. The values of all three metrics range from 0 to 1, with values closer to 1 indicating better performance.

Table 2 presents the performance of Removert, ERASOR, and the proposed method across three metrics on the SemanticKITTI dataset. The results demonstrate that the proposed highly dynamic point cloud filtering algorithm achieves high accuracy, with fewer missed and false detections. It can effectively filter highly dynamic objects in single-frame scanned point clouds and eliminate the “long trailing shadows” in the map caused by moving objects.

5.3. Online Filtering Experiment

A dataset at an intersection on the campus of Beijing Institute of Technology (BIT) was obtained to evaluate the proposed online highly dynamic point cloud filtering algorithm in this paper. The intersection contains numerous highly dynamic objects, such as heavy traffic and a large pedestrian flow.

In the experiment, the Wuling Hongguang miniEV serves as the test vehicle, equipped with an environmental perception system including three sensors. The Robosense Helio-32 mechanical LiDAR offers a 360° horizontal field of view and a 70° vertical field of view, with the ranging accuracy within 2 cm and a scanning frequency of 10 Hz. The GW-NAV100B is a dual-antenna, high-precision MEMS integrated navigation system. It incorporates a built-in three-axis gyroscope, a three-axis accelerometer, and a four-mode (BD/GPS/GLONASS/GALILEO) receiver, enabling it to measure the velocity, position, and attitude of the test vehicle. Xsens Mti-630 is a 9-axis IMU with a 100 Hz output frequency. During the data collection process, the total duration of the experiment was approximately 1000 s, covering a distance of about 3000 m. To construct local submaps, the data was divided equally into five segments, with each being approximately 600 m long. The parameters of the Robosense Helio-32 mechanical LiDAR and GW-NAV100B are listed in

Table 3. Parameters of Robosense Helio-32 mechanical LiDAR and GW-NAV100B.

Robosense Helio-32			GW-NAV100B
Parameters		Values	Parameters	Values
vertical field	range	+15°~−55°	frequency	100 Hz
		2°(+15°~+7° and −8°~−55°)		(10 Hz for GNSS)
	resolution	1.5(+7°~+4°)	position resolution	0.01 m in horizonal field
		1.33(+4°~−8°)		0.15 m in vertical field
horizontal field	range	360°	velocity resolution	0.03 m/s
	resolution	0.2°		0.2° in roll
			attitude resolution	0.2° in pitch
				0.1° in yaw

.

Two typical highly dynamic scenarios are shown in Figure 5 and Figure 6. The black point clouds represent static environmental point clouds, while the red point clouds represent the highly dynamic objects detected by the method proposed in this paper.

In Figure 5 and Figure 6, it can be observed that at the intersection, there are a large number of point cloud “long trailing shadows” caused by pedestrians, cyclists, and vehicles, which degrades the quality of the point cloud map. Typical highly dynamic objects, such as driving vehicles, cyclists and pedestrians, are clearly identifiable. In Figure 5, the blue and green boxes, respectively, enclose the detailed point cloud maps of vehicles and cyclists; in Figure 6, the blue and green boxes, respectively, enclose the detailed point cloud maps of vehicles and pedestrians. As demonstrated in Figure 5 and Figure 6, the online highly dynamic point cloud filtering algorithm proposed in this paper can effectively detect major highly dynamic objects such as pedestrians, cyclists, and vehicles in highly dynamic scenarios.

6. Conclusions

This paper improves the mapping method with weight matching LiDAR-IMU-GNSS odometry and a proposed online highly dynamic point cloud filtering algorithm. The weight matching LiDAR-IMU-GNSS odometry includes IMU pre-integration, ground point extraction, motion compensation, and weight feature point matching. Specifically, within the weight matching LiDAR-IMU-GNSS odometer, a method for point cloud keyframe selection and submap construction based on an adaptive threshold is designed. Ground point clouds and planar point clouds are extracted separately using the PMF and KD-Tree methods. A weight feature point matching method based on geometric-reflectance intensity similarity is proposed to improve point cloud matching. Subsequently, by using the pseudo-occupancy ratio between the current frame point cloud and the previous local submap as an observation, grid probability values are updated to obtain the distribution of dynamic features in the map. In addition, the curved voxel point cloud clustering algorithm is used to obtain object-level point cloud clusters. This approach enables comprehensive filtering of highly dynamic object-level point clouds.

The higher accuracy of the weight matching LiDAR-IMU-GNSS odometer is valid in the KITTI, UrbanLoco, and NCD datasets in comparison with the LIO-SAM and FAST-LIO2 methods. The proposed highly dynamic point cloud filtering algorithm outperforms the Removert and ERASOR algorithms based on the sequences from the SemanticKITTI dataset. Finally, online mapping with highly dynamic point cloud filtering is applied in a real-time dataset, effectively detecting typical highly dynamic objects such as driving vehicles, cyclists, and pedestrians. We hope this research will contribute to advancements in high-accuracy online mapping with dynamic object filtering and promote the development of navigation and locomotion in autonomous driving and robotics.