LiDAR-Inertial Odometry in Dynamic Driving Scenarios using Label Consistency Detection

We denote $(\cdot)^{w}$, $(\cdot)^{l}$ and $(\cdot)^{b}$ as a 3D point in the world coordinate, the LiDAR coordinate and the IMU coordinate respectively. The world coordinate is coinciding with $(\cdot)^{b}$ at the starting position.

We denote the LiDAR coordinate for taking the $i_{th}$ sweep at time $t_{i}$ as $l_{i}$ and the corresponding IMU coordinate at $t_{i}$ as $b_{i}$, then the transformation matrix (i.e., external parameters) from $l_{i}$ to $b_{i}$ is denoted as $\mathbf{T}_{l_{i}}^{b_{i}}\in SE(3)$, which consists of a rotation matrix $\mathbf{R}_{l_{i}}^{b_{i}}\in SO(3)$ and a translation vector $\mathbf{t}_{l_{i}}^{b_{i}}\in\mathbb{R}^{3}$. The external parameters are usually calibrated once offline and remain constant during online pose estimation. Therefore, we can represent $\mathbf{T}_{l_{i}}^{b_{i}}$ as $\mathbf{T}_{l}^{b}$ for simplicity. The pose from the IMU coordinate $(\cdot)^{b_{i}}$ to the world coordinate $(\cdot)^{w}$ is strictly defined as $\mathbf{T}_{b_{i}}^{w}$.

The entire system maintains two global maps: the tracking-map and the output map. The former is utilized for state estimation, while the latter is utilized for label consistency detection and serves as the final reconstruction outcome. In Dynamic-LIO, the tracking-map has already removed out the vast majority of dynamic points. However, to prevent over-filtering that could lead to insufficient geometric information for LIO, we refrain from further processing the tracking-map and instead focus on the output map (as illustrated in Sec. IV-E). Thus compared to the tracking-map, the dynamic points in the output map are removed more thoroughly. Both the tracking-map and the output map are managed by voxel, whose voxel resolution is $1.0\times 1.0\times 1.0$ (unit: m) and each voxel contains a maximum of 20 points.

Before dynamic point identification, we first construct binarized descriptors, i.e., ground label and non-ground label, for each 3D point of current sweep. We utilize a fast 2D connected component method [14], which is the same as LeGO-LOAM [29], to separate ground points from current input sweep with very low computational cost. By default, the LiDAR is mounted horizontally on the vehicle, and the LiDAR’s $z$-axis is perpendicular to the ground. If the LiDAR is equipped at an inclined angle, external parameters can be introduced to ensure the $z$-axis remains perpendicular. After changing the $z$-axis to perpendicular, we configure the range image $img$ [22] with the number of LiDAR lines ($N$ $scan$ in Fig. 3 (a)) as the vertical axis and the horizontal resolution ($horizon$ $scan$ in Fig. 3 (a)) as the horizontal axis, and position the 3D points of current sweep in their corresponding locations within the range image according to their horizontal and vertical line indices. In the range image, each point is equipped with $x$, $y$, $z$ coordinates in $(\cdot)^{l}$, as well as a Boolean variable $tag$, which is used to denote whether the point is a ground point. Initially, all $tag$ are set to false. We set the $tag$ of point at $img(1,1)$ to $true$. Then following the red arrows indicated in Fig. 3 (a), we calculate the pitch angles of two 3D points at the adjacent positions $img(1,2)$ and $img(2,1)$ to that at $img(1,1)$ respectively. Then the pitch angle of $img(2,1)$ to $img(1,1)$ can be calculated as:

If the calculated pitch angle is less than a certain threshold (e.g., 5 degree in our system), $img(2,1).tag$ is set as $true$, which means the corresponding 3D point is determined as ground point. Similarily, we can calculate the pitch angle of $img(1,2)$ to $img(1,1)$, and set value for $img(1,2).tag$. If the 3D point located at $img(1,2)$ or $img(2,1)$ is determined as ground point, we follow the blue arrows indicated in Fig. 3 (a) to indentify other points adjacent to them. The entire process is executed recursively, continuing until all pixels in the range image have been visited or the recursive exit condition is met. The visualization of segmented ground points is illustrated in Fig. 3 (b), where the orange points are labeled as “ground points” and the white points are labeled as “non-ground points”.

In the process of executing label consistency detection, it is necessary to find the nearest neighbors for each point of current sweep. Points that are close to the vehicle platform can reliably find their nearest neighbors, whereas points that are farther may fail due to the incomplete reconstruction of their locations. In outdoor driving scenarios (excluding extreme occlusion), the map structures within a 30 meter radius around the vehicle platform are usually already reconstructed. Therefore, we set a empirical threshold of 30 meters, and define points within 30 meters of the vehicle platform as fore-points and those beyond 30 meters as back-points. For fore-points and back-points, we employ determinations that are specifically tailored to their characteristics.

For a specific point $\mathbf{p}^{w}$ with the “non-ground point” label, to determine whether its label is consistent with surrounding points in the global map, we first need to search for the nearest neighbors of $\mathbf{p}^{w}$. An intuitive alternative is to utilize the 8-nearest neighbor search which is the same as the nearest neighbor search method for point-to-plane distance computation (as illustrated in Fig. 4 (a)) in LIO. Specially, we locate the voxel $V$ to which $\mathbf{p}^{w}$ belongs and the 8 voxels adjacent to $V$, and set all points in these voxels as candidate points. Subsequently, the 20 nearest points of $\mathbf{p}^{w}$ are identified from 9 candidate voxels by comparing the magnitudes of Euclidean distances to $\mathbf{p}^{w}$.

However, calculating the Euclidean distance of each candidate point to $\mathbf{p}^{w}$ is an extremely time-consuming process. When LIO builds the point-to-plane distance residuals, in order to ensure that the fitted plane can reflect as much of the geometric information around $\mathbf{p}^{w}$ as possible, we have to utilize the conventional 8-nearest neighbor search. Fortunately, only 600 point-to-plane distance residuals are required for estimating the pose of each sweep in our system, so the total computational overhead is acceptable. However, in order to ensure that the final output map does not contain dynamic points, it is necessary to determine each point of the current sweep, which requires searching the nearest neighbors for each point from the global map. A single sweep of one 32-line LiDAR can yield more than 50,000 points, making the conventional 8-nearest neighbor search inapplicable here.

In the proposed label consistency detection method, we qualitatively assess whether the non-ground point $\mathbf{p}^{w}$ is a dynamic point by comparing its label with those of its surround points. Since the surround points are not involved in quantitative calculations, it is not necessary to strictly satisfy the concept of nearest neighbors. Instead, an approximate approach, i.e., voxel-location-based nearest neighbor search, can be adopted to significantly reduce the computational cost. As illustrated in Fig. 4 (b), we locate the voxel $V$ to which $\mathbf{p}^{w}$ belongs and consider the other points within $V$ as the approximate nearest neighbors. Since the voxel map has a query operation with a computational complexity of $O(1)$, the entire nearest neighbor search process is extremely fast. In addition, the computational overhead associated with Euclidean distance is also saved. The voxel-location-based nearest neighbor search plays a crucial role in ensuring the low computational cost of label consistency detection, as evidenced by the results of the ablation study, which are documented in Sec. VI-G.

In Sec. IV-B, we categorize the points of current sweep into fore-points and back-points based on their distance from the vehicle platform. For dynamic point determination of fore-points and back-points, we employ the following two distinct modes.

Mode for fore-points. If the number of nearest neighbors is below a certain threshold (5 in our system), it indicates that the location of $\mathbf{p}^{w}$ was originally unoccupied, and thus $\mathbf{p}^{w}$ is classified as a dynamic point. If the number of nearest neighbors is sufficiently large (greater than 5), we calculate the proportion of non-ground points among all nearest neighbors. If this proportion is sufficiently low (less than 30$\%$), $\mathbf{p}^{w}$ is classified as a static point and added to both the tracking-map and the output map. Conversely, if the proportion is larger than 30$\%$, $\mathbf{p}^{w}$ is classified as a dynamic point and excluded in the map. Inevitably, this determination criteria potentially results in the erroneous removal of some static points in close proximity to the ground. Nonetheless, the points most commonly affected by this misfiltration are situated at the transition between walls and the ground. Despite the possibility of such misfiltration, the overall geometric integrity of the scene remains intact, and it does not affect the performance of the LIO system. The visualization of dynamic point determination results for fore-points is shown in Fig. 5.

Mode for back-points. If the number of nearest neighbors is below a certain threshold (5 in our system), we cannot identify the back-point as a dynamic point, because it is possible that the location has not yet been reconstructed, preventing the obtainment of the nearest neighbors. Such points are labeled as undetermined-points, and a determination will be made once the vehicle platform continues to move and the geometric structures of the locations of these points are recovered. To ensure that newly acquired point clouds can be properly registered during state estimation, it is necessary to incorporate the undetermined-points into the tracking-map. This will not significantly affect the accuracy of state estimation, as even if there are dynamic objects among the back-points, the number of LiDAR points scanned onto them is very sparse. As for the final output map, it is imperative to ensure that it contains as few dynamic points as possible, hence the determination for undetermined-points will be conducted subsequently. When the number of nearest neighbors is sufficiently large (greater than 5), the processing approach is the same as that for fore-points, and the static points are added to both the tracking-map and the output map. The visualization of dynamic point determination results for back-points is shown in Fig. 6.

As mentioned in Sec. IV-D, some back-points may fail to find nearest neighbors due to the incomplete reconstruction of their locations. For such points, we label them as undetermined points and place them in a container. As the vehicle platform continues to move forward, the geometric structure informations of previously unreconstructed positions are recovered (as shown in Fig. 7). Then we can make re-determination for those undetermined-points. When a point $\mathbf{p}_{u}^{w}$ in the undetermined point container is close to the current position of the vehicle platform (less than 30 meter), it is highly likely that the geometric structure information around $\mathbf{p}_{u}^{w}$ has been reconstructed. We can then determine whether $\mathbf{p}_{u}^{w}$ is a dynamic point. If the number of nearest neighbors is below a certain threshold (5 in our system), it suggests that the location of $\mathbf{p}_{u}^{w}$ was originally unoccupied, leading to the classification of $\mathbf{p}_{u}^{w}$ as a dynamic point. If the number of nearest neighbors is larger than the threshold of 5, we calculate the proportion of non-ground points among all nearest neighbors. If this proportion is sufficiently low (less than 30$\%$), it is classified as a static point and added to the output map. On the contrary, if the proportion is not smaller than the threshold of 30$\%$, it is classified as a dynamic point and would not be included in output map. If an undetermined-point is more than 30 meters away from the vehicle platform’s position for 10 consecutive sweeps, it is likely to be a sparse background point at far distance. Thus, we directly classify it as a static point and add it to the output map.

Fig. 8 illustrates the framework of our self-developed system Dynamic-LIO which consists of four main modules: cloud processing, static initialization, ESIKF based state estimation and dynamic point identification. The cloud processing module constructs binarized label (i.e., ground label or non-ground label) for each 3D point of current sweep. Subsequently, it performs spatial down-sampling to ensure uniform density of current point cloud. The static initialization module [11] utilizes the IMU measurements to estimate some state parameters such as gravitational acceleration, accelerometer bias, gyroscope bias and initial velocity. The ESIKF based state estimation module estimates the state of current sweep, and utilize the estimated pose to transform all points of current sweep from $(\cdot)^{l}$ to $(\cdot)^{w}$. The dynamic point identification module identifies dynamic points from current input point cloud data, to ensure that the global map contains only static points. The yellow rectangles indicate the specific locations of five steps of label consistency detection within the overall system framework.

To alleviate the substantial computational load caused by the overwhelming number of 3D points collected by a LiDAR in a single sweep, we implement a decimation strategy on point cloud. Initially, a uniform subsampling approach is applied to retain a single point for every set of four. Subsequently, we integrate the uniformly subsampled points into a voxel grid defined by a $0.5\times 0.5\times 0.5$ (unit: m) resolution, ensuring that each voxel is occupied by a solitary point.

It is worth noting that the down-sampling of current input sweep must be performed after binarized label construction. The reason is that down-sampling can disrupt the adjacency relationships of 3D points in range images, which can affect the execution of binarized label construction.

In our Dynamic-LIO, a static initialization procedure is employed to estimate essential parameters, including initial poses, initial velocities, gravitational forces, as well as biases in both accelerometer and gyroscope measurements. For a detailed elucidation of the methodology, please refer to reference [11].

We adapt the error state iterated Kalman filter (ESIKF) to perform state estimation, which is the same as Fast-LIO2 [35]. Fast-LIO2 utilized their own self-developed toolbox IKFoM [13] for the implementation of on-manifold Kalman filter, and we utilized the more widely recognized Eigen3 library [9] to implement this function. We have documented the entire execution process of our ESIKF in Appendices for readers to refer the details of implementation.

It is worth mentioning that during state estimation, we need to find the nearest neighbors for randomly selected 600 points of current sweep from the global map, and fit a plane with these nearest neighbors to construct point-to-plane distance constraints. To ensure that the final fitted plane as accurately as possible reflect the surrounding geometric information, we still use the conventional 8-nearest neighbor search method here. Additionally, we search for nearest neighbors based on the tracking-map. Compared to the output map, the tracking-map is more conducive to the accurate and robust running of LIO. Because the tracking-map retains enough geometric details while removing out the vast majority of dynamic points.

We compare our label consistency based dynamic point detection method with three state-of-the-art 3D point-based dynamic point detection methods, i.e., Removert [18], Erasor [20] and Dynamic Filter [10], on $semantic$-$kitti$ dataset [1]. Among them, Removert and Erasor are offline methods which need the pre-built map as input, Dynamic Filter and our method are online methods which do not rely on any prior information.

Results in Table II and Table III demonstrate that our method outperforms Dynamic Filter for almost all sequences in terms of higher PR and RR. Although Dynamic Filter achieves higher RR than our method on sequence $kitti\_1$, our result is very close to theirs, with only a 0.36$\%$ difference.

We compare our Dynamic-LIO with two state-of-the-art LIO systems for dynamic scenes, i.e., RF-LIO [25] and ID-LIO [33], on $ulhk$-$CA$ [32] and $urban$-$Nav$ [16] datasets. Both RF-LIO, ID-LIO have loop detection module, and use GTSAM [17] to optimize the factor graph. Thus, we also add the same loop detection module and global optimization module to Dynamic-LIO when comparing with them. Both results of RF-LIO and ID-LIO are recorded from their literatures because they have not released the code. The selected four sequences both encompass highly dynamic scenarios and can effectively evaluate the performance of LIO systems in dynamic scenes.

Results in Table IV demonstrate that the accuracy of our Dynamic-LIO is superior to that of RF-LIO and ID-LIO on $ulhk\_1$ and $ulhk\_2$. Since RF-LIO is neither open-sourced nor tested on the $urban$-$Nav$ dataset, we are unable to obtain its results on sequence $urban\_1$ and $urban\_2$. Although ID-LIO achieves smaller ATE than our system on $urban$-$Nav$ dataset, our act of open-sourcing the code better substantiates the reproducibility of our results.

We also compare our Dynamic-LIO with six state-of-the-art LIO systems for static scenes, i.e., LiLi-OM [19], LIO-SAM [30], Fast-LIO2 [35], DLIO [4], IG-LIO [6] and Point-LIO [12], on $nclt$ [3], $utbm$ [37] and $ulhk$-$HK$ [32] datasets, to demonstrate that our Dynamic-LIO still has excellent pose estimation performance in static scenes. The six methods we compared have released corresponding code, thus we obtain the results of them based on the source code provided by the authors.

Results in Table V demonstrate that our Dynamic-LIO outperforms state-of-the-arts for more than half sequences in terms of smaller ATE. Although IG-LIO achieves comparable results to our system on $nclt$ dataset, it shows poor accuracy on $utbm$ dataset. In addition, although our accuracy is not the best on $nclt\_2$, $nclt\_3$, $nclt\_5$, $nclt\_6$ and $ulhk\_4$, we are very close to the best accuracy. “-” means the corresponding value is not available. LIO-SAM needs 9-axis IMU data as input, while the $utbm$ dataset only provides 6-axis IMU data. Therefore, we cannot provide the results of LIO-SAM on the $utbm$ dataset. “$\times$” means the system fails to run entirely on the corresponding sequence. Except for our system, Fast-LIO2 and Point-LIO, other systems break down on several sequences, which also demonstrate the robustness of our system.

In our system, the purpose of incorporating undetermined-points is to remove dynamic points as much as possible, thereby increasing the proportion of static points in the output map. In this section, we validate the necessity of incorporating undetermined-points through comparing the PR and RR value of our Dynamic-LIO with and without considering undetermined-points.

Results in Table VI and Table VII demonstrate that incorporating undetermined-points can slightly improve PR and RR of our dynamic point detection and removal method.

In this section, we evaluate the effectiveness of removing dynamic points for pose estimation by comparing the ATE results of our Dynamic-LIO with and without removing dynamic points.

Results in Table VIII demonstrate that removing dynamic points can enhance the pose estimation accuracy of our Dynamic-LIO in dynamic scenes, especially on $urban-Nav$ dataset. In static scenes, our label consistency based dynamic point detection and removal has no negative effect on the pose estimation accuracy, which also reflects the robustness and practicability of the proposed method.

We compare the time consumption of our label consistency based dynamic point detection and removal method with two state-of-the-art online 3D point-based dynamic point detection and removal methods, i.e., Dynamic Filter [10] and RH-Map [38], on $semantic$-$kitti$ dataset [1]. Then, we compare the time consumption of our Dynamic-LIO with RF-LIO and ID-LIO. Both results of other approaches are recorded from their literatures because they have not released the code.

Results in Table IX demonstrate that the time consumption of our dynamic point detection and removal method is much smaller than Dynamic Filter and RH-Map. The front-end of Dynamic Filter requires 55.71ms to process the data of a single sweep. When accounting for the back-end overhead, the total duration for processing a single sweep will be even longer. Given that the current LiDAR acquisition frequency is typically between 10$\sim$20Hz, this implies that the processing time for a single sweep must be within 50ms to ensure the real-time performance. It can be seen that neither Dynamic Filter nor RH-Map can guarantee real-time capability, whereas our method can rum in real time stably. Since the $semantic$-$kitti$ dataset does not include IMU data, we complete pose estimation and mapping in a LiDAR-only odometry mode while simultaneously detecting and removing dynamic objects online. Therefore, the time consumption recorded in Table IX represents the total time cost for both LiDAR-only odometry and dynamic point detection and removal, and the results for Dynamic Filter and RH-Map are the same. Although the testing platforms are not entirely the same, the CPUs used for testing Dynamic Filter and RH-Map have a higher clock speed than the one we used. This also serves to a certain extent as evidence of the reference value of our experimental results. Results in Table X demonstrate that the time consumption of our Dynamic-LIO is much smaller than RF-LIO [25] and ID-LIO [33], while our system operates at a speed approximately 5X faster than that of RF-LIO and ID-LIO. Since RF-LIO is neither open-sourced nor tested on the $urban$-$Nav$ dataset, we are unable to obtain its results on sequence $urban\_1$ and $urban\_2$. As stated in ID-LIO [33], the authors tested their system using an i5 CPU. However, no specific model was provided in [33], thus we record the range of clock speeds for the entire i5 series of CPUs in Table X. Furthermore, although the i7-11700 has an advantage over the i5 series and i7-8559U in terms of thread count, our entire Dynamic-LIO system is implemented based on a single thread and does not take advantage of the multi-threading capability.

We evaluate the runtime breakdown (unit: ms) of our system for all testing sequences. For each sequence, we test the time consumption of cloud processing (except for binarized label construction), state estimation and label consistency detection. The label consistency detection module can be further decomposed into two sub-steps: binarized label construction and dynamic point identification. Results in Table XI show that our system takes only 1$\sim$9ms to identify dynamic points of a sweep, while the total duration for completing all tasks of LIO is 16$\sim$46ms. This implies that our method can accomplish the dynamic point detection and removal with extremely low computational overhead in LIO systems.

As mentioned in Sec. IV-C, compared to the conventional 8-nearest neighbor search, the voxel-location-based nearest neighbor search can significantly reduce the computational cost required for nearest neighbor search in label consistency detection. This subsection will provides quantitative comparative results to substantiate this conclusion.

Table XII demonstrates that the voxel-location-based nearest neighbor search we employ has achieved an order-of-magnitude reduction in time consumption compared to the conventional 8-nearest neighbor search, primarily for the following two reasons: (1) The voxel-location-based nearest neighbor search only requires to process the voxel to which the current point belongs, whereas the 8-nearest neighbor search requires to process the voxel to which the current point belongs and its eight adjacent voxels; (2) The voxel-location-based nearest neighbor search does not necessitate the computation of the Euclidean distance between candidate points and the current point.

We visualize the trajectories and the local point cloud map estimated by our system. The comparison results between our estimated trajectories and ground truth of the exemplar sequences $nclt\_1$ and $ulhk\_3$ are shown in Fig. 9 (a) and (b), where our estimated trajectories and ground truth almost exactly coincide. Fig. 10 shows the ability of our Dynamic-LIO to reconstruct a static point cloud map on the exemplar sequence $kitti\_04$. As illustrated in Fig. 10 (a), before removing dynamic points, the ghost tracks of moving objects (green points) are clearly visible on the map. As illustrated in Fig. 10 (b), after removing dynamic points, the output map almost no longer contains ghost tracks.