{'fraction_non_alphanumeric': 0.029070798028858084, 'fraction_numerical': 0.007515864856241359, 'mean_word_length': 4.800534649393379, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 0, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Semantic Geometric Fusion Multi-object Tracking and Lidar Odometry in Dynamic Environment

Since point clouds obtained from lidar are spatially discrete and non-repetitive, directly using point clouds to achieve object data association and robust state estimation is not a simple task. Further, tracking and analyzing the object states facilitates determining how they are involved in localization and mapping works. In this paper, we propose a least-squares estimator incorporating semantic 3D bounding boxes and geometric point clouds to achieve accurate and robust tracking of multiple objects. Then, the proposed tracker is integrated into a multi-object lidar odometry (MLO) system using only point clouds as input. By analyzing object motion states, the mapping module uses static objects and environmental features to eliminate accumulated errors. Meanwhile, the MLO system provides continuous object trajectories in map coordinate. Finally, we evaluate the effectiveness of the proposed semantic geometric fusion multi-object tracking (SGF-MOT) module and the localization accuracy of the MLO system under the public KITTI dataset.


I. INTRODUCTION
Multi-object tracking (MOT) technology is to detect objects in the sensing range and consistently associate these objects with a unique id over time. Compared with the image-based tracking methods, using lidar point clouds theoretically can achieve higher tracking accuracy. Meanwhile, point clouds always have better illumination and low texture robustness.
On the other hand, achieving accurate and robust multiple object tracking is not simple because the point clouds acquired by 3D lidar are always discrete and non-repetitive. Firstly, tracking with point cloud clusters obtained by segmentation [11] requires stable point-level data associations. Robust state estimation becomes more difficult when the initial value of the estimator is not accurate. In recent years, with the development of deep learning techniques, the 3D MOT task can be accomplished by tracking the 3D bounding boxes (3D-BB) inferred from the network model [23]. The object-level data association based on semantic 3D-BB makes the tracking more robust than directly using point clouds. However, the 3D-BB positions obtained by the prior inference model are not as accurate as the directly measured point clouds.
For simultaneous localization and mapping (SLAM) techniques, moving objects should be processed to ensure system robustness in dynamic scenes. Checking geometric constraints [26] or rejecting possible dynamic points via a powerful prior model [12] are both available solutions. However, tracking semantic objects and determining how they participate in SLAM tasks by analyzing their motion states is more significant potential. Because the tracked objects in map coordinate, such as vehicles and people, can also be provided to the planning and decision-making modules. These real needs promote the development of simultaneous localization, mapping, and moving object tracking technology (SLAMMOT) [20].
In this paper, we propose a novel multi-object lidar odometry (MLO) system that aims to solve the problem of simultaneous localization, mapping, and multi-object tracking using only a lidar sensor. The main contributions of this paper are as follows: • A semantic geometric fusion multi-object tracking (SGF-MOT) module runs in real-time on the CPU. Semantic bounding box constraints are used to guarantee the right converged direction for least-squares estimation, and object point cloud constraints are used to improve the state estimation accuracy. The orange and green boxes are the semantic geometric fusion tracking module and temporary object mapping method proposed in this paper. The transform integration module uses the correction matrix calculated by mapping module to eliminate accumulated errors.
• A temporary object mapping method. The method analyzes object motion states by the tracked object pose increments and creates temporary object maps in map coordinate. Then, static semantic objects and environmental features can both be used to eliminate accumulated errors.
II. RELATED WORK In the field of 3D MOT technology, most early research aims to segment objects directly from point clouds and track them by constructing geometric error constraints. Moosmann et al. [11] implemented a general object segmentation method based on the local convexity criterion. They used the Kalman filter and ICP algorithm for multi-object tracking. Assuming local geometric constancy and the regularization of smooth motion field, Ferri et al. [4] estimate object motion by rigid scene flow. In recent years, deep learning technology has brought some new solutions for multi-object tracking problem. Weng et al. [23] used POINTRCNN to detect semantic objects in point clouds and track 3D-BB by Kalman filter. Kim et al. [8] used 2D/3D-BB detected in image and point clouds for filter tracking, achieving better tracking accuracy. Wang et al. [22], and Huang et al. [7] designed the end-to-end multi-object tracking framework using different deep learning models to accomplish object detection and data association tasks.
Due to its efficiency, using the filter method for object state prediction and update has become a standard scheme for multiobject tracking. However, many works [17], [18] in the field of SLAM have demonstrated that using a nonlinear optimization scheme can achieve higher accuracy with only a slight increase in computation. This paper aims to develop a multi-object tracking module based on the least squares principle that works in real-time and uses a combination of semantic detection (3D-BB) and geometric point cloud information.
For the SLAM technology, maintaining objects in maps has broader application scenarios than detection and outlier rejection methods [2], [12]. Wang et al. [20] derived the first Bayesian formulation for localization, mapping, and moving object tracking, laying the theoretical foundation for solving the problem. Their system uses two extended Kalman filters to perform SLAM and multi-object tracking tasks based on 2D lidar. Dewan et al. [3] used RANSAC to estimate the motion model sequentially. The point clouds were then associated with the corresponding model by a bayesian approach. Pomerleau et al. [13] match measurements to local spherical coordinates and identify dynamic points from a priori maps. The transformation of each 3D point is calculated using the nonrigid ICP algorithm, and the translation vector is extracted to calculate the velocity. Henein et al. [6] and Qiu et al. [15] use the feature or optical flow methods for object tracking. Then they jointly optimize the ego poses, object trajectories, and landmarks by bundle adjustment method in the back-end to improve the system mapping accuracy. In addition, Henein et al. [6] derived the point feature coordinate transform formula for rigid moving objects.
Currently, most work focuses on separating and separately tracking environmental information and semantic objects in the field of 3D lidar SLAMMOT technology. However, estimating the absolute object motion, and exploiting them in the mapping process, is an essential and lesser concern problem. Therefore, another purpose of this paper is to recover the continuous object trajectories in map coordinate and improve the accuracy and robustness of the SLAM system by simultaneously using static objects and environmental features.

III. METHOD
Fig. 2 shows the MLO system flowchart. The proposed SGF-MOT tracker is integrated into the odometry module to achieve robust and accurate tracking of objects. After object state analysis, the mapping module uses both static object and environment features [28] to eliminate accumulated errors.

A. Notation and Coordinate Frames
In this paper, we define the notation as follows: The lidar

B. Multi-task Fusion Perception
After getting the ROI point clouds Φ i of the current frame, we perform ground feature extraction and semantic object detection in parallel to obtain preliminary object point clouds and geometric ground features. Then, the information fusion module can receive more accurate object points segmentation and environment features through a mutual correction step.
1) Ground feature extraction. First, we fit ground parameters in the point cloud using the iterative PCA algorithm proposed in [27] and mark the corresponding points near the ground in point cloud Φ i g . Then, the distortion correction time (taking the value range [0,1]) is calculated based on the relative offset of each 3D point angle from the 3D point angle at the initial time of the frame. The scan line is uniformly segmented. Through the pointwise inspection of ground labels (0: background point, 1: ground point) calculated above, it is possible to record whether each segment contains the ground point or background point labels. The segment head and tail point id pairs containing the ground or background points are stored in the ground container V g or background container V b .
Based on the smoothness (readers can refer to [28] for the calculation formula) calculated by segment, ground features χ i f can be extracted from the point cloud contained in V g . The smoothness prior S a used for subsequent background surface feature check is obtained as follows: where c o is the smoothness of o-th ground feature, and N c is the total number of ground features in current frame. After downsampling all ground points, the candidate ground point cloud χ i c is generated. For point clouds contained in V b , only smoothness is calculated and sorted here. After that, the information fusion module will finely select the background features.
2) Semantic object detection. The semantic object detection module is to acquire 3D-BB and the foreground object point clouds they enclose. In addition, the 3D-BB will be used to provide optimization constraints for the motion estimation of movable objects. We accomplish the above tasks using 3DSSD [25], a single-stage, point-based lightweight object detection model that balances detection accuracy and efficiency. Assume that the k-th 3D-BB in current detection result is: where (x, y, z) represents the object position, (l, w, h) represents the 3D-BB size, and yaw is the object direction angle. Each group of object point cloud in 3D-BB is stored as γ k,i . Each 3D point is marked with an independent object id in Φ i s . Object semantic confidences are denoted as µ k,i .
3) Information fusion. Considering that 3D-BB obtained by semantic detection module usually encloses both object and ground points. Meanwhile, features obtained by ground feature extraction module also contain a small number of points belonging to the object. Therefore, it is necessary to perform mutual correction first to ensure their motion consistency.
According to the object id marked in Φ i s , object points in ground feature sets χ i f and χ i c are eliminated. Similarly, the ground feature in each object point cloud γ k,i can be rejected by labels in Φ i g . The ambiguous intersection information will not participate in the subsequent SLAM and MOT tasks.
For point clouds contained in V b , we perform two feature extractions based on the computed smoothness: The candidate object edge feature γ k,i c is extracted from the point clouds belonging to objects, which will be used in mapping module to eliminate accumulated errors. Then, background edge features ψ i f , surface features ζ i f , and the corresponding candidate sets ψ i c , ζ i c are extracted from the point clouds that do not belong to semantic objects and ground features, and they will participate in both localization and mapping modules.
Finally, we define the edge and surface (including ground) features used for state estimation in this paper. The distance from the point to the edge or surface features in lidar coordinate l are as follows:

C. Multi-object Lidar Odometry
Based on the perception results in Section B, the odometry module firstly estimates the absolute increment for ego motion and relative motion increment for each semantic object in parallel. Then, each object pose is calculated in body-fixed coordinate [6].
1) Robust ego odometry. Before entering the ego-motion estimation process, Algorithm 1 is used to achieve a geometric consistency check on the ground and background features, eliminating unknown dynamic influences that the object detection module cannot identify.
Algorithm 1: Geometric consistency feature check including distortion correction time, feature position at the initial time of current frame is computed; Search their closest points in kd-trees formed by ( Random select matching pairs from m χ f , m ψ f and their candidate sets; Update i−2 l δ i−1 with matching pairs and ICP model; Find closest points in kd-trees to perform Euclidean distance check; Record the number of points passing the check as To improve the computational efficiency of ego odometry, we use the two-step algorithm proposed by [16] for pose estimation. First, frame increment [t z , θ roll , θ pitch ] is computed based on matched points in the checked ground feature set χ i f , and the distortion-corrected candidate feature setχ i−1 c . Then, increment [t z , θ roll , θ pitch ] is fixed, and another 3-DOF increment [t x , t y , θ yaw ] is estimated based on the edge feature setψ i f , surface feature inζ i f with more "flat" smoothness than S a and their corresponding candidate sets. Finally, we obtain ego pose T i ol by accumulating the 6-DOF increment 2) Multi-object tracking (SGF-MOT). In order to track semantic objects robustly and accurately, a least-squares multiobject tracking module fusing semantic 3D-BB and geometric point clouds is designed in this paper. The module maintains a tracking list containing object information (object point cloud, sample points on 3D-BB, and poses in lidar coordinate) and tracking states (optimization quality evaluation and tracking continuity results) for object data association and relative motion increment estimation.
First, we use the constant motion model to predict object position, which, together with the object detection position, forms the position error term e p . Meanwhile, the direction error term e d is formed by the relative object motion direction, and the 3D-BB size is used to calculate the scale error term e s . Then the association matrix is solved by Kuhn-Munkres algorithm to obtain the object matching relationship. For objects that are tracked in last frame but no matching relationship is currently found, we use the constant motion model to infer their positions in the subsequent two frames. If still no successful matching is found, they will be removed. Then, we use the voxelized G-ICP algorithm proposed in [9] to construct the cost function of object point cloud γ i (object id k is omitted). Assuming that in lidar coordinate l, the j-th point on current processing object is l p g i,j , and its corresponding timestamp is t j . Let i−1 l H i j denote pose transformation between interval t i−1 , t j . i−1 l H i j can be calculated by linear interpolation of the relative motion increment i−1 l H i : The geometric error term e gj containing motion distortion correction is expressed as: where l p g i−1,m is the m-th point in matched voxel at the end time of last frame, N j is the number of 3D points contained in the matched voxel. The covariance matrix Ω j corresponding to the error term e gj is as follows: C g i−1,m and C g i,j denote the covariance matrix. They describe the surrounding shape distribution of the point in the matched voxel and point l p g i,j . Considering objects detected in a single frame always have less constrained information. Meanwhile, the inaccurate initial value of the estimator may also lead to insufficient matchings at the beginning of the optimization. Eq. (6) sometimes fails to constrain the 6-DOF motion estimation. Therefore, while using geometric point cloud constraints, this paper further introduces semantic constraints to increment estimation for providing the right converged direction.
By uniformly sampling each edge of 3D-BB τ i , it is easy to obtain 3D points with a direct matching relationship between frames. Assuming that l p b i,r is the r-th sampling 3D point on 3D-BB at time i in lidar coordinate l, its semantic error term e br can be denoted as: Finally, Eq. (9) is the cost function of relative motion estimation considering the semantic confidence µ i : e T br e br (9) where N g is the number of object point cloud. When sampling point number on each edge is Λ, the total number of sampling points on 3D-BB is N b = 12Λ − 4.
In this paper, we use LM [10] algorithm to solve Eq. (9) and the estimated relative increment to get motion-distorted object point cloudγ i and object edge featureγ i c , which will be used for object tracking in next frame and accumulated error elimination in mapping module.
3) Object odometry. Based on the calculation results of ego odometry and multi-object tracking, object pose in odom coordinate can be computed. First, object motion increment i l X i−1 in lidar coordinate is denoted as: Then, combined with the body-fixed coordinate proposed in [6] (as shown in Fig.4), and assuming that the transform matrix from object to lidar at time i − 1 is i−1 L lt , we can obtain absolute object motion increment i−1 t X i in body-fixed coordinate: Odometry pose T i ot for each object at time i can be computed by accumulating increment i−1 t X i . This paper uses ego odometry T ol and transform matrix L lt extracted from semantic detection module to set object odometry T ot at the first detected frame.

D. 4D Semantic Object Mapping
To reuse the created map, we suggest maintaining the long-term static map and movable object map separately to build a consistent 4D semantic object map. Among them, the temporary object mapping method consists of the following steps 1) and 3).
1) Object motion analysis. In order to analyze object motion states in current frame, object odometry poses T i ot should be transformed to map coordinate. As shown in Fig. 4, object odometry pose T i ot can be expressed as: Then, we use the current error correction matrix T i−1 mo to predict the object pose T i mt in map coordinate. By comparing the oldest static object pose with the same id maintained in the tracking list, it is possible to determine whether the object is moving.
2) Static map creation. By matching the static object featuresγ i c in the current frame with the temporary object map, ground featureχ i c , background edge featureψ i c and surface featureζ i c with their surrounding point cloud map, ego pose T i ol can be corrected to map coordinate. Then, a long-term static map can be built by aligning the non-semantic features into map coordinate. Readers can read [28] for more map matching and optimization details.
According to the ego pose T i ol in odom coordinate and the corrected pose T i ml in map coordinate, the updated accumulated error correction matrix T i mo can be obtained. 3) Object state maintain. The temporary object map is updated based on object motion states. If the object is moving, the map should be cleared first. Otherwise, the map will remain unchanged. Then, we use T i mo and T i ml to convert the object odometry pose T i ot and extracted object featuresγ k,i c into map coordinate, respectively.

IV. EXPERIMENT
This section presents experimental details for the proposed SGF-MOT tracker and MLO system on the public KITTI [5] dataset. The experiments are divided into two parts: 1) Evaluate the ego localization accuracy of the MLO system. The MLO system is compared with other feature-based 3D lidar SLAM systems on the KITTI-odometry and the KITTIraw benchmarks. The geometric consistency check algorithm and the temporary object mapping method are verified. 2) In the KITTI tracking benchmark, the SGF-MOT module is compared with another two efficient multi-object tracking methods that also perform on the CPU and use point clouds as input. Then, we evaluate the absolute object trajectory accuracy generated by MLO under the KITTI-raw benchmark.

A. Experimental Setup and Error Metrics
The experimental environment of the proposed framework is AMD® Ryzen 7 5800h (8 cores @3.2 GHz), 16GB RAM, ROS Melodic, and NVIDIA GeForce RTX 3070. We use absolute trajectory error (ATE) [19], relative rotation error (RRE) and relative translation error (RTE) [5] to evaluate the accuracy and odometry drift of ego and object localization. The CLEAR MOT metric [1] is used to evaluate estimated accuracy and tracking object configurations consistently over time for multi-object tracking module.
Note that our multi-object tracking module is performed in lidar coordinate. Therefore, the relative motion tracking ability of the object is evaluated. Object trajectories in map coordinate evaluate the overall accuracy of ego localization and multiobject tracking module.
Furthermore, the point cloud in the KITTI-raw benchmark does not use known GPS/IMU information to correct motion distortion nor provides ground-truth trajectories that can be directly used for localization evaluation. For obtaining a smooth and accurate 6-DOF pose ground truth, we use the extended Kalman filter to process GPS/IMU data in the KITTI raw dataset. Combined with the high-precision artificial object annotation in lidar coordinate, the ground truth of object pose in map coordinate can be obtained.

B. Ego Odometry Localization Accuracy Experiment
The KITTI-odometry benchmark contains rich static objects with long sequences. Meanwhile, the KITTI-raw dataset contains more challenging scenes, such as the city sequence with unknown dynamic semantic objects (trains). We evaluate the MLO system in these two datasets by comparing it with other state-of-the-art 3D lidar SLAM. Since the KITTI dataset labels semantic objects within the camera's field of view, only the ROI-cutting lidar data is input into the fusion perception module.
For the experimental lidar SLAM system, A-LOAM [14] is a simplified version of LOAM [28] with the removal of the IMU used. It also extracts edge and surface features from point cloud to perform frame-to-frame matching for odometry estimation and frame-to-map matching to complete mapping. Lego-LOAM [16] takes into account ground constraints. Use ground features and background edge features, respectively, to estimate the increment [t z , θ roll , θ pitch ] and [t x , t y , θ yaw ] for improving efficiency. F-LOAM [21] proposes a two-step motion distortion removal algorithm, and then localization and mapping are done directly through frame-to-map matching. As shown in Table I, our method successfully runs all sequences under the KITTI-odometry benchmark and achieves the best results in most cases. When estimating [t x , t y , θ yaw ] based on background edge features, the Lego-LOAM [16] system shows severe drift on many sequences due to only using point clouds from the camera's view. Our system also uses the two-step estimation algorithm to ensure computational efficiency. However, using background surface features based on smoothness prior S a and geometric consistency check algorithm improve the MLO system robustness. Further, analyzing the object motion states and adding object constraints for static map creation can better eliminate accumulated error compared to MLO-tom system, which does not use the proposed temporary object mapping method in mapping module. In sequences 00, 02, 05, 07, and 08 containing rich static objects, the MLO system achieved better localization accuracy. Table II shows that in complex scenes, the MLO system can still achieve better localization accuracy in most cases. Meanwhile, the MLO system outperforms the other two frameworks in terms of average RRE and RTE. For sequences 0926-0056, there is a train that the object detection module cannot recognize. Since the A-LOAM [14] system does not distinguish the ground and background features and makes use of all the information for localization and mapping, it can improve the system robustness to a certain extent. For the MLO system that also uses the two-step estimation algorithm, especially for the estimation of [t x , t y , θ yaw ], a geometric consistency check can reduce the proportion of abnormal associations and avoid the trajectory drift problem of the MLO-gc system (ego odometry module uses the features extracted after removing the object point cloud, but does not consider the geometric consistency check algorithm).

C. Object Tracking Accuracy and Robustness Experiment
First, we compare the SGF-MOT module proposed in this paper with AB3DMOT [23] and PC3T [24] trackers under the KITTI-tracking benchmark. The tracking results are evaluated   Table III, thanks to the accurate point cloud, three methods have little difference in the MOTP metric, which illustrates the estimation accuracy of the tracker. Since the object is transformed into map coordinate using the prior ego ground truth, PC3T [24] tracker only needs to consider the uncertainty caused by absolute object motion between frames. Moreover, it achieves the best results on the MOTA metric. The proposed SGF-MOT tracker does not use ego localization information but also improves the tracking robustness by introducing a least-squares estimator fusing geometric and semantic constraints. So we achieve better MOTA results than the AB3DMOT [23] tracker.
Like the other two methods, our method runs in real-time on the CPU. Fig. 6 shows the time-consuming results when the fusion tracking module executes sequence 0001. It can be concluded that the time-consuming of our method is always less than 25ms, even during peak hours.
Then, we selected 8 sequences containing moving objects from KITTI-raw City and Road sequences. The semanticgeometric fusion tracking method (Fusion Method) is compared with methods using only semantic boxes (Semantic Only) and geometric point clouds (Geometry Only) by evaluating the object trajectory accuracy in map coordinate. The object localization accuracy of geometry-only method exhibit a distinct "bipolar" distribution. Furthermore, we found that it can only achieve stable and accurate tracking for objects perceived within 15m. The main reason for this phenomenon is that the lidar points successfully hit on the object will decrease rapidly with the increase of measurement range. When the constraints are not sufficient, and the initial value of optimization is not precise enough, the optimization results diverge. However, if the object is in the near field of view, the estimation method based on the measured point cloud will be more accurate than that of 3D-BB inferred by the semantic detection module. We can draw this conclusion by comparing the semantic and converged geometric tracking results.
We hope to use the directly measured object point cloud to improve state estimation accuracy. Meanwhile, the tracker's robustness should be guaranteed when the object moves away from the robot. Therefore, using a fusion estimation method with semantic and geometric information seems natural. As shown in Table IV, our proposed method does not exhibit significant object trajectory drift and achieves better localization accuracy on the test sequences.

V. CONCLUSIONS AND FUTURE WORK
In this paper, we propose a least-squares estimator fusing semantic 3D bounding boxes and geometric point clouds for object state estimation, which achieves accurate and robust tracking of multiple objects in the sensing range. Then, by analyzing the object motion states in the mapping module, the multi-object lidar odometry (MLO) system can reliably complete localization and mapping works in the complex scene containing static and dynamic objects. On the other hand, dynamic object-aware visual SLAM technology has developed rapidly. Works such as [6] have verified that the tightly coupled optimization of ego poses, object trajectories and landmarks based on batch processing can improve the system mapping accuracy. Therefore, exploring the tightly coupled lidar backend will be a challenging and exciting task.