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

1. Introduction

LiDAR mapping technology has achieved remarkable progress and been widely applied in various fields, including autonomous driving[1,2,3], SLAM system[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 shows that information fusion obtained from a Global Navigation Satellite System(GNSS), an Inertial Measurement Unit(IMU) and a LiDAR sensor has achieved high-precision mapping and localization in complex environments[12,13]. But in reality, the presence of dynamic objects poses significant challenges to precision in LiDAR mapping. On one hand, conventional point cloud matching algorithms generally assume a static environment. This assumption holds in most cases due to short single-frame scanning time, and thus the impact of dynamic objects on odometry accuracy can be negliable[14]. But when objects move in 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] appeared into the generated map. The resulting map redundancy consequently misleads localization and path planning terribly, decreasing accuracy and safety in navigation decision-making. Therefore, to effectively suppress and eliminate the side effects of highly dynamic point clouds on map is of great significant in enhancing the reliability and practicality of LiDAR mapping systems.

According to real-time performance of disposing 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 usually select several consecutive point cloud frames as a reference, and then compared 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 initial pose of frontend odometry and local submaps to filter dynamic points on the current frame adaptively and iteratively, eventually 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 in the 3D-MiniNet, which enables online detection and filtering of dynamic point clouds. Post-processing methods utilize all frames during the entire SLAM cycle as reference, thus achieving a higher accuracy and precision. Peopleremover algorithm[20] constructs a regular voxel occupancy grid and then determine 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, the real-time performance of maps must be guaranteed as a priority. However, at the same time, to optimize the effectiveness of decision-making and the robustness of motion behaviors, the map is required to be built in a 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, and then employs a pseudo-occupancy grid-based method to filter object-level highly dynamic point clouds in real time, ultimately effectively enhancing the 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. Compared to the Removert and ERASOR algorithms, the filtering of highly dynamic point clouds in this paper is more effective. The proposed algorithm is applied in 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 of high accuracy multi-sensor fusion of LiDAR-IMU-GNSS and object-level highly dynamic point filtering. The framework consists of data preprocessing, weight feature point matching, and dynamic point filtering, as shown 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, including IMU pre-integration, ground points through PMF and motion compensation.

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

iii) Dynamic point filtering: dynamic objects is removed from the point cloud and map is built in real time by means of 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, including attitude, velocity and position information. After acquirement of IMU data and consideration for bias and noise, the predicted values of pre-integration for attitude $R_i$, velocity $v_i$ and position $p_i$ between frame $i$ and $j$ is derived by discrete-time integration method. Thus, yielding their relative differences $\Delta R_{i,j}$, $\Delta v_{i,j}$ and $\Delta p_{i,j}$ 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 linear acceleration by accelerometer at k^th frame, respectively. $n_k^{\omega }$ and $n_k^a$​ denote the bias noise of the angular velocity and acceleration at k^th frame. $b_k^{\omega }$ and $b_k^a$ are the measurement biases of the angular velocity and acceleration at k^th frame. $\Delta t_k$ is the time interval between k^th frame and (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 (k+1)^th frame can be predicted by integrating the state at k^th frame.

Assuming that the IMU biases $b_k^{\omega }=const$ and $b_k^a=const$ within a short period, the noise terms can be simplified further. According to the linear approximation in 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}$. And 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 the right-hand side are the measured values obtained through IMU integration and a random noise term. In order to explicitly represent the measured values in Equation (2), it is necessary to obtain the state transition equations for 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 operator $Log\left(.\right)$ which denotes the mapping from a rotation matrix to a rotation vector, the noise term can be simplified linearly. The first-order term is kept and 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. $n_{j-1}^{\omega }$ represents (j-1)^th the bias noise of the angular velocity. $T$ is transpose.

Assuming that 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 satisfy 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{and}{\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{and}{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. Consequently, the state transition equation for covariance matrix of 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)

IMU biases varying with time severely influences the accuracy of measurement. By assuming that pre-integrated measurements vary with IMU biases linearly, we retain the first-order partial derivatives of pre-integrated measurements relative 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 for reducing storage. In order to achieve the adaptive adjustment of the keyframe threshold, parameter $d_k$ is determined according to the surrounding. Therefore, the keyframe threshold $d{i}_k$ is expressed in Equation (8).

$$d{i}_k=\left\{\begin{array}{c}5.0，d_k>20\\ \mathrm{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 constant, and $\alpha =0.9$, $\beta =0.1$.

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

PMF optimizes window dimension by gradually increasing the window size of the filtering. The update principles of the filtering window size $w_k$ and the height difference threshold $d{h}_k$ are shown 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 k^th iteration. $b$ represents the initial window size. $d{h}_0$ and $d{h}_{max}$ refer to the initial and maximum height difference threshold. $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 speed, motion distortion compensation for point clouds becomes essential. If IMU data is available, the IMU-based motion compensation in continuous time is used. Otherwise, in the case of IMU data dropout, the motion compensation method based on the constant velocity model is adopted. Assuming that the current frame keeps the same motion state as the previous frame of the LiDAR, the LiDAR pose at each timestamp within the current frame can be calculated by interpolation.

$$\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

Mahalanobis distance between points is generally use as the criterion for feature point matching, and further in order to obtain the optimized pose. However, this method with the same weight assigned to each matched point fails to accurately represent the influence from the uncertainty of different matched point pairs. During the process of point matching, the weight of false matches should be significantly reduced. Besides, the position uncertainty of the two matched points produces noise, and thus the weight of such points should decrease as their uncertainty increases.

For the sake of both improving point matching accuracy and promoting calculation efficiency, an eigenvalue-based approach is employed for extracting planar feature points. Thus, the centroid and the covariance matrix of the point distribution around 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}\right.\mathrm{and}\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.$$

(11)

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

If eigenvalues satisfy Equation (12), 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 the 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 $\mathit{n}$ of the planar feature points $i$ in one point cloud and $j$ in the target point cloud, separately. Note that ${\lambda }_2$ is the smallest eigenvalue, its eigenvector ${\mathit{v}}_2$ is thus approximately equivalent to the normal vector of the corresponding plane. The subscript $i$ and $j$ correspond to points $i$ and points $j$. Therefore, the geometric similarity $S_{G,i}$ can be expressed as,

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

(13)

The reflection intensity of matched points exhibits similar distributions, thus chosen as the second similarity. Assuming 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)$, respectively, and the distances from two points to the sensor center are $r_i$ and $r_j$, with $r_j=\kappa r_i$ (where $\kappa$ is the scale factor), their reflection intensity satisfies scaling law as well. Therefore, a modified Kullback-Leibler (KL) divergence model is yield 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, and thus the reflection intensity similarity $S_I$​ is obtained 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 Gaussian scale factor.

To avoid the sparsity error, the average planarity $S_p$ of 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 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 transform matrix is obtained by minimizing the weighted Mahalanobis distance obtained 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 constructed 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 superscript $P$ and $M$ represents the covariance matrix 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 are planar points, 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 $F$, after extracting the ground points, they 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$M_d$, then the static point cloud map can be expressed as $M_s=M-M_d$. Removing point cloud in a map probably leads to point cloud loss and over-filtering, but removing only the point clouds of one frame or several frames, the point clouds of other frames can still fill the loss in the map. Thus, we obtain the static map by filtering the object-level highly dynamic point clouds at each keyframe, 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 k^th keyframe, which is regarded as static point cloud. $I_k^d$ represents object-level highly dynamic point cloud set of the k^th keyframe. $P_k$ denotes all object-level point clouds in the k^th keyframe.

Objects that generate highly dynamic points, such as pedestrians, vehicles, and so forth, temporarily occupy a certain area in the map within a short time, while static points persistently remain in fixed areas. 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 abnormal points 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 z}_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 in the 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. And thus, the rates $\Delta h_{i,j}^k$ and $\Delta h_{i,j}^{M_k^{sub}}$ of the current keyframe $k$ and the local sub-map are expressed as follows,

$$\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}$$

(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 is defined to the ratio between the current frame and the submap grid 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 i^th 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.

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)

where $\mathit{log}it\left(p\left(x\right)\right)$ are increased by a gain factor $\kappa$ ($\kappa >1$) for ${\displaystyle \frac{\Delta h_{i,j}^k}{\Delta h_{i,j}^{M_k^{sub}}}}>{\tau }_h$ , and thus the updated probability value $p\left(\left.m_{i,j}\right|z_k\right)>p_0$. 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 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 boundary by mistake, a curved voxel point cloud clustering algorithm is adopted[26]. This algorithm helps detect the point cloud corresponding to the object in the map, and then the object-level segmentation of the highly dynamic point cloud in the map is further carried out. If all objects containing dynamic grids are filtered out without distinguishing the dynamic attributes of the objects, static objects may be deleted by mistake. Therefore, the object dynamic score for each object point cloud is defined in Equation (27) as the dynamic evaluation criterion to 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 i^th object. $G\left(p_k^{{Cl}_i}\right)$ represents the grid corresponding to each point in the object ${Cl}_i$, and the function $f\left(.\right)$ returns the corresponding value according to 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 is greater than 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 is always $p_0$​, and the log odds value of the grid is always $\mathit{log}it\left(p_0\right)$​​. When both static and dynamic objects exist in the grid, due to the existence of $p_{-}$, the object dynamic score is reduced, and the probability of mistakenly deleting the static object is also reduced. The finally obtained dynamic object point cloud is,

$$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, and ${\tau }_{score}$​ is the adaptive probability threshold for dynamic objects. ${Cl}_i$ is the point cloud cluster.

When performing point cloud clustering, it is possible that two objects close to each other are identified as a single point cloud cluster. As a result, static objects within may be removed incorrectly. Since such point clouds typically occupy a large number of grids but 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 have been taken into account by LiDAR-IMU-GNSS odometry when computing the Mahalanobis distance of the matching point cloud.

where $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 deleting highly dynamic grids in the under-segmented point cloud, the static map is eventually established.

5. Algorithm Validation

5.1. Accuracy of Proposed Odometry Systems

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

Figure 2 presents the trajectory comparison between the proposed algorithm and the LIO-SAM algorithm on 00 sequences of the KITTI dataset. The ground truth, LIO-SAM trajectory and the trajectory of the proposed algorithm are shown in green, blue and red colors, separately. Obviously, the trajectory of the proposed algorithm is significantly closer to the ground truth, indicating that its accuracy is higher than that of 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 extracting the translational component of the pose, and the Euclidean norm is used for vector magnitude computation in the RMSE calculation.

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

5.2. Comparison of Highly Dynamic Point Cloud Filtering Algorithm

Figure 3 and 4 gives the visualization result of the proposed highly dynamic point cloud filtering algorithm, the Removert and ERASOR algorithms based on the sequence 00 and 05 of the SemanticKITTI dataset. In the figures, the red point clouds represent the highly dynamic object point clouds detected by the corresponding algorithm, while the blue ones represent the static point clouds.

In Figure 3(a) and 3(b), compared with the ground truth, it can be seen that the Removert algorithm and ERASOR algorithm can basically filter out dynamic points, but they incorrectly remove a large number of static points. Finally, excessive false positives points in detection lead to a relatively sparse map generated. In contrast, the proposed method in this paper ensures a high detection precision while having fewer false positive points. In Figure 4, based on 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 algorithm proposed in this paper outperforms the above two algorithms on this dataset.

Furthermore, based on the sequences 00, 01, 05, and 07 of the SemanticKITTI datasets, which contain many dynamic objects, the accuracy of the proposed highly dynamic point cloud filtering algorithm is compared with Removert and ERASOR algorithms. Table 2 presents the Preservation Rate (PR), Recall Rate (RR), and F1 score of the proposed highly dynamic point cloud filtering module compared with 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 detected as dynamic categories. FN (False Negative) represents the number of samples that are actually dynamic but detected as static categories. The values of all three metrics range from 0 to 1, and the closer they are to 1, the better the performance.

Table 2 presents the performance of Removert, ERASOR, and the proposed method in this paper across three metrics on the SemanticKITTI dataset. It can be observed that the proposed highly dynamic point cloud filtering algorithm in this paper exhibits high accuracy, with fewer missed detections and false detections. It can effectively filter high 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) is obtained in order to test 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, which equips with an environmental perception system including three sensors. The Robosense Helio-32 mechanical LiDAR offers a 360° horizontal field of view and 70° vertical field of view, with the ranging accuracy within 2cm and the 10Hz scanning frequency. The GW-NAV100B is a dual-antenna high-precision MEMS integrated navigation. 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 car. Xsens Mti-630 is a 9-axis IMU with a 100Hz output frequency. During the data collection process, the total duration in this experiment was approximately 1000s, covering a distance of about 3000m. Meantime, to construct local sub-maps, the data was equally divided into five segments, with each is approximately 600m long.

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

In Figure 5 and 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, can be picked up. 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 can be seen from Figure 5 and 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 points 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 sub-map construction based on an adaptive threshold is designed. Ground point clouds and plane point clouds are extracted respectively using PMF and KD-Tree methods. A weight feature point matching method based on geometric-reflectance intensity similarity is proposed, which improves the accuracy of point cloud matching. Subsequently, taking the pseudo-occupancy ratio between the current frame point cloud and the previous local sub-map as an observation, the probability values of the grid are updated to obtain the distribution of dynamic feature in the map. In addition, the curved voxel point cloud clustering algorithm is used to obtain object-level point cloud clusters. Thereby, this algorithm realizes complete 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 LIO-SAM and FAST-LIO2 methods. The proposed highly dynamic point cloud filtering algorithm outperforms the Removert and ERASOR algorithms based on the sequences of the SemanticKITTI dataset. Finally, online mapping with highly dynamic point cloud filtering is applied in a real-time dataset, and typical highly dynamic objects, such as driving vehicles, cyclists and pedestrians, can be clearly picked up. We hope that this research can benefit the research about high accuracy mapping online with the filtering of highly dynamic objects, and promote the development of navigation and locomotion of in autonomous driving and robotics.