MAPPING WITHOUT DYNAMIC: ROBUST LiDAR-SLAM FOR UGV MOBILE MAPPING IN DYNAMIC ENVIRONMENTS

: Detection And Tracking of Moving Objects (DATMO) is essential and necessary for mobile mapping system to generate clean and accurate point clouds maps since dynamic targets in real-world scenarios will deteriorate the performance of whole system. In this research, a robust LiDAR-SLAM system is presented incorporated with a real-time dynamic objects removal module to improve the accuracy of 6 DOF pose estimation and precision of maps. The key idea of the proposed method is to efficiently cluster the sparse point clouds of moving objects and then track them independently so as to relieve their influence on the odometry and mapping results. In the back-end, in order to further refine the point clouds maps, a valid probabilistic map fusion method is performed based on the free-space theory. We have evaluated our system on the dataset collected from daily crowded environments full of moving objects, providing competitive results with the state-of-the-art system both on the pose estimation and point cloud mapping.


INTRODUCTION
Simultaneous Localization and Mapping (SLAM) is designed to estimate accurate 6 DOF pose information of the platform itself as well as the positions of landmarks in a real-time manner. SLAM is usually used as an efficient technique to collect information of 3D environments which has been widely studied and used due to its efficiency to not only get the true trajectory but also the accurate surrounding map, especially for the Unmanned Ground Vehicles (UGVs). The principle of generic SLAM is based on the assumption of relatively static environments with static landmark distribution. However, such ideal static environment is not common in urban or daily realworld scenarios. There are always full of moving objects such as people and cars, resulting in distortion of the pose estimation and fuzzy map area.
As shown by the early work of (Wang et al., 2007;Wang and Thorpe, 2002), SLAM with DATMO is quite necessary for a robust UGV mapping system in dynamic environments. On the other hand, surveyors may care much more about the quality and the precision of map which will be used for their specialized applications as they normally use SLAM as an effective tool to accomplish the job of scene reconstruction. Motivated by the mentioned above, we therefore seek to develop a robust localization and mapping system filtering out the influence of dynamic objects.
Considering the time efficiency and adaption with the SLAM framework, a robust mobile mapping system is proposed incorporating LiDAR SLAM with a novel dynamic objects detection and removal module as well as a probabilistic map fusion process in this paper, to further improve the accuracy of trajectory and point cloud map. The main contributions of our system are as belows: • In the front-end, a real time detection and tracking of dynamic objects module based on fast density-aware clustering is presented to alleviate invalid feature points used for the 6 DOF pose estimation of LiDAR SLAM.
• In the back-end, a probabilistic map fusion algorithm is adopted to prevent unreliable observations from deteriorating the final point cloud map caused by motion blur. As shown in Fig. 1, our system mainly consists of three parts: laser mapping, laser odometry and DATMO. Upon a new frame of laser scan, the ground points will be identified and the nonground points will be coarsely segmented in order to denoise the whole point cloud. Then edge features and planar features collected from the segmented cloud will be used for laser odometry and laser mapping to retrieve 6 DOF ego-motion of the system. The LiDAR registration method used here is the robust point-to-line and point-to-plane workflow illustrated in LOAM (Zhang and Singh, 2017). Generally, the odometry runs at 10hz (same with the frequency of the laser scanner) while the laser mapping usually runs at a slower rate to get more precise poses information. With regard to this, we innovatively add a

UGV MOBILE MAPPING SYSTEM OVERVIEW
The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLIII-B1-2020, 2020 XXIV ISPRS Congress (2020 edition) dynamic objects removal thread accompanied with laser odometry thread in the front-end to prevent the dynamic points on moving objects to be processed by laser mapping thread later.
In the back-end of our system, a probabilistic LiDAR map fusion process is integrated to further differentiate the static background map and dynamic targets based on the free-space theory. As we don't focus on the LiDAR registration, the method of DATMO and probabilistic map fusion will be detailed followed.

Candidates Detection
The following process is applied to cluster potential moving objects online: 1) the ground filtering method proposed by (Himmelsbach et al., 2010) is firstly used to get rid of the futile points (ground points) as in Fig. 2; 2) The non-ground point cloud is coarsely segmented using the scheme of (Bogoslavskyi and Stachniss, 2016). The coarsely segmented result is scattered where, for example, a person may be separated into several parts ( Fig. 3-(a)); 3) In order to refine the segments to retrieve objects efficiently, we first extract the convex hull of the ground point clouds and then filter the segmented cloud within this convex hull by means of simple range comparison. Actually, incorporating all of the segmented points for clustering is not sensible because we are more concerned of the points that are likely to belong to dynamic objects such like pedestrians and cars which in fact move only on the ground. As in Fig. 2-(b), points with ranges smaller than maximum ground range are assumed to be points that are above the ground. With the largely reduced number of points, an improved DBSCAN(Navarro-Hinojosa, 2016) clustering method with adaptive thresholds is proposed here to re-cluster the segments to generate the final object clusters. Since this method is density sensitive, the adaptive clustering thresholds solve the numerical relation between the distance form sensor and sparsity of the point clouds for better grouping of the sparse segments. Adopted from the ring sections in (Yan et al., 2017), desired clustering radius ℛ and minimal point number can be calculated correspondingly. This density-adaptive parameter setting is designed based on the physical characteristic of the laser. The clustered potential candidates are shown in (Fig. 3-(b)).

Tracking
To truly distinguish the moving objects among candidate clusters, we use general EKF method to respectively track the clustered objects extracted from the previous step. As the laser scanner works at 10Hz, there is actually no large displacement of objects between frames. We use common Multiple Hypothesis Tracking (MHT) strategy to avoid mixed match, and we only care about the x-y plane motion of the objects based on the fact that there is ordinarily no movement in the z direction. Then through analysing the valid tracklets, we are able to identify different dynamic objects which is similar to the thought of (Moosmann and Stiller, 2013). Once we recognize the moving targets, we remove the corresponding points before pose estimation and mapping followed.

Probabilistic Mapping
According to the physical characteristic of LiDAR sensors, there should be no obstruction between the sensor origin and the endpoint which can be referred as the free-space (the path from origin to footprint) of individual laser point (Yoon et al., 2018). Following the mapping framework in (Zhang and Singh, 2017), we additionally model the confidence of every point by the occupancy probability stored in the octree structure. To lessen the impact of side effect, the confidence will decrease only if the grids appear in the free-space for multiple times. The detailed criterions are as follows: where O A represent the voxel in map. The previous estimation term ' .:/1. models the accumulation of previous observations. P C , P D are the confidence prior terms that represent the probability of voxels being hitted and passed through. In probabilistic fusion process, the only output is the occupancy probability value ' .:/ , where sensor readings and previous state are integrated into current occupancy probability. For simplification, the confidence of a voxel given observation can be easily updated using log-odds probability. To maintain the robustness of estimation, P C is usually higher than initial value while P D is a little lower.
The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLIII-B1-2020, 2020 XXIV ISPRS Congress (2020 edition)  Table. 1 System Equipment Description

System design and dataset
To evaluate the performance of our system, we have collected datasets of different scenes in Wuhan university including a large loop around the playground and a smaller loop in front of the building (Fig. 5) which are both typical outdoor scenes with lots of dynamic moving objects. The trajectories of the UGV are shown in yellow and red lines. The dataset descriptions are shown in table 2. Fig. 4 shows our mobile mapping platform integrated with a VLP16 laser scanner with 16 scan channels which have a 360° horizontal and around 30° vertical field-of-view. Each laser scan frame contains around 30,000 points which is much sparser than 100,000 of 32-beam and 200,000 of 64-beam mentioned before. It rotates at 10 Hz with a maximum scan range of 100 m. Other sensors are not included in the processing workflow (Table  1). Our framework has been fully implemented into the Robot Operating System (ROS) with high modularity. Data collection, as well as all experiments reported in this paper were carried out with Ubuntu 16.04 LTS (64-bit) and ROS Kinetic, with an Intel i7 processor and 8 GB memory.

Evaluation Results
Since the detection and tracking of moving objects as well as the probabilistic map fusion are fully integrated into LiDAR SLAM system, we will respectively illustrate the improvement on the performance of SLAM by means of evaluating the results of pose estimation and point clouds map in this section. With collected datasets, our system outperforms other well-known LiDAR SLAM works (i.e. A-LOAM and LeGO-LOAM (Shan and Englot, 2018)) especially w.r.t. the laser mapping.
Positioning Evaluation: This part will demonstrate how the removal of dynamic objects benefit the pose estimation of LiDAR SLAM. For better positioning evaluation, we have individually tested A-LOAM, which is a popular implementation of (Zhang and Singh, 2017), LeGO-LOAM (Shan and Englot, 2018), which achieved robust performance in outdoor scenes, and our system with the collected datasets. Considering we mainly focus on the relative pose precision, loop closure detection is excluded for all systems. The overall trajectories of two datasets are shown in Fig.  6. As it is depicted obviously, A-LOAM is not able to achieve adequate results in both situations due to the complexity of the dataset environments (with abundant trees and leaves). There is relatively large drift in the height direction. For playground dataset, we deliberately travel two marked sites twice so that we can exhibit the precision of pose estimation more clearly through comparing the position/translation part of platform while revisiting. As deduced by Fig. 7, there is less accumulated drift along the trajectory of our system as the UGV revisits these two places compared with LeGO-LOAM, which proves the effect of our dynamic module. To be more specific, here we ignore the result of A-LOAM to better illustrate the difference of translation (Fig. 7). There are around 5m discrepancies in the first loop closure spot and 2m in the second one between our system and LeGO-LOAM. While on the other dataset, the translation drift along z direction are relatively small (Fig. 8).
The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLIII-B1-2020, 2020 XXIV ISPRS Congress (2020 edition)  Mapping Evaluation: This part will qualitatively and quantitatively shows the point cloud map refinement of our system. For probabilistic map fusion, we set the initial occupancy probability to be 0.5 in general. As the point cloud map incrementally being fused, the voxels' confidence will be increasing with more and more observations while the confidence of voxels that are in the free-space of other laser endpoints will decrease accordingly with the times they are passed through. At the end of LiDAR SLAM, we only save the points with confidence higher than the average for better comparison and visualization. This means more repeated observations and higher guaranteed quality.
The mapping result of playground are shown in Fig. 9: After filtering, the global map is clearly improved compared with the noisy original map; The final map consistency is improved, since there are no fuzzy point clouds of the moving objects especially on the road. The refined point cloud map without "ghost" points is more applicable. The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLIII-B1-2020, 2020 XXIV ISPRS Congress (2020 edition) Figure 10. Outdoor mapping result of building with white points corresponding to the moving objects. The first row is the result of LeGO-LOAM while the second row is from our system.
The mapping result of the building dataset is shown in Fig. 10. Compared with the point clouds generated by LeGO-LOAM(Shan and Englot, 2018), our system can obviously produce a better map without heavy region of blurred points floating on the ground. Even though there are some areas of the static background being mistakenly removed as a result of relatively lack of observations. The probabilistic map fusion has achieved satisfied results. The promising results achieved by our system exhibit its' potential ability for efficient large-scale mobile mapping.
Quantitative evaluation results of the probabilistic mapping process are shown in Table 3. As it can be learnt from the results, the dynamic point clouds are more than 10% of the whole data points in both scene and the proposed method eliminate those data contaminations to improve the quality of the final point clouds maps.  Table. 3 Quantitative evaluation of the dynamic object detection

CONCLUSION
In this paper, we have presented a robust LiDAR mobile mapping system incorporating SLAM with a real-time dynamic objects detection-tracking module and an efficient probabilistic map fusion process. With density-adaptive clustering mechanism, the proposed system is able to recognize and filter out the moving objects regardless of the sparsity of data, resulting in both improved trajectory estimating and map generating in real world scenarios. The generated point clouds map containing only the static landmarks are much more applicable for mobile mapping applications, such as façade modeling etc.