LiDAR SLAMMOT based on Confidence-guided Data Association

In the field of LiDAR-based SLAM, many solutions have been proposed. One of the most popular methods is LOAM [12], which is a low-drift and real-time LiDAR odometry and mapping method. However, the performance of LOAM is limited when resources are scarce or when operating in noisy environments. To address these limitations, LeGO-LOAM [1] has been proposed, which is a lightweight and ground-optimized LOAM method and offers improved performance with limited resources. Additionally, auxiliary sensors like IMU can be closely integrated with LiDAR to improve SLAM performance. Researchers presents LIOM [13], which uses graph-based optimization to refine the extrinsic parameters of LiDAR and IMU, as well as IMU bias and robot pose. Following this, the LIO-SAM method [2] was introduced, estimating IMU bias within a factor graph framework and providing better real-time performance and more accurate state estimation compared to LIOM. To simplify computations, FAST-LIO [14] was developed, employing a tightly-coupled LiDAR-inertial odometry method based on an iterated Kalman filter with a new Kalman gain. Researchers then enhanced FAST-LIO with FAST-LIO2 [3], which directly matches raw point clouds without feature extraction and boosts mapping velocity and odometry accuracy using iKD-Tree. Nevertheless, all of these LiDAR-based methods rely on the static environment assumption and are easily affected by dynamic objects.

For LiDAR-based SLAM in dynamic environments, researchers have also made some efforts. In [15] and [16], the researchers use convolutional neural networks (CNN) to detect moving objects from LiDAR point clouds, and simply filter all points belonging to dynamic objects for reliable SLAM in dynamic environments. In [17], a two-stream CNN method was applied to segment potential dynamic objects from point clouds. After removing the points associated with the potential dynamic objects, the point clouds were used to perform SLAM. In [18], researchers present the RF-LIO, which detects moving points in a LiDAR scan using adaptive multi-resolution range images and removes them, then realizes a tightly-coupled LiDAR inertial odometry in high dynamic environments. However, these methods may overlook valuable information, like points from stationary objects (e.g., parked vehicles), or face tracking errors due to the absence of a relationship between the ego-vehicle state and object state.

Multi-object tracking is a crucial module of the vehicle perception system. Existing tracking methods can be categorized into two groups: non-detection based methods and detection based tracking. In the non-detection based method, one approach is to integrate detection and tracking into a single network, as demonstrated by JDE [19] and CenterTrack [20]. Another approach is attention mechanism based tracking, exemplified by TrackFormer [21]. However, these strategies suffer from drawbacks such as poor interpretability and generalization, and are primarily used in image based object tracking. Therefore, LiDAR based multi-object tracking typically employs a detection based method.

In [22], researchers proposed AB3DMOT, which is based on a baseline for 3D multi-object tracking, featuring a lightweight architecture and low computational demands, enabling real-time operation. In [23], researchers introduced a new 3D multi-object tracker guided by prediction confidence for data association, which can better leverage object features for multi-object tracking in point clouds. These two methods are both advanced LiDAR-only real-time 3D multi-object tracking techniques, and they utilize detection results obtained from LiDAR point cloud data and employ Kalman filtering with Hungarian matching and greedy algorithm respectively for state estimation and data association. However, if the initial assignment in data association is incorrect, such filter-based approaches often struggle, which can easily occur. In [24], researchers present a factor graph-based 3D multi-object tracking (FG-3DMOT) method, which represents the detection results as a Gaussian mixture model (GMM) and is integrated into a factor graph framework. FG-3DMOT does not rely on explicit and fixed assignments; instead, it flexibly assigns all detections to all objects simultaneously. The assignment issue is addressed implicitly and jointly with object state estimation by nonlinear least squares optimization. However, such methods ignore the consideration of the quality of detection and object prediction. For example, in cases of continuous missed detections, the predicted states can accumulate errors leading to inaccuracy, thereby affecting tracking performance. Additionally, there are some camera-LiDAR fusion-based multi-object tracking methods. In [25], researchers present a 3D multi-object tracking framework based on camera-LiDAR fusion with deep association, achieving seamless integration of 2D and 3D trajectories for more accurate and robust tracking, especially when tracked objects are far away. And in [26], researchers present CAMO-MOT, which introduces a combined appearance-motion optimization technique that significantly reduces tracking failures caused by occlusions and false detections by using camera and LiDAR.

Currently, there have been many research efforts in coupled SLAM and object tracking. Specifically, researchers perform separate SLAM and object tracking systems simultaneously and estimate the ego-vehicle state and object state with a loosely-coupled approach [27]. In [28], visual odometry and persistent tracking are performed simultaneously to detect humans in dynamic environments. Additionally, in [15], researchers use neural networks to estimate object motion or segment potential dynamic objects from LiDAR point clouds, and SLAM is performed after filtering out dynamic object points. Another category integrates vehicle state estimation and object tracking into a unified optimization problem for tightly coupled SLAM with object tracking, which mainly revolve around visual-based and range sensor-based methods. In [29], researchers propose a tightly-coupled approach for SLAMMOT based on stereo visual information. In [5], Dynamic SLAM is proposed, which performs object detection and tracking based on deep learning during the operation of SLAM in a dynamic environments. Furthermore, DynaSLAM II [6] demonstrates tightly-coupled multi-object tracking and visual SLAM through instance semantic segmentation and ORB features.

Regarding range sensor-based solutions, researchers in [4] propose a feasible tightly-coupled SLAMMOT system based on a laser scanner, deriving a dynamic Bayesian networks algorithm for coupling. In [30], researchers present an online SLAMMOT method using a grid-based approach, achieving pose estimation and object tracking in two-dimensional space. In [9], researchers present DL-SLOT, which uses sliding window-based graph optimization to simultaneously optimize the vehicle state and dynamic-static object states based on LiDAR. Based on DL-SLOT, the researchers present LIMOT [31], which is also a tightly-coupled multi-object tracking and LiDAR-inertial odometry system, and it presents a dynamic feature filtering method for the LiDAR odometry module. Although their introduced trajectory-based state prediction improves the tracking performance, this sliding window-based local optimization reduces its ability to perform global optimization for ego-vehicle and object states. In [10], researchers propose the IMM-SLAMMOT, which integrates interactive multiple model into object tracking for SLAMMOT to deal with the ambiguous motion patterns of objects. The data association in all above tracking methods follows fixed assignments, which tend to struggle if the initial assignment is incorrect. In [8], researchers propose LIO-SEGMOT for asynchronous estimation of simultaneous ego-motion state and multiple object tracking, which integrates the factor graph based tracking that does not rely on explicit and fixed assignments in data association. However, these methods ignore considering the confidence during tracking, especially in scenarios with continuous missed detection due to occluded or distant objects, which can lead to performance degradation, affecting the accuracy and stability of SLAMMOT.

The proposed Conf SLAMMOT solution extends from the foundation of LiDAR SLAM based on graph optimization. The architecture of the Conf SLAMMOT solution is shown in Fig. 2, which mainly consists of an alternative LiDAR odometry module, an alternative moving object detection module, and a joint graph optimization backend, which is a tightly-coupled LiDAR SLAM and multi-object tracking method in dynamic environments for simultaneously estimating the states of the ego-vehicle and moving objects. The 3D point cloud information serves as the input for the LiDAR odometry module and the moving object detection network, respectively. The LiDAR SLAM method (the module used here is from LeGO-LOAM [1]), as shown in Sec. III-B, consists of a replaceable LiDAR odometry module and partial graph optimization for the joint optimization backend of the overall system. For object detection, the advanced deep learning-based method, PV-RCNN [32], is used, as shown in Sec. III-C. As for the object tracking part of the system, inspired by FG-3DMOT [24], we use a max-mixture model for implicitly associating object prediction states with detection results and integrate it with a prediction confidence model for confidence-guided data association. To demonstrate the improvements we have made in data association, we display this implicit data association as a module in the figure. It should be noted that this implicit data association is managed as part of the factor graph optimization and is to be jointly optimized with the overall system, as shown in Sec. III-D and E. Finally, the joint factor graph optimization backend outputs the state estimation of the ego-vehicle and moving objects simultaneously, as shown in Sec. III-F.

In presented Conf SLAMMOT solutions, the SLAM can be any representative LiDAR SLAM framework. Due to the several sub-modules of the proposed system, the lightweight of each module should be considered under conditions of limited computing resources. Also, the IMU is not a mandatory sensor in many cases. Therefore, LeGO-LOAM [1] is applied for LiDAR odometry module, which maintains high localization and mapping accuracy while consuming fewer computing resources compared to the LOAM [12].

The LeGO-LOAM receives input from a 3D LiDAR and outputs a 6 Degrees of Freedom (DoF) ego-pose estimate and a 3D point cloud map. This framework is divided into five submodules: segmentation, feature extraction, LiDAR odometry, LiDAR mapping, and trajectory integration [1].The LeGO-LOAM project the raw point cloud into a distance image and differentiating between ground points and segmented points before feature extraction. It then matches feature points of the same category and incorporates the iSAM2 [33] optimizer in the backend. Finally, the ego-vehicle pose estimation obtained by this module will provide to the multi-object tracking and joint graph optimization backend.

For object detection in Conf SLAMMOT, it can be the any representative detection method. Herein, we use the PV-RCNN, which is an advanced framework capable of achieving precise 3D object detection from point clouds with manageable memory consumption. PV-RCNN integrates the advantages of 3D voxel CNN with sparse convolution [34] and PointNet-based networks [35] to learn more discriminative point cloud features. The method utilizes a 3D voxel sparse CNN as the backbone network and employs a voxel set abstraction module to convert 3D scenes into keypoints, followed by a keypoint-to-grid Region of Interest (RoI) feature abstraction for refinement, used for proposal confidence prediction and location refinement. The two-step strategy of this framework effectively combines voxel-based and point-based learning, demonstrating accuracy and efficiency in 3D object detection using point cloud data without additional sensor inputs. PV-RCNN’s innovative approach and its performance on the KITTI benchmark have established it as a popular solution in the field of autonomous driving object detection [11].

For object state prediction, considering prediction accuracy and efficiency, most current 3D object detection algorithms typically employ motion models to predict future object states, with constant velocity (CV)[36, 37, 22], constant acceleration (CA)[23], and constant turn rate and velocity (CTRV) models [38] being widely utilized. The CV model assumes that the object moves at a constant velocity, disregarding the effects of acceleration. This model is particularly effective when object motion is smooth, without sharp turns or changes in acceleration. The CA model assumes the object moves with constant acceleration, making it suitable for scenarios where the object’s acceleration is relatively stable. The CTRV model assumes the object maintains a constant turn rate and velocity during turning, which is applicable to cases involving curved motion. Additionally, some learning-based tracking methods explore predicting states by learning object position transformations from historical trajectory data through neural networks [39, 40, 41]; however, they are typically several times slower than the aforementioned motion models and often face challenges in practical applications. Therefore, taking into account both prediction accuracy and efficiency, as well as addressing the missed detection issue we discuss, where objects are more likely to be lost during turning scenarios, we adopt the CTRV model to predict the future states of objects.

After obtaining the object state predictions and detection results, most traditional methods directly associate them (explicitly or implicitly) to solve the assignment problem. However, the predicted states of objects are not always accurate; for instance, after missing detections in several consecutive frames, they can accumulate significant errors, thus enlarging the possible range of detected objects when resuming tracking. Meanwhile, the quality of detection results also needs to be considered. Therefore, we have incorporated a prediction confidence model [23] into our present system to make data association more sensitive to the quality of each prediction.

Here, $c_{pre}\in(0,1]$ denotes the prediction confidence and $\alpha\in[0,1]$ denotes a parameter that adjusts the overall impact of prediction confidence on data association and is determined by dataset. Applying this model can address the issue of continuous multi-frame missed detections, where when a miss occurs, the prediction confidence decreases, and the implicit search range for data association is subsequently enlarged. This flexible adjustment of the implicit search range for data association can better recover re-tracking after continuous missed detection, preventing ID switches.

The designed implicit confidence-based data association solves the assignment problem between detection results and predicted states with their corresponding confidence. This method employs the GMM to simultaneously represent all detection results, and we utilize the max-mixture model [42, 24] to represent the full GMM of each object that is integrated with the prediction confidence model. Specifically, we apply the GMM as a probabilistic model of detection results that overcomes the limitations of simple single Gaussian models and breaks away from the least squares formulation inherent in the maximum likelihood problem, selecting one from all possible predicted states of ${\bf{\bar{O}}}_{k}$ such that the likelihood of observing the current data given the detection results ${\bf{Z}}_{k}$ is the highest. This confidence-guided implicit data association integrates the assignment problem into the factor optimization to jointly estimate the object state and solve the data association implicitly as part of the optimization, which allows for a flexible association between predicted states and detections that can change during the optimization process, unlike most online trackers, and goes beyond merely estimating the association by optimizing state positions across the entire sequence to account for inaccuracies in object prediction and detection. By applying the negative logarithm, the optimization problem arising from the maximum likelihood formulation can be reformulated as follows [43]:

Where the optimal estimated ${{\bf{\hat{O}}}}_{k}$ denotes the maximum-likelihood estimator of ${\bf{\bar{O}}}_{k}$; ${\bf{P}}({\bf{Z}}{}_{i,k}|{{\bf{\bar{O}}}}_{i,k})$ denotes the conditional probability that associates the $i$-th predicted object state ${{\bf{\bar{O}}}}_{i,k}$ with the corresponding detection result ${{\bf{Z}}_{i,k}}$ that belongs to it. For a sum of Gaussians with $n$ components ($c$), the conditional probability ${\bf{P}}$ is defined as:

Herein, $m$ deonots the sum of detection measurements; $j$ denotes the $j$th detection. ${\bf{T}}_{E,k}^{{M_{E}}}$ denotes the transformation from ego-vehicle coordinate system to map coordinate system ${M_{E}}$. And ${c_{j}}={\omega_{j}}\cdot\det(\Sigma_{j}^{-\frac{1}{2}})$, where ${\omega_{j}}$ and $\Sigma_{j}$ denotes the weight and uncertainty of $j$th detection. Herein, the $\Sigma_{j}$ considers the confidence score of detection results, denoting $\Sigma_{j}=\Gamma\cdot(1-c_{det,k}^{j})\cdot\beta$, instead of using the overall uncertainty of the detector $\Gamma$ determined by sensor’s error characteristic. $c_{det,k}$ denotes the confidence score of the corresponding detection box, and $\beta$ denotes a scaling parameter that can vary depending on the detector and the dataset. And for the conditional probability $\mathbf{P}$, the logarithm cannot be pushed inside due to the summation, so we need to calculate the log-likelihood differently. There are two suggested solutions: an approximate max-mixture method [42] and an exact sum-mixture method [44]. And the max-mixture approach simplifies calculations compared to the sum-of-Gaussians method while maintaining similar effectiveness. Therefore, the summation of a GMM can be replaced by the maximum operator as following equation (4), and we push inside the logarithm for maximum becoming a minimum as shown in equation (5):

To maintain a uniform surface in the optimization formula, we perform dimensional separation between the logarithmic normalization term and the quadratic error term. This not only aids in achieving better convergence but also allows us to consider the quadratic error term individually since the log-normalization term is a constant. The least squares problem in this optimization is expressed as follows:

The term $-\ln\frac{{{c_{j}}}}{{{c_{\max}}}}$ represents the independent dimension of the vectorized error function, where $c_{\max}$ is a normalization constant to ensure that the expression under the square root is positive. ${\bf{e}}$ denotes the weighted error between the $i$th object state and $j$th detection measurement. Then, considering the prediction confidence model in equation (1), the quadratic error term can be rewritten as follows:

Herein, $\sigma>0$ denotes an association threshold to determine whether the detection belongs to any existing object with prediction confidence. Specifically, for each predicted state, a maximum likelihood model is employed to correlate all detection results, and the minimum value of the aforementioned formula is selected. If this minimum value is less than the threshold, the detection is considered associated. Compared to approaches that do not take into account the prediction confidence, this method effectively enlarges the implicit range of association, which is highly effective for resuming tracking after consecutive missed detections, as shown in Fig. 3(b).

The aforementioned implicit data association is integrated into graph optimization, after which the tracking state for matched detection states and predicted states are updated by using the optimization, in order to obtain the updated object states. Concurrently, the prediction confidence, based on the confidence of the corresponding detection results $c_{det,k}$, will also be updated as follows:

where $m$ denotes the matched paris; $c_{pre,k-1}^{m}$ denotes the confidence of missed detection at the moment $k-1$.

For unmatched detection and prediction results, we employ the following tracking management strategy. Detection results with very low confidence are directly discarded, while those with higher confidence are initialized to establish new tracks. For unmatched predicted object states, there are two scenarios. The first scenario is when the object naturally disappears from the field of view. The second scenario occurs in challenging situations where the object is temporarily occluded by other objects or is too far from the sensor, leading to missed detections. We set a threshold $N$ for the number of prediction frames to distinguish between these two cases. If a predicted state cannot be updated by detection states for more than $N$ frames (set to 12 here), it is considered to have naturally vanished. Such predicted states are removed and no longer tracked. Otherwise, the predicted states are retained, as the object may have been temporarily missed and could reappear in future frames, as illustrated in Fig. 3(b), where the state performs prediction from time $k-t$ to $k-1$ and re-associates data guided by confidence at time $k$ to resume tracking.

The unified graph optimization backend couples SLAM with the multi-object tracking to jointly optimize the state estimation of the ego-vehicle and moving objects. It extends the factor graph optimization of the LiDAR SLAM module and is integrated with factor graph-based tracking. As depicted in Fig. 3(a), all variable nodes represent the estimated 3D states. The node corresponding to the ego-vehicle pose ${{\bf{X}}_{E,k^{*}}}$ at the timestamp of selected key frames $k*$ is obtained through LiDAR odometry. The edge connecting two nodes represents the ego-vehicle motion constraint factor ${{\bf{e}}_{odo}}$:

The object perception factor connects the ego-vehicle pose node and the $i$-th moving object pose node ${{{\bf{O}}}_{i,k}}$ that deals with the implicit data association between prediction objects and detection measurements by the confidence-guided implicit data association introduced in Sec. III-D, as shown in Fig. 3(b). For computational efficiency in LiDAR odometry, keyframe selection is used for backend state estimation, meaning that the vehicle state is updated only in keyframes, and non-keyframe LiDAR data, including moving object detections, is ignored. Conventional methods typically estimate vehicle and object states synchronously, but factor graph optimization-based solutions do not have this limitation. Inspired by [8], we use asynchronous state estimation to utilize object information from non-keyframes. This involves transforming object detection results from non-keyframes to virtual moving object detection measurements in the latest keyframes, as shown in Fig. 3(c). This transformation uses the relative vehicle state estimates at timestamps $k$ and $k^{*}$ through scan matching. Such an object perception factor is denoted as:

The edge between two object pose nodes is denoted as the object motion constraint factor ${{\bf{e}}_{mov,i}^{k-1,k}}$ shown in equation (11), where $f(\cdot)$ denotes the CTRV movement function of object states driven by its velocity ${{\bf{V}}_{i,k}}$ in a time step $\Delta T$, where the ${{\bf{V}}_{i,k}}=[{{\bf{v}}_{i,k}},{{\bf{\omega}}_{i,k}}]^{T}$ denotes the linear velocity and angular velocity respectively. The edge constraints between two poses can be represented by velocity factors ${{\bf{e}}_{v,i}^{k-1,k}}$ in equation (12), which assume a reasonable approximation that the object moves with a constant velocity over a short time.

Finally, the joint factor graph optimization is defined as minimizing the sum of nonlinear least-square errors:

Where $\chi$ denotes the set of all variables, and ${\left\|{{{\bf{e}}_{p}}({\chi_{0}})}\right\|^{2}}$ denotes the prior information error term. $\sum\nolimits_{{odo}}$, $\sum\nolimits_{{op}}$, $\sum\nolimits_{{mov}}$, $\sum\nolimits_{{v}}$, and $\sum\nolimits_{{p}}$ denote the covariance or standard deviation matrix of each variable factor, respectively.

We provide a comprehensive overview of the experiments conducted to evaluate the performance of the proposed Conf SLAMMOT and the baseline methods in various aspects. We conducted experiments on several sequences of the KITTI Tracking dataset[11]. This dataset was primarily collected on urban roads and highways, and it includes raw point clouds, IMU/GPS data, and ground truth labels that can be used for tracking evaluation. We use 18 sequences of dataset with ground truth label data as evaluation sequences, excluding sequences 16, 17, and 20, because they are either highway sequences that cause SLAM to fail or sequences that primarily focus on pedestrian objects. To highlight the coupling advantages of the Conf SLAMMOT over standalone LiDAR SLAM or multi-object tracking systems, we implement comparative experiments by several methods to evaluate our proposed solution for state estimation of ego-vehicle and objects. And we also evaluate the overall performance of Conf SLAMMOT with other baseline methods in different sequences. For the parameters and thresholds used in confidence-guided implicit data association, we set $\alpha$ to 0.03, $\beta$ to 80 and $\sigma$ to 6.5 for the applied method and dataset. Additionally, we evaluate the time consumption of each module in the proposed solution.

In the ego-pose estimation evaluation experiments, we compare the pose estimation among three methods. Considering the potential different outcomes of whether to filter out dynamic object points during LiDAR odometry for scan matching, LOAM [12], LeGO-LOAM [1], and Conf SLAMMOT represent the methods that perform dynamic point filtering, while LOAM*, LeGO-LOAM*, and Conf SLAMMOT* indicate the methods that do not perform dynamic point filtering. We use the mean error (MEAN) and root mean square error (RMSE) as evaluation metrics. The experimental results in various sequences of the KITTI Tracking dataset are shown in Table I. It can be observed from the table that in a few individual sequences, the performance after filtering out dynamic points is inferior to that without such filtering. This is because points belonging to potential dynamic objects (such as temporarily parked vehicles, etc.) are removed, leading to feature sparsity and thus reducing the pose estimation accuracy. For most scenarios with many moving objects and few potential dynamic objects, filtering out dynamic points to ensure the reliability of feature points can enhance pose estimation accuracy. Moreover, the pose estimation accuracy of the Conf SLAMMOT is generally higher than that of LOAM and LeGO-LOAM, indicating that in dynamic environments, tightly coupling SLAM and MOT in the graph optimization backend can benefit each other, as for SLAM, moving objects are no longer treated as interfering information but as auxiliary information to enhance the ego-pose estimation. Additionally, Fig. 4 displays the ego-trajectory error maps for several sequences, providing a more intuitive comparison.

We conducted several experiments on the KITTI Tracking dataset to evaluate the tracking performance of several comparative methods with different detectors, as shown in Table II. The evaluation consider Multi-Object Tracking Accuracy (MOTA), Multi-Object Tracking Precision (MOTP), Recall and Precision based on intersection over union (IoU) thresholds of 0.5. The compared detectors include SECOND [34], PointRCNN [45] and PV-RCNN [32], which are the three common and popular detectors with different detection quality. PV-RCNN combines the advantages of voxel-based and pointnet-based networks, reaching superior performance on various detection datasets compared to the former two [32, 11]. And the compared trackers involve AB3DMOT[22], FG-3DMOT[24], PC3T [23], Conf SLAMMOT^# and Conf SLAMMOT. It is noted that the standalone multi-object tracking methods are evaluated by the virtual LiDAR odometry of LeGO-LOAM to provide ego-vehicle pose. Compare to Conf SLAMMOT, the Conf SLAMMOT^# method considers only the confidence of object predictions in the data association of tracking, without taking into account the confidence scores of object detection results. That is, the covariance matrix of the detection results during association is obtained using the overall uncertainty of the detector determined by the sensor’s error characteristics, just like in the FG-3DMOT method. From the table, it is evident that the tracking results of the PV-RCNN-based methods generally surpass those of SECOND and PointRCNN. This indirectly confirms that higher detection performance correlates with better tracking effects. Compared to Conf SLAMMOT^#, considering the confidence scores of the detection result bounding boxes in Conf SLAMMOT will also help to adaptively adjust the implicit object association range, thereby enhancing tracking performance. From the comparative results of various tracking methods, it can be seen that the Conf SLAMMOT has achieved competitive results. The reason is that its confidence-guided implicit data association integrates the advantages of several methods, and the accuracy of simultaneously optimized ego-pose estimation by joint graph optimization backend is higher than when only using LiDAR odometry as input, which also enhances its tracking performance.

We also evaluate the overall performance of Conf SLAMMOT by assessing the accuracy of state estimation for moving objects with several open-source baseline SLAMMOT methods. Specifically, we compare the multi-object tracking performance in terms of RMSE for longitudinal (long) and lateral (lat) position and yaw angle estimation, as shown in Table III. These methods include LIO-SEGMOT [8] and DL-SLOT [9], both of which utilize LeGO-LOAM for LiDAR odometry and employ PV-RCNN for object detection to ensure consistency in comparison. Herein, several objects with different IDs are selected for error analysis. It can be seen that Conf SLAMMOT shows more advantages compared to other SLAMMOT methods. For example, the tracked frame length of object 2 in sequence 04, other methods would occur ID switching. This is because it considers the confidence of prediction and detection during tracking, which is beneficial for better data association, especially in challenging scenarios that there are continuous missed detections. The improvement in tracking results will further enhance ego-vehicle state estimation and achieve an overall performance boost through joint graph optimization.

Furthermore, Fig. 5 shows the visualization results of LIO-SEGMOT and the proposed Conf SLAMMOT method on different sequences. We tested the aforementioned compared methods on multiple sequences. Due to space limitations, we only present the results of two compared methods on representative sequences here. Additionally, considering that both LIO-SEGMOT and our proposed Conf SLAMMOT utilize factor graph-based tracking methods, the effectiveness of the confidence-guided implicit data association is more intuitively demonstrated through the figures. Three sequences (00, 02, 03) are presented, with (a-d) and (a*-d*) representing different moments of LIO-SEGMOT and Conf SLAMMOT runs. In sequence 00, object ID 1 is heavily occluded by the adjacent object for many consecutive frames, as shown in (1b-1c) and (1b*-1c*). Due to the accumulation of errors in the predicted state, ID switches can easily occur when recovering tracking, as seen in the result of LIO-SEGMOT in (1d). However, even with significant temporary occlusions, Conf SLAMMOT correctly tracked the object by successfully associating the currently detected state with the predicted state, as shown in (1d*). In sequence 02, object ID 0, as shown in (2b-2c) and (2b*-2c*), as well as in sequence 03 for object IDs 14, as shown in (3b-3c), and object ID 11, as shown in (3b*-3c*), consecutive missed detections occurred due to the objects being temporarily occluded and being far away. Compared to the ID switches that occurred in LIO-SEGMOT after recovery, as shown in (2d) and (3d), the proposed method was able to correctly track these objects, as shown in (2d*) and (3d*). These visualization results further demonstrate the effectiveness of Conf SLAMMOT. All kinds of experimental results demonstrate the superior performance of Conf SLAMMOT. It not only addresses the limitations in challenged scenes compared to other baseline SLAMMOT but also improves ego-vehicle pose estimation and multi-object tracking compared to standalone SLAM and MOT methods.

All of the experiments are implemented on a laptop with an Intel Core i7-10750H 2.60 GHz CPU and 12 GB RAM. And we compute the average time consumption of the main functional modules except for object detection, which can be accelerated by GPU. Specifically, the average time consumed for LiDAR odometry to process one scan is approximately 7 ms. The average time consumption for graph optimization backend (integrating the confidence-guided implicit data association) is approximately 62.4ms in the experiments. The proposed Conf SLAMMOT can be implemented in real-time.